OpenCV 与多视图几何

欢迎访问我的博客首页。

1. 工具函数

---

  像素坐标、归一化坐标和相机坐标之间的相互转化。

#include <iostream>
#include <opencv2/opencv.hpp>

// 相机坐标系到像素坐标系的变换。
cv::Point2f cam2pix(const cv::Point3f &p, const cv::Mat &K) {
    return cv::Point2f((p.x * K.at<double>(0, 0)) / p.z + K.at<double>(0, 2), (p.y * K.at<double>(1, 1)) / p.z + K.at<double>(1, 2));
}

// 像素坐标系到归一化成像平面的变换。
cv::Point2f pix2nor(const cv::Point2f &p, const cv::Mat &K) {
    return cv::Point2f((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0), (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

// 相机坐标系到归一化成像平面的变换。
cv::Point2f cam2nor(const cv::Point3f &p, const cv::Mat &K) { return pix2nor(cam2pix(p, K), K); }

// 相机坐标系到归一化成像平面的变换。
cv::Point2f cam2nor(const cv::Point3f &p) { return cv::Point2f(p.x / p.z, p.y / p.z); }

int get_random_integer(int a = 10, int b = 100) { return rand() % (b - a + 1) + a; }

2. 本质矩阵

---

  本质矩阵利用匹配的像素坐标计算相机位姿关系。

int recoverPoseFromEssentialMat() {
    // 相机内参和外参。
    cv::Mat K = (cv::Mat_<double>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    cv::Mat R = (cv::Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<double>(3, 1) << 0, -1, 0);
    // 三维坐标。
    cv::Mat m1 = cv::Mat::zeros(3, 1, CV_32FC1);
    cv::Mat m2 = cv::Mat::zeros(3, 1, CV_32FC1);
    cv::Point3f p1;
    cv::Point3f p2;
    // 像素坐标。
    std::vector<cv::Point2d> pts1;
    std::vector<cv::Point2d> pts2;
    // 利用随机产生的三维坐标求像素坐标。
    srand((unsigned)time(NULL));
    for (int i = 0; i < 15; i++) {
        m1 = (cv::Mat_<double>(3, 1) << get_random_integer(), get_random_integer(), get_random_integer());
        m2 = R * m1 + t;
        p1 = cv::Point3f(cv::Point3f(m1.at<double>(0, 0), m1.at<double>(1, 0), m1.at<double>(2, 0)));
        p2 = cv::Point3f(cv::Point3f(m2.at<double>(0, 0), m2.at<double>(1, 0), m2.at<double>(2, 0)));
        pts1.push_back(cam2pix(p1, K));
        pts2.push_back(cam2pix(p2, K));
    }
    // 根据像素坐标求本质矩阵。
    cv::Mat mask;
    cv::Mat E = cv::findEssentialMat(pts1, pts2, K, cv::RANSAC, 0.99, 1.0);
    // 根据本质矩阵求位姿。
    cv::Mat rotation, translation;
    int inlier_count = cv::recoverPose(E, pts1, pts2, K, rotation, translation, mask);
    // 输出。
    std::cout << "-- 本质矩阵：" << std::endl;
    std::cout << inlier_count << std::endl << std::endl;
    std::cout << rotation << std::endl << std::endl;
    std::cout << translation.t() << std::endl;
}

3. 单应矩阵

---

  单应矩阵利用匹配的像素坐标计算相机位姿关系。

int recoverPoseFromHomographyMat() {
    // 相机内参和外参。
    cv::Mat K = (cv::Mat_<double>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    cv::Mat R = (cv::Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<double>(3, 1) << 0, -1, 0);
    // 三维坐标。
    cv::Mat m1 = cv::Mat::zeros(3, 1, CV_32FC1);
    cv::Mat m2 = cv::Mat::zeros(3, 1, CV_32FC1);
    cv::Point3f p1;
    cv::Point3f p2;
    // 像素坐标。
    std::vector<cv::Point2d> pts1;
    std::vector<cv::Point2d> pts2;
    // 利用随机产生的三维坐标求像素坐标。
    srand((unsigned)time(NULL));
    for (int i = 0; i < 15; i++) {
        m1 = (cv::Mat_<double>(3, 1) << get_random_integer(), get_random_integer(), get_random_integer());
        m2 = R * m1 + t;
        p1 = cv::Point3f(cv::Point3f(m1.at<double>(0, 0), m1.at<double>(1, 0), m1.at<double>(2, 0)));
        p2 = cv::Point3f(cv::Point3f(m2.at<double>(0, 0), m2.at<double>(1, 0), m2.at<double>(2, 0)));
        pts1.push_back(cam2pix(p1, K));
        pts2.push_back(cam2pix(p2, K));
    }
    // 根据像素坐标求单应矩阵。
    cv::Mat mask;
    cv::Mat H = cv::findHomography(pts1, pts2, cv::RANSAC);
    // 根据单应矩阵求位姿。
    std::vector<cv::Mat> rotations, translations, normals;
    int pose_count = cv::decomposeHomographyMat(H, K, rotations, translations, normals);
    assert(pose_count == rotations.size());
    // 输出。
    for (int i = 0; i < pose_count; i++) {
        std::cout << "-- 单应矩阵 " << i + 1 << "/" << pose_count << "：" << std::endl;
        std::cout << rotations[i] << std::endl << std::endl;
        std::cout << translations[i].t() << std::endl << std::endl;
        std::cout << normals[i].t() << std::endl << std::endl << std::endl;
    }
}

4. 基础矩阵

---

  基础矩阵利用匹配的归一化坐标计算相机位姿关系。参考 vins mono。

int recoverPoseFromFundamentalMat() {
    // 相机外参。
    cv::Mat R = (cv::Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<double>(3, 1) << 0, -1, 0);
    // 三维坐标。
    srand((unsigned)time(NULL));
    cv::Mat m1 = cv::Mat::zeros(3, 1, CV_32FC1);
    cv::Mat m2 = cv::Mat::zeros(3, 1, CV_32FC1);
    cv::Point3f p1;
    cv::Point3f p2;
    // 归一化坐标。
    std::vector<cv::Point2d> pts1;
    std::vector<cv::Point2d> pts2;
    // 利用随机产生的三维坐标求归一化坐标。
    for (int i = 0; i < 15; i++) {
        m1 = (cv::Mat_<double>(3, 1) << get_random_integer(), get_random_integer(), get_random_integer());
        m2 = R * m1 + t;
        p1 = cv::Point3f(cv::Point3f(m1.at<double>(0, 0), m1.at<double>(1, 0), m1.at<double>(2, 0)));
        p2 = cv::Point3f(cv::Point3f(m2.at<double>(0, 0), m2.at<double>(1, 0), m2.at<double>(2, 0)));
        pts1.push_back(cam2nor(p1));
        pts2.push_back(cam2nor(p2));
    }
    // 根据归一化坐标求基础矩阵。
    cv::Mat mask;
    cv::Mat F = cv::findFundamentalMat(pts1, pts2, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
    // 根据基础矩阵求位姿。
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
    cv::Mat rotation, translation;
    int inlier_cnt = cv::recoverPose(F, pts1, pts2, cameraMatrix, rotation, translation, mask);
    // 输出。
    std::cout << "-- 基础矩阵：" << std::endl;
    std::cout << inlier_cnt << std::endl << std::endl;
    std::cout << rotation << std::endl << std::endl;
    std::cout << translation.t() << std::endl;
}

5. 参考

---

1. 相机矩阵求解，CSDN。