视觉里程计基础学习:对极约束求解估计相机运动显示出筛选特征点后的图像
对极约束原理
求取两帧图像 I1, I2 之间的运动,设第一帧到第二帧的运动为R, t。两个相机中心分别为 O1, O2。现在,考虑 I1 中有一个特征点 p1,它在 I2 中对应着特征点 p2。我们晓得这俩是通过特征匹配得到的。如果匹配正确,说明它们确实是同一个空间点在两个成像平面上的投影。这里我们需要一些术语来描述它们之间的几何关系。首先,连线 −−−→O1p1 和连线 −−−→O2p2 在三维空间中会相交于点 P。这时候点 O1, O2, P 三个点可以确定一个平面,称为极平面(Epipolar plane)。O1O2 连线与像平面 I1, I2 的交点分别为 e1, e2。e1, e2,称为极(Epipoles),O1O2 被称为基线(Baseline)。称极平面与
两个像平面 I1, I2 之间的相交线 l1, l2 为极线(Epipolar line)。
直观上讲,从第一帧的角度上看,射线 −−−→O1p1 是某个像素可能出现的空间位置——因
为该射线上的所有点都会投影到同一个像素点。同时,如果不知道 P 的位置,那么当我们
在第二个图像上看时,连线 −−→e2p2(也就是第二个图像中的极线)就是 P 可能出现的投影的位置,也就是射线 −−−→O1p1 在第二个相机中的投影。现在,由于我们通过特征点匹配,确定了 p2 的像素位置,所以能够推断 P 的空间位置,以及相机的运动。
在第一帧的坐标系下,设 P 的空间位置为:P = [X, Y, Z]T .
根据针孔相机模型
我们知道两个像素点 p1, p2 的像素位置为:s1p1 = KP , s2p2 = K (RP + t)
这里 K 为相机内参矩阵,R, t 为两个坐标系的相机运动如果使用齐次坐标,我们也可以把上式写成在乘以非零常数下成立的等式:p1 = KP , p2 = K (RP + t)
取x1 = K 1p1, x2 = K 1p2,这里的 x1, x2 是两个像素点的归一化平面上的坐标
代入上式得:x2 = Rx1 + t
两边同时与t做外积,然后在左乘xT得:xT2 t∧x2 = xT2 t∧Rx1
这两个式子都称为对极约束。它的几何意义是 O1, P, O2 三者共面。对极约束中同时包含了平移和旋转。
把中间部分记作两个矩阵:基础矩阵(FundamentalMatrix)F 和本质矩阵(Essential Matrix)E,可以进一步简化对极约束:
对极约束简洁地给出了两个匹配点的空间位置关系
相机位姿估计问题就分为了两步:
- 根据配对点的像素位置,求出 E 或者 F;
- 根据 E 或者 F,求出 R, t。
E为本质矩阵,F为基础矩阵,
后面还会有单应矩阵H,它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上(墙面、地面等),则可以通过单应性来进行运动估计。
对极约束求解估计相机运动
//-- 估计两张图像间运动
Mat R,t;
pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );
//-- 验证E=t^R*scale
//求t的反对称矩阵
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 = FeatureDetec