SLAM视觉里程计：图像去畸变与对极几何估计相机运动

之前做了手机相机的标定，得到了相机的内参矩阵和径向畸变系数 $k_1,k_2$ ，于是想先尝试一下从两张相邻图片估计相机的运动，步骤大致如下：

• 图像去畸变处理。
• 匹配相邻图像特征点。
• 从匹配点估计相机的运动。

1. 图像去畸变处理

1.1. 原理

在标定手机相机内参时，就已经考虑到了图像的畸变。公式如下：

径向畸变+切向畸变：
$\left\{\begin{matrix} x_{corrected}=x(1+k_1r^2+k_2r^2+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\\ \\ y_{corrected}=y(1+k_1r^2+k_2r^2+k_3r^6)+p_1(r^2+2y^2)+2p_2xy \end{matrix}\right.$

则有：
$\left\{\begin{matrix} u=f_x x_{corrected}+c_x\\ \\ v=f_yy_{corrected}+c_y \end{matrix}\right.$

这里 $x$， $y$， $x_{corrected}$， $y_{corrected}$ 均为归一平面上的点。

我们需要没有畸变的图像，则相当于把去畸变的图像上的每一点与原图像相对应，即相当于把去畸变图像上的一点加上畸变，与原图对应的点相对应。

设去畸变的图像上一点 $p_c=[u_c,v_c]^T$ ，则把此点转化到相机的归一化平面为：
$[x,y,1]=[\frac{u_c-c_x}{f_x},\frac{v_c-c_y}{f_y},1]^T$

再用畸变关系得到加上畸变后的图像位置 $p=[u,v]^T$ 。

这里有一个问题：我们计算得到的 $u, v$ 不一定为整数，而图像上的点的RGB（或灰度）值则相当于图像转化乘的矩阵在第 $v$ 行、第 $u$ 列的值， $u,v$ 应为整数。故我们这里要用二维线性插值来确定这个点 $[u_c,v_c]$ 的值。

这里有一篇介绍二维线性插值的文章。

大致的内容是这样的，如上图，我们要求对应的原图项上的点 (v+dv, u+du) 的值，即根据与之相邻的四个点的值来计算为 (与一维线性方法一样) ：
$P=(1-du)\cdot(1-dv)\cdot A+du\cdot(1-dv)\cdot B+du\cdot dv\cdot C+(1-du)\cdot dv\cdot D$

如此，我们便能求出去畸变图像上每一个点的像素值。

1.2. OpenCV 实现

void cv::undistort( InputArray  	src,
					OutputArray  	dst,
					InputArray  	cameraMatrix,
					InputArray  	distCoeffs,
					InputArray  	newCameraMatrix = noArray() 
	)
//newCameraMatrix 为去畸变后图像对应的相机内参矩阵，默认为与之前相同

Mat cv::getOptimalNewCameraMatrix( 	InputArray  	cameraMatrix,
									InputArray  	distCoeffs,
									Size  			imageSize,
									double  		alpha,
									Size  			newImgSize = Size(),
									Rect *  		validPixROI = 0,
									bool  			centerPrincipalPoint = false 
	)
//alpha的值为0-1，0则输出内矩阵，即去畸变图像没有黑色边框。1则输出外矩阵，即去畸变图像包括原图像所有点。

下为直接用cv::undistort做的图像去畸变：

左为原图，右为去畸变后的图像。因为 $k_1$ 很小， $k_2$ 稍微大一点，所以可以看到图像中心附近基本没有什么改变，离中心较远的地方略微有点变化。

---

2. 匹配相邻图像特征点

具体原理见《SLAM 十四讲》，书上讲的极其详细。

代码思路如下：

//1--初始化
//两个vector放两个图像的角点，两个Mat存储对应的描述子
std::vector<KeyPoint> k1;
std::vector<KeyPoint> k2;
Mat descriptors_1, descriptors_2;

//用于提取角点和提取对应的描述子
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();

//用于匹配特征点，距离为Hamming
Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );

//用于存放所有匹配点和筛选后的匹配点
std::vector<DMatch> match;
std::vector<DMatch> matches;

//2--提取并匹配特征点
//检测角点并提取相应描述子
detector->detectAndCompute( img_1, noArray(), keypoints_1, descriptors_1 );
detector->detectAndCompute( img_2, noArray(), keypoints_2, descriptors_2 );

//匹配特征点
matcher->match( descriptors_1, descriptors_2, match );

//3--筛选特征点
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
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 );

//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值25作为下限.
for ( int i = 0; i < descriptors_1.rows; i++ ) {
	if ( match[i].distance <= max ( 2*min_dist, 25.0 ) ) {
		matches.push_back ( match[i] );
	}
}

//4--绘制匹配结果
Mat img_match;
drawMatches( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
imshow ( "所有匹配点对", img_match );

这里 k1[ matches[i].queryIdex ].pt与 k2[ matches[i].queryIdex ].pt就是两幅图对应的特征点的坐标，输出类型是Point2f。

匹配运行结果如下：

---

3. 从匹配点估计相机运动

3.1. 对极几何

如上图： $p_1,p_2$ 为两个匹配好的特征点。 $\Pi_{O_1O_2P}$ 是极平面， $O_1O_2$ 与平面 $I_1,I_2$ 分别交于 $e_1,e_2$ ，这两点被称为极点。 $O_1O_2$ 被称为基线，直线 $l_1=p_1e_1,l_2=p_2e_2$ 被称为极线。

具体推导过程见《SLAM 十四讲》，这里直接给出结论：
$p^T_2 K^{−T} t^∧ RK^{−1} p_1 = 0$

其中 $K$ 为相机的内参矩阵， $R,t$ 为相机从 $O_1$ 运动到 $O_2$ 的旋转矩阵和平移向量。

这里可以简化：本质矩阵 $E = t^∧ R$ ，基础矩阵 $F = K^{−T} EK^{ −1}$ 。

于是，在解出本质矩阵后，我们就可以用奇异值分解的方法解出旋转矩阵和平移向量。

3.2. 实现

//把匹配点转换为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 ( cam.cx_, cam.cy_ );	//相机光心, TUM dataset标定值
double focal_length = ( cam.fx_+cam.fy_ )/2;			//相机焦距, TUM dataset标定值
Mat essential_matrix; 
essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
E = essential_matrix;
cout<<"essential_matrix is "<<endl<< essential_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;

这里cam为一个存储相机参数的类，keypoints_1，keypoints_2和matches都是上一步做好的特征点匹配。

下面介绍一下 findEssentialMat：

Mat cv::findEssentialMat( 	InputArray  	points1,
							InputArray  	points2,
							double  		focal = 1.0,
							Point2d  		pp = Point2d(0, 0),
							int  			method = RANSAC,
							double  		prob = 0.999,
							double  		threshold = 1.0,
							OutputArray  	mask = noArray() 
	)

• 这里 method 包括 RANSAC和LMEDS，是处理那一对堆匹配点的算法，具体参见RANSAC LMedS 详细分析。
• prob是置信区间，范围是 0-1 。
• threshold是RANSAC算法里的误差阈值，即为点到极线的最大距离，这个用来判断点是 inlier 还是 outlier 。

介绍一下findFundamentalMat：

Mat cv::findFundamentalMat( InputArray  	points1,
							InputArray  	points2,
							int  			method = FM_RANSAC,
							double  		ransacReprojThreshold = 3,
							double  		confidence = 0.99,
							OutputArray  	mask = noArray() 
	)

• method ：
  CV_FM_7POINT：7点算法，输入点的数量为7。
  CV_FM_8POINT ：8点算法，输入点的数量大于等于8。
  CV_FM_RANSAC ：RANSAC 算法， $N≥8$ 。
  CV_FM_LMEDS：LMedS 算法， $N≥8$ 。
• ransacReprojThreshold：同之前的threshold。