视觉SLAM 十四讲——对极约束求解相机运动(2D-2D)
主要内容
1. 对极约束
几何意义是 ,P, 三者共面,对极约束同时包含了平移和旋转。
基础矩阵:
本质矩阵:
对极约束表示:
其中, 分别表示为相机坐标系下归一化的平面坐标
2. 本质矩阵的特点(3×3)
1)E在不同尺度下是等价的
2) 内在性质:奇异值必定是 的形式
3) 由于平移+旋转,共有6个自由度,但因为尺度等价性,E实际上有5个自由度
3 本质矩阵求解——八点法
1) E的内在性质是一种非线性性质,在求解线性方程时会带来麻烦,因此不会用5对点来求解。
2) 未知数共有9个,只考虑尺度等价性,所以用8对2D-2D点来求解,线性变换的方法。
3) 求解矩阵后,进行奇异值分解分(SVD),得到运动信息
4) 分解的时候,会有4中情况,选取一个点进行求解,检测该点在两个相机下的深度信息,选取两个正的深度。
5) 线性方程解可能不满足E的内在性质,将求解出来的矩阵投影到E所在的流形上。
4 单应矩阵H
定义与旋转,平移及平面的参数有关。
1) 自由度为8的单应矩阵可以通过4对匹配点进行计算(注意,这些特征点不能有三点共线的情况)
2) 求解方法:直接线性变换法,对其进行分解得到运动信息,分解方法有数值法和解析法
4组解,(根据计算点的深度以及平面的法向量信息选取合理的解)
3) 意义
3.1) 描述了两个平面之间的映射关系,若所有特征点落在同一平面,则可以通过H估计运动,
具体应用在无人机携带俯视相机或扫地机器人的顶视相机中。
3.2)当特征点共面或者相机发生纯旋转时,基础矩阵自由度下降,H可以估计纯旋转的运动信息。
5. 在实际应用,为避免退化现象造成的影响,会同时估计F和H,选择其中重投影误差比较小的那个作为最终运动的估计结果。
6. 对极几何2D-2D的特点
1)尺度不确定性
t归一化相当于固定了尺度信息,成为单目SLAM的初始化,后面的轨迹和地图以此单位进行计算。
初始化不可避免,初始化完毕以后,计算特征点空间位置,可以利用3D-2D的方法计算相机运动。
2) 初始化的纯旋转问题
纯旋转 → t=0 → E = 0 → 无法求解R,
or: 用H求解R, 但是t为0,导致无法利用三角测量求解特征点位置
单目SLAM初始化,不能只有纯旋转,必须要有一定程度的平移。
3) 多余8对点的情况
利用最小二乘进行求解矩阵E。
当可能存在误匹配的情况时,更倾向于使用随机采样一致性(Random Sample Concensus,RANSAS),可以处理带有错误匹配的数据。
7. 代码中需要注意的点:
1) Mat类型的操作:C++ ,赋值初始化,乘法,转置等运算
2)三个矩阵的求解
3) 从match信息中得到对应图像中像素的坐标信息
4)根据求得的R t在反向通过对极约束进行验证, 计算的精度情况等。
参考链接
使用cv::findFundamentalMat要注意的几点
代码
#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" // use this if in OpenCV2 using namespace std; using namespace cv; /**************************************************** * 本程序演示了如何使用2D-2D的特征匹配估计相机运动 * **************************************************/ 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 ); void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1, std::vector<KeyPoint> keypoints_2, std::vector< DMatch > matches, Mat& R, Mat& t ); // 像素坐标转相机归一化坐标 Point2d pixel2cam ( const Point2d& p, const Mat& K ); int main ( int argc, char** argv ) { if ( argc != 3 ) { cout<<"usage: pose_estimation_2d2d 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 ); //-- 验证E=t^R*scale Mat t_x = ( Mat_<double> ( 3,3 ) << 0, -t.at<double> ( 2,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ), 0, -t.at<double> ( 0,0 ), -t.at<double> ( 1,0 ), t.at<double> ( 0,0 ), 0 ); cout<<"t^R="<<endl<<t_x*R<<endl; //-- 验证对极约束 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); for ( DMatch m: matches ) { Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K ); Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 ); Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K ); Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 ); Mat d = y2.t() * t_x * R * y1; cout << "epipolar constraint = " << d << 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] ); } } } Point2d pixel2cam ( const Point2d& p, const Mat& K ) { return Point2d ( ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) ); } void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1, std::vector<KeyPoint> keypoints_2, 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标定值 double 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, CV_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; }
结果及分析
Starting: /home/qifarui/code/slambook-master/ch7/build/pose_estimation_2d2d 1.png 2.png -- Max dist : 94.000000 -- Min dist : 4.000000 一共找到了79组匹配点 fundamental_matrix is [4.544437503937326e-06, 0.0001333855576988952, -0.01798499246457619; -0.0001275657012959839, 2.266794804637672e-05, -0.01416678429258694; 0.01814994639952877, 0.004146055871509035, 1] essential_matrix is [0.01097677479889588, 0.2483720528328748, 0.03167429208264108; -0.2088833206116968, 0.02908423961947315, -0.674465883831914; 0.008286777626839029, 0.66140416240827, 0.01676523772760232] homography_matrix is [0.9261214281395963, -0.1445322024422802, 33.26921085290552; 0.04535424466077615, 0.9386696693994352, 8.570979963061975; -1.00619755759245e-05, -3.0081402779533e-05, 1] R is [0.9969387384756405, -0.0515557418857258, 0.05878058527448649; 0.05000441581116598, 0.9983685317362444, 0.02756507279509838; -0.06010582439317147, -0.02454140007064545, 0.9978902793176159] t is [-0.9350802885396324; -0.03514646277098749; 0.352689070059345] t^R= [-0.01552350379194682, -0.3512511256306389, -0.04479421344178829; 0.2954056249626309, -0.04113132612112196, 0.9538388002732133; -0.01171927330817152, -0.9353667366876339, -0.02370962657084997] epipolar constraint = [-0.0005415187894598494] epipolar constraint = [-0.002158321996443405] epipolar constraint = [0.0003344241034267809] epipolar constraint = [-8.762783075558223e-06] epipolar constraint = [0.0002121321904275941] epipolar constraint = [0.0001088766988978362] epipolar constraint = [0.0004246621761395189] epipolar constraint = [-0.003173128213173212] epipolar constraint = [-3.716645900272639e-05] epipolar constraint = [-0.001451851842095554] epipolar constraint = [-0.0009607717987651782] epipolar constraint = [-0.0008051993509270439] epipolar constraint = [-0.001424813547085908] epipolar constraint = [-0.0004339424745618652] epipolar constraint = [-0.0004786109668010741] epipolar constraint = [-0.0001965692841337796] epipolar constraint = [0.001542309775822037] epipolar constraint = [0.003107154827523384] epipolar constraint = [0.0006774648880640624] epipolar constraint = [-0.001176167495898923] epipolar constraint = [-0.003987986032579591] epipolar constraint = [-0.001255263863970228] epipolar constraint = [-0.001553941958799671] epipolar constraint = [0.002009914868860929] epipolar constraint = [-0.0006068096317080607] epipolar constraint = [-2.769881084775661e-05] epipolar constraint = [0.001274573010445104] epipolar constraint = [-0.004169986054276958] epipolar constraint = [-0.001108104734518528] epipolar constraint = [-0.0005154295528846109] epipolar constraint = [-0.001776993209361748] epipolar constraint = [6.67735429456251e-07] epipolar constraint = [-0.001853977733712375] epipolar constraint = [-0.0004823070343765445] epipolar constraint = [0.000474090447079023] epipolar constraint = [-0.00296104174875747] epipolar constraint = [-0.003347655248199201] epipolar constraint = [-0.000197568098110501] epipolar constraint = [-0.002824643387693802] epipolar constraint = [-0.0004311798178931181] epipolar constraint = [0.001181203683149307] epipolar constraint = [1.756253096285576e-07] epipolar constraint = [-0.002137829860220063] epipolar constraint = [0.001153415526361834] epipolar constraint = [-0.002120357729634967] epipolar constraint = [2.741528250471692e-06] epipolar constraint = [0.0009044582009310256] epipolar constraint = [-0.002100484436687942] epipolar constraint = [-0.0003517789230311139] epipolar constraint = [-2.72046495066805e-05] epipolar constraint = [-0.003784823354081965] epipolar constraint = [-0.001588562609431521] epipolar constraint = [-0.002928516012703969] epipolar constraint = [-0.001016592267328753] epipolar constraint = [0.0001241874570146462] epipolar constraint = [-0.0009797104639974082] epipolar constraint = [0.001457875224074562] epipolar constraint = [-1.153738168938045e-05] epipolar constraint = [-0.002733247428635111] epipolar constraint = [0.001415721256946334] epipolar constraint = [0.001790255871245483] epipolar constraint = [-0.002547929672679236] epipolar constraint = [-0.006257078862295637] epipolar constraint = [-0.001874877416209744] epipolar constraint = [-0.002104542913954163] epipolar constraint = [1.392530300986516e-06] epipolar constraint = [-0.004013502582045692] epipolar constraint = [-0.004726241072653375] epipolar constraint = [-0.001328682673556458] epipolar constraint = [3.99592575944796e-07] epipolar constraint = [-0.00508051172487578] epipolar constraint = [-0.001977141851277564] epipolar constraint = [-0.001661334265550506] epipolar constraint = [0.004285762869538863] epipolar constraint = [0.004087325193214478] epipolar constraint = [0.0001482534277866508] epipolar constraint = [-0.000962530114253686] epipolar constraint = [-0.00234107601156941] epipolar constraint = [-0.006138005036133154] *** Exited normally ***
可以看到,最后利用求得的R t再带入到对极约束中去,得到的精度在0.00x 精度级别。
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步