欢迎访问我的博客首页。
OpenCV 与多视图几何
- 1. 工具函数
- 2. 本质矩阵
- 3. 单应矩阵
- 4. 基础矩阵
- 5. 参考
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 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);// 三维坐标。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. 参考
- 相机矩阵求解，CSDN。
