SLAM14讲 第七章 2D-2D[2] 三角测量
#include <iostream> #include <opencv2/core/core.hpp> #include <opencv2/features2d/features2d.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/calib3d/calib3d.hpp> // #include "extra.h" // used in opencv2 using namespace std; using namespace cv; //找特征点和描述子 void find_feature_matches ( const Mat& img_1, const Mat& img_2, std::vector<KeyPoint>& keypoints_1, std::vector<KeyPoint>& keypoints_2, std::vector< DMatch >& matches ); //DMatch是用于匹配特征关键点的特征描述子的类:查询特征描述子索引, 特征描述子索引, 训练图像索引, 以及不同特征描述子之间的距离. //位姿估计,求R,t void pose_estimation_2d2d ( const std::vector<KeyPoint>& keypoints_1, const std::vector<KeyPoint>& keypoints_2, const std::vector< DMatch >& matches, Mat& R, Mat& t ); //三角测量/化,求特征点的三维空间坐标,求double类型的三维点 void triangulation ( const vector<KeyPoint>& keypoint_1, const vector<KeyPoint>& keypoint_2, const std::vector< DMatch >& matches, const Mat& R, const Mat& t, vector<Point3d>& points ); // 像素坐标转相机归一化坐标,p-O1(相机光心坐标=相机光心标定值/焦距) Point2f pixel2cam( const Point2d& p, const Mat& K ); //—————————————————————————————————————— //定义float类型的二维点 int main ( int argc, char** argv ) { if ( argc != 3 ) { cout<<"usage: triangulation img1 img2"<<endl; return 1; } //-- 读取图像 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); //-- 找匹配点 vector<KeyPoint> keypoints_1, keypoints_2; vector<DMatch> matches; find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl; //-- 估计两张图像间运动 Mat R,t; pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t ); //-- 三角化 vector<Point3d> points; triangulation( keypoints_1, keypoints_2, matches, R, t, points ); //-- 验证三角化点与特征点的重投影关系 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); for ( int i=0; i<matches.size(); i++ ) { Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K ); Point2d pt1_cam_3d( points[i].x/points[i].z, points[i].y/points[i].z ); cout<<"point in the first camera frame: "<<pt1_cam<<endl; cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points[i].z<<endl; // 第二个图 Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K ); Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t; pt2_trans /= pt2_trans.at<double>(2,0); cout<<"point in the second camera frame: "<<pt2_cam<<endl; cout<<"point reprojected from second frame: "<<pt2_trans.t()<<endl; cout<<endl; } return 0; } void find_feature_matches ( const Mat& img_1, const Mat& img_2, std::vector<KeyPoint>& keypoints_1, std::vector<KeyPoint>& keypoints_2, std::vector< DMatch >& matches ) { //-- 初始化 Mat descriptors_1, descriptors_2; // used in OpenCV3 Ptr<FeatureDetector> detector = ORB::create(); Ptr<DescriptorExtractor> descriptor = ORB::create(); // use this if you are in OpenCV2 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming"); //-- 第一步:检测 Oriented FAST 角点位置 detector->detect ( img_1,keypoints_1 ); detector->detect ( img_2,keypoints_2 ); //-- 第二步:根据角点位置计算 BRIEF 描述子 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); descriptor->compute ( img_2, keypoints_2, descriptors_2 ); //-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离 vector<DMatch> match; // BFMatcher matcher ( NORM_HAMMING ); matcher->match ( descriptors_1, descriptors_2, match ); //-- 第四步:匹配点对筛选 double min_dist=10000, max_dist=0; //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 for ( int i = 0; i < descriptors_1.rows; i++ ) { double dist = match[i].distance; if ( dist < min_dist ) min_dist = dist; if ( dist > max_dist ) max_dist = dist; } printf ( "-- Max dist : %f \n", max_dist ); printf ( "-- Min dist : %f \n", min_dist ); //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限. for ( int i = 0; i < descriptors_1.rows; i++ ) { if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) { matches.push_back ( match[i] ); } } } void pose_estimation_2d2d ( const std::vector<KeyPoint>& keypoints_1, const std::vector<KeyPoint>& keypoints_2, const std::vector< DMatch >& matches, Mat& R, Mat& t ) { // 相机内参,TUM Freiburg2 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); //-- 把匹配点转换为vector<Point2f>的形式 vector<Point2f> points1; vector<Point2f> points2; for ( int i = 0; i < ( int ) matches.size(); i++ ) { points1.push_back ( keypoints_1[matches[i].queryIdx].pt ); points2.push_back ( keypoints_2[matches[i].trainIdx].pt ); } //-- 计算基础矩阵 Mat fundamental_matrix; fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT ); cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl; //-- 计算本质矩阵 Point2d principal_point ( 325.1, 249.7 ); //相机主点, TUM dataset标定值 int focal_length = 521; //相机焦距, TUM dataset标定值 Mat essential_matrix; essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point ); cout<<"essential_matrix is "<<endl<< essential_matrix<<endl; //-- 计算单应矩阵 Mat homography_matrix; homography_matrix = findHomography ( points1, points2, RANSAC, 3 ); cout<<"homography_matrix is "<<endl<<homography_matrix<<endl; //-- 从本质矩阵中恢复旋转和平移信息. recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point ); cout<<"R is "<<endl<<R<<endl; cout<<"t is "<<endl<<t<<endl; } void triangulation ( const vector< KeyPoint >& keypoint_1, const vector< KeyPoint >& keypoint_2, const std::vector< DMatch >& matches, const Mat& R, const Mat& t, vector< Point3d >& points ) { // --变换矩阵T3x4 T1=[I;0],T2=[R;t] Mat T1 = (Mat_<float> (3,4) << 1,0,0,0, 0,1,0,0, 0,0,1,0); Mat T2 = (Mat_<float> (3,4) << R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0), R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0), R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0) ); //相机内参K Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); vector<Point2f> pts_1, pts_2;//特征点的相机归一化坐标 for ( DMatch m:matches ) { // 将像素坐标转换至相机归一化坐标 pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) ); pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) ); } Mat pts_4d; //points4D – 4xN array of reconstructed points in homogeneous coordinates. //输出的3D坐标是齐次坐标,共四个维度,因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz。这个坐标是在相机坐标系下的坐标 cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d ); // 转换成非齐次坐标 for ( int i=0; i<pts_4d.cols; i++ ) { Mat x = pts_4d.col(i); x /= x.at<float>(3,0); // 归一化 Point3d p ( x.at<float>(0,0), x.at<float>(1,0), x.at<float>(2,0) ); points.push_back( p );//三角化后的特征点在归一化平面的3D坐标 } } Point2f pixel2cam ( const Point2d& p, const Mat& K ) { return 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) ); }