1. 特征点提取和匹配
以orb特征为例,步骤1初始化:
//-- 读取图像
Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );
//-- 初始化
std::vector<KeyPoint> keypoints_1, keypoints_2;
Mat descriptors_1, descriptors_2;
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
// Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name);
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name);
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" );
步骤2检测两幅图像的角点
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect ( img_1,keypoints_1 );
detector->detect ( img_2,keypoints_2 );
步骤3计算各个特征点的相似度
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute ( img_1, keypoints_1, descriptors_1 );
descriptor->compute ( img_2, keypoints_2, descriptors_2 );
Mat outimg1;
drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
imshow("ORB特征点",outimg1);
步骤4进行匹配
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离
vector<DMatch> matches;
//BFMatcher matcher ( NORM_HAMMING );
matcher->match ( descriptors_1, descriptors_2, matches );
步骤5进行筛选
//-- 第四步:匹配点对筛选
double min_dist=10000, max_dist=0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for ( int i = 0; i < descriptors_1.rows; i++ )
{
double dist = matches[i].distance;
if ( dist < min_dist ) min_dist = dist;
if ( dist > max_dist ) max_dist = dist;
}
// 仅供娱乐的写法
min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
printf ( "-- Max dist : %f \n", max_dist );
printf ( "-- Min dist : %f \n", min_dist );
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
std::vector< DMatch > good_matches;
for ( int i = 0; i < descriptors_1.rows; i++ )
{
if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) )
{
good_matches.push_back ( matches[i] );
}
}
步骤6绘图
//-- 第五步:绘制匹配结果
Mat img_match;
Mat img_goodmatch;
drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );
imshow ( "所有匹配点对", img_match );
imshow ( "优化后匹配点对", img_goodmatch );
waitKey(0);
结果显示:
2. 2d-2d对极几何
(单目相机)在只知道2d的像素信息的情况下,估计相机位置需要使用2d-2d对极几何的原理.
2.1 对极几何原理
基本概念:
- 基线(baseline):O1O2为基线
- 极平面(epipolar plane):任何包含基线的平面都称为对极平面,或者说是对极平面束中的平面
- 极点(epipole):摄像机的基线与每幅图像的交点;即上图中的点el和er
- 极线(epipolar line):对极平面与图像的交线;例如,上图中的直线ep
对应点的约束
现在假设只知道图像点x,那么,它的对应点x’如何约束呢?
根据前面的讨论,点x和x’一定位于平面π上,而平面π可以利用基线和图像点x的反投影射线确定;
且点x的对应点x’一定位于它的极线上!
2.2 基本矩阵F和本质矩阵E
两幅图像中的像素点p1,p2对应的归一化平面的坐标分别为x1和x2
由一次旋转和平移和进行坐标转化:
x
1
=
K
−
1
p
1
x_1={K^{-1}}p_1
x1=K−1p1;
x
2
=
K
−
1
p
2
x_2={K^{-1}}p_2
x2=K−1p2
x
2
=
R
x
1
+
t
x_2=Rx_1+t
x2=Rx1+t
两边同时左乘t^, 再做乘
x
2
T
x_2^T
x2T,t^t=0得:
x
2
T
x_2^T
x2Tt^
x
2
x_2
x2 =
x
2
T
x_2^T
x2Tt^R
x
1
x_1
x1
左边容易证得为0,所以有:
x
2
T
x_2^T
x2Tt^R
x
1
x_1
x1=0
代入p1和p2,得到:
p
2
T
K
−
T
p_2^T{K^{-T}}
p2TK−Tt^R
K
−
1
p
1
{K^{-1}}p_1
K−1p1=0
该式子是对极约束的代数表达,几何意义是O1,O2,P三点共面
令E={t^}R为本质矩阵
令
F
=
K
−
T
F={K^{-T}}
F=K−TE
K
−
1
{K^{-1}}
K−1为基本矩阵
注意t^是平移向量对应的反对称矩阵
t
=
[
0
,
−
z
,
y
;
z
,
0
,
−
x
;
−
y
,
x
,
0
]
t^=[0,-z,y; z,0,-x; -y,x,0]
t=[0,−z,y;z,0,−x;−y,x,0]
2.3 2d-2d位姿估计
-
问题求解:
1.根据配对点像素点位置求解E或者F
2.根据E,F求解R,t -
一般用8点法求E,接下来介绍其原理
由对极约束 x 2 T x_2^T x2T E E E x 1 = 0 x_1=0 x1=0, E E E为3x3矩阵, x 1 = ( u 1 , v 1 , 1 ) T x_1=(u_1,v_1,1)^T x1=(u1,v1,1)T, x 2 = ( u 2 , v 2 , 1 ) T x_2=(u_2,v_2,1)^T x2=(u2,v2,1)T,得
( u 1 , v 1 , 1 ) (u_1,v_1,1) (u1,v1,1) ( e 1 , e 2 , e 3 ; e 4 , e 5 , e 6 ; e 7 , e 8 , e 9 ) (e_1,e_2,e_3; e_4,e_5,e_6;e_7,e_8,e_9) (e1,e2,e3;e4,e5,e6;e7,e8,e9) ( u 2 , v 2 , 1 ) T (u_2,v_2,1)^T (u2,v2,1)T=0
由于KE=0,所以 E E E有9-1=8个自由度,求解E的时候,解不唯一(有无数解,kx)
得线性方程组:
( u 1 u 2 , u 1 v 2 , u 1 , v 1 u 2 , v 1 v 2 , v 1 , u 2 , v 2 , 1 ) e = 0 (u_1u_2,u_1v_2,u_1,v_1u_2,v_1v_2,v_1,u_2,v_2,1)e=0 (u1u2,u1v2,u1,v1u2,v1v2,v1,u2,v2,1)e=0
所以8对点才能求解
e = ( e 1 , e 2 , e 3 , e 4 , e 5 , e 6 , e 7 , e 8 , e 9 ) T e=(e_1,e_2,e_3, e_4,e_5,e_6,e_7,e_8,e_9)^T e=(e1,e2,e3,e4,e5,e6,e7,e8,e9)T
设E的奇异值分解为:
E = U Σ V T E=U{\Sigma}V^T E=UΣVT -
从E恢复R和t
通过奇异值分解求解:
有四个解分别 t 1 R 1 , t 2 R 2 , t 2 R 1 , t 2 R 2 t_1R_1,t_2R_2,t_2R_1,t_2R_2 t1R1,t2R2,t2R1,t2R2为:
图解:
(1), (3)表示 O 1 到 O 2 O_1到O_2 O1到O2, 且像素点不能发生变化, 只能将 O 2 O_2 O2点处相机位置旋转-90度
(2), (4)表示 O 2 到 O 1 O_2到O_1 O2到O1, 且像素点不能发生变化, 只能将 O 2 O_2 O2点处相机位置旋转-90度
注意, 只有(1)情况下, P点在两个相机处具有正深度, 同样可以通过测试深度的正负来判断属于哪种情况 -
八点法注意的问题:
用于单目SLAM的初始化
尺度不确定性:归一化 t 或特征点的平均深度
纯旋转问题:t=0 时无法求解
多于八对点时:最小二乘
有外点时:RANSAC -
利用5点法求解R,t
-
利用单应矩阵H求解R,t
原理:
注意: 通常会同时利用E和H估计位姿,最后取重投影误差小的为最优位姿估计. -
代码实践:
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, 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;
}
- 实验效果: