[转] 视觉里程计-对极几何

最新推荐文章于 2024-10-04 21:37:57 发布

Zolony

最新推荐文章于 2024-10-04 21:37:57 发布

阅读量118

点赞数

分类专栏： SLAM进阶之路文章标签：数码相机计算机视觉人工智能

原文链接：https://blog.csdn.net/weixin_43891708/article/details/121136651

版权

SLAM进阶之路专栏收录该内容

16 篇文章 1 订阅

订阅专栏

视觉里程计-对极几何

完成特征提取以及特征匹配后，将基于关联后的特征点对进行配准以完成相机运动估计。当相机为单目相机时，我们只知道 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_1O_2}$ 称为基线，而 $\overrightarrow{O_1O_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^T_2Ex_1 = 0$ 和 $p^T_2Fp_1 = 0$ 其中E为本质矩阵（Essential Matrix），F为基础矩阵（Fundamental Matrix）， $F = K^{-T}EK^{-1}$ ，K为相机内参矩阵。对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

根据配对点的像素位置，求出 E 或者 F；
根据 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 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;

  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;
}