视觉SLAM–视觉里程计-对极几何

完成特征提取以及特征匹配后，将基于关联后的特征点对进行配准以完成相机运动估计。当相机为单目相机时，我们只知道 2D 的像素坐标，因而问题是根据两组 2D 点估计运动。该问题用对极几何来解决。
解决思路为：提取ORB特征–>特征点匹配–>计算本质矩阵或基础矩阵–>估计相机运动。前两步骤已经探讨过了，接下来，我们将介绍对极几何约束，本质矩阵，基础矩阵。

对极几何约束

希望求取两帧图像 $I_1$和$I_2$之间的运动，设第一帧到第二帧的运动为R， t。两个相机中心分别为 $O_1$、 $O_2$。现在，考虑 $I_1$中有一个特征点 $p_1$，它在 $I_2$ 中对应着特征点$p_2$。我们把$O_1$、 $O_2$、P组成的平面称为极平面，$\overrightarrow{O_1{O}_2}$称为基线，而$\overrightarrow{O_1{O}_2}$和两个图像平面的交点$e_1$和$e_2$称为极点，极平面与图像平面的相交线$l_1$和$l_2$称为极线。对极几何约束的几何意义即为极平面的$O_1$、 $O_2$、P三点共面。
$x_1$和$x_2$分别为特征点 $p_1$和$p_2$在归一化平面上的坐标， $p_1$和$p_2$分别为在像素坐标系下坐标。对极几何约束的两个公式为$$x_2^{T}E{x}_1=0$$和$$p_2^{T}F{p}_1=0$$其中E为本质矩阵（Essential Matrix），F为基础矩阵（Fundamental Matrix），$F=K^{-T}E{K}^{-1}$，K为相机内参矩阵。对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

1. 根据配对点的像素位置，求出 E 或者 F；
2. 根据 E 或者 F，求出 R，t。

由于 E 和 F 只相差了相机内参，而内参在 SLAM 中通常是已知的，所以实践当中往往使用形式更简单的 E。我们以 E 为例，介绍上面两个问题如何求解。

本质矩阵求解运动估计

OpenCV cv::findEssentialMat 函数计算本质矩阵，输入参数为两帧图像的匹配特征点集，相机焦距，相机光心。
再利用 cv::recoverPose 函数求出R、t，输入参数为两帧图像的匹配特征点集，相机焦距，相机光心，本质矩阵。

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,std::vector<KeyPoint> keypoints_2,std::vector<DMatch> matches,Mat &R, Mat &t) {// 相机内参,TUM Freiburg2Mat 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;//-- 从本质矩阵中恢复旋转和平移信息.// 此函数仅在Opencv3中提供recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);cout << "R is " << endl << R << endl;cout << "t is " << endl << t << endl;
}