单目视觉里程计求解无人机Roll/Pitch/Yaw角：Pitch/Yaw计算问题

问题描述 #

使用OpenCV读取无人机前视摄像头采集的.mp4视频，通过ORB检测器提取连续帧的关键点与描述子，尝试用匹配关键点求解变换矩阵并推导Roll、Pitch、Yaw角。当前Roll角结果与实际值吻合，但Pitch和Yaw角计算结果无效，需修正姿态推导逻辑。

原代码 #

#include <opencv2/opencv.hpp>

int main() {
    cv::VideoCapture cap("video.mp4");

    if (!cap.isOpened()) {
        std::cout << "Video file not found." << std::endl;
        return -1;
    }

    cv::Mat prev_frame, curr_frame;
    cap >> prev_frame;

    cv::Ptr<cv::ORB> orb = cv::ORB::create();
    std::vector<cv::KeyPoint> keypoints_prev, keypoints_curr;
    cv::Mat descriptors_prev, descriptors_curr;

    while (true) {
        cap >> curr_frame;
        if (curr_frame.empty()) {
            break;
        }

        orb->detectAndCompute(prev_frame, cv::noArray(), keypoints_prev, descriptors_prev);
        orb->detectAndCompute(curr_frame, cv::noArray(), keypoints_curr, descriptors_curr);

        cv::BFMatcher matcher(cv::NORM_HAMMING, true);
        std::vector<cv::DMatch> matches;
        matcher.match(descriptors_prev, descriptors_curr, matches);

        // 提取匹配点对
        std::vector<cv::Point2f> src_pts, dst_pts;
        for (size_t i = 0; i < matches.size(); i++) {
            src_pts.push_back(keypoints_prev[matches[i].queryIdx].pt);
            dst_pts.push_back(keypoints_curr[matches[i].trainIdx].pt);
        }

        // 求解仿射变换矩阵
        cv::Mat M = cv::estimateAffinePartial2D(src_pts, dst_pts);

        // 姿态角计算（原错误逻辑）
        double roll = std::atan2(M.at<double>(1, 0), M.at<double>(0, 0));
        double pitch = std::atan2(-M.at<double>(1, 2), std::sqrt(M.at<double>(0, 2) * M.at<double>(0, 2) + M.at<double>(2, 2) * M.at<double>(2, 2)));
        double yaw = std::atan2(M.at<double>(0, 2), M.at<double>(2, 2));

        roll = roll * 180.0 / CV_PI;
        pitch = pitch * 180.0 / CV_PI;
        yaw = yaw * 180.0 / CV_PI;

        std::cout << "Roll: " << roll << " degrees" << std::endl;
        std::cout << "Pitch: " << pitch << " degrees" << std::endl;
        std::cout << "Yaw: " << yaw << " degrees" << std::endl;

        prev_frame = curr_frame;

        cv::imshow("Current Frame", curr_frame);
        if (cv::waitKey(30) == 27) {
            break;
        }
    }

    cap.release();
    cv::destroyAllWindows();

    return 0;
}

问题根源 #

1. 仿射变换的局限性：cv::estimateAffinePartial2D求解的是2D仿射变换矩阵（2×3），仅包含旋转（对应Roll）、平移和缩放，无法建模3D姿态中的Pitch和Yaw带来的透视变形。
2. 姿态计算逻辑错误：原代码中使用的Pitch、Yaw推导公式是针对3×3旋转矩阵的，但仿射矩阵没有第三行元素，访问M.at<double>(2,2)属于越界访问，直接导致计算结果无效。

解决方法与修正代码 #

核心改进点 #

• 改用cv::findHomography求解单应性矩阵（3×3），可建模相机3D旋转、平移及透视变形，适合从平面场景中恢复Pitch和Yaw角。
• 使用KNN匹配+距离筛选去除错误匹配，提升矩阵求解的准确性。
• 基于单应性矩阵推导正确的欧拉角（Roll、Pitch、Yaw）。

修正后的代码 #

#include <opencv2/opencv.hpp>
#include <vector>
#include <cmath>
#include <iomanip>

using namespace cv;
using namespace std;

// 从单应性矩阵提取欧拉角（Yaw-Pitch-Roll顺序，单位：弧度）
void homographyToEulerAngles(const Mat& H, double& yaw, double& pitch, double& roll) {
    // 归一化单应性矩阵的前两列（消除缩放影响）
    Mat H_normalized = H.clone();
    double norm1 = norm(H_normalized.col(0));
    double norm2 = norm(H_normalized.col(1));
    H_normalized.col(0) /= norm1;
    H_normalized.col(1) /= norm2;

    // 计算旋转矩阵的前两行
    Mat r1 = H_normalized.col(0);
    Mat r2 = H_normalized.col(1);
    Mat r3 = r1.cross(r2);

    // 构造完整的旋转矩阵
    Mat R(3, 3, CV_64F);
    r1.copyTo(R.col(0));
    r2.copyTo(R.col(1));
    r3.copyTo(R.col(2));

    // 分解旋转矩阵为欧拉角（Z-Y-X顺序，对应Yaw-Pitch-Roll）
    yaw = atan2(R.at<double>(0, 1), R.at<double>(0, 0));
    pitch = atan2(-R.at<double>(0, 2), sqrt(pow(R.at<double>(0, 0), 2) + pow(R.at<double>(0, 1), 2)));
    roll = atan2(R.at<double>(1, 2), R.at<double>(2, 2));
}

int main() {
    VideoCapture cap("video.mp4");
    if (!cap.isOpened()) {
        cout << "Video file not found." << endl;
        return -1;
    }

    Mat prev_frame, curr_frame;
    cap >> prev_frame;
    cvtColor(prev_frame, prev_frame, COLOR_BGR2GRAY); // 转灰度图提升ORB检测效率

    Ptr<ORB> orb = ORB::create(500); // 增加关键点数量
    vector<KeyPoint> keypoints_prev, keypoints_curr;
    Mat descriptors_prev, descriptors_curr;

    while (true) {
        cap >> curr_frame;
        if (curr_frame.empty()) break;
        cvtColor(curr_frame, curr_frame, COLOR_BGR2GRAY);

        // 检测关键点与描述子
        orb->detectAndCompute(prev_frame, noArray(), keypoints_prev, descriptors_prev);
        orb->detectAndCompute(curr_frame, noArray(), keypoints_curr, descriptors_curr);

        // KNN匹配
        BFMatcher matcher(NORM_HAMMING);
        vector<vector<DMatch>> knn_matches;
        matcher.knnMatch(descriptors_prev, descriptors_curr, knn_matches, 2);

        // 筛选最优匹配（Lowe's比例测试）
        vector<DMatch> good_matches;
        for (size_t i = 0; i < knn_matches.size(); i++) {
            if (knn_matches[i][0].distance < 0.75 * knn_matches[i][1].distance) {
                good_matches.push_back(knn_matches[i][0]);
            }
        }

        if (good_matches.size() < 10) { // 匹配点过少跳过
            cout << "Insufficient good matches." << endl;
            prev_frame = curr_frame;
            imshow("Current Frame", curr_frame);
            waitKey(30);
            continue;
        }

        // 提取匹配点对
        vector<Point2f> src_pts, dst_pts;
        for (size_t i = 0; i < good_matches.size(); i++) {
            src_pts.push_back(keypoints_prev[good_matches[i].queryIdx].pt);
            dst_pts.push_back(keypoints_curr[good_matches[i].trainIdx].pt);
        }

        // 求解单应性矩阵（RANSAC剔除异常点）
        Mat H = findHomography(src_pts, dst_pts, RANSAC, 5.0);
        if (H.empty()) {
            cout << "Failed to compute homography." << endl;
            prev_frame = curr_frame;
            imshow("Current Frame", curr_frame);
            waitKey(30);
            continue;
        }

        // 计算欧拉角
        double yaw, pitch, roll;
        homographyToEulerAngles(H, yaw, pitch, roll);

        // 转换为角度
        yaw *= 180.0 / CV_PI;
        pitch *= 180.0 / CV_PI;
        roll *= 180.0 / CV_PI;

        cout << "Roll: " << fixed << setprecision(2) << roll << " degrees" << endl;
        cout << "Pitch: " << fixed << setprecision(2) << pitch << " degrees" << endl;
        cout << "Yaw: " << fixed << setprecision(2) << yaw << " degrees" << endl;
        cout << "-------------------------" << endl;

        prev_frame = curr_frame;
        imshow("Current Frame", curr_frame);
        if (waitKey(30) == 27) break;
    }

    cap.release();
    destroyAllWindows();
    return 0;
}

补充说明 #

• 单应性矩阵的姿态推导仅适用于场景近似平面的情况（比如无人机拍摄地面），如果场景是复杂3D结构，需要结合相机内参使用cv::solvePnP算法，同时需要已知关键点的3D坐标。
• 代码中使用了灰度图处理，可提升ORB检测的速度和稳定性；增加了匹配点数量阈值，避免因匹配点过少导致的矩阵求解异常。

内容的提问来源于stack exchange，提问作者arcibiyo