【SLAM】对极几何学习与实践

Carlos橘子柠檬

已于 2022-02-26 13:54:09 修改

阅读量923

点赞数

分类专栏： SLAM 文章标签：学习计算机视觉矩阵

于 2020-05-26 15:23:24 首次发布

本文链接：https://blog.csdn.net/heroybc/article/details/106357146

版权

SLAM 专栏收录该内容

15 篇文章 18 订阅

订阅专栏

一、对极约束

1.前言

问：对极约束是用来干嘛的？什么是对极约束？
答：对于已经知道的图像2D点，通过若干点来匹配得出相机的运动情况，什么是对极约束呢，接下来我会介绍一下。

如上图所示，可以简单的了解了什么是对极约束。

对极约束是一种约束条件，是在我们不知道三维点P的真实空间三维位置的时候，通过相关的几何关系将三维P点在第二帧的图像上有可能的投影位置在约束条件下为一条直线上（就是极线）。然后，通过相关特征匹配，我们确定两个三维点p2与p1一样，都是空间点P的投影。

2.约束

我们通过相关计算，经过相机内参、旋转矩阵、平移矩阵就可以推出，基础矩阵(Fun-damental Matrix)F和本质矩阵(Essential Matrix)E。

通过上式，可以看出，要求得的旋转矩阵、平移矩阵都是可以通过基础矩阵(Fun-damental Matrix)F 和本质矩阵(Essential Matrix)E求出来的。现在，相机的运动问题就转化为了，如何求取基础矩阵(Fun-damental Matrix)F 和本质矩阵(Essential Matrix)E的问题。

二、本质矩阵

如上图所示，相机的运动估计可以通过本质矩阵求取得到。

我们再看看本质矩阵的公式，可以发现，旋转矩阵有3个自由度，平移矩阵有3个自由度，而本质矩阵是一个3*3的矩阵，就是9个未知数。

那是不是要估计本质矩阵就需要9个匹配的特征点呢，不是这样的，上一节我们讲了极线约束，在极限约束的条件下，本质矩阵具有尺度不变性，所只需要8个点进行估计，就是所谓的8点法。

得到了本质矩阵，如何求取旋转矩阵和平移矩阵呢？

就需要奇异值分解了。

由于本质矩阵的内在性质，本质矩阵的奇异值必为（，，0）的形式。奇异值分解后会有四个结果，但是只有一种结果符合现实情况。

三、单应矩阵

单应矩阵是什么呢？

单应矩阵描述了从平面到平面的映射关系，如上图的H矩阵。

单应矩阵也是一个3*3的矩阵，理论来讲可以通过9个点去估计。

在计算单应矩阵的时候，由于矩阵的约束概念和自由度为8的单应矩阵，一对特征点可以构造出2个约束，4对特征点就可以完成8自由度的单应矩阵的求解。

其求解过程和本质矩阵类似，也有奇异值分解后的4个结果，当然，只有一个结果符合实际。

四、实践

#include &lt;iostream&gt;
#include &lt;opencv2/core/core.hpp&gt;
#include &lt;opencv2/features2d/features2d.hpp&gt;
#include &lt;opencv2/highgui/highgui.hpp&gt;
#include &lt;opencv2/calib3d/calib3d.hpp&gt;
// #include "extra.h" // use this if in OpenCV2

using namespace std;
using namespace cv;

/@@****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * 本程序演示参考https://github.com/gaoxiang12/slambook2
 * **************************************************/

void find_feature_matches(
  const Mat &amp;img_1, const Mat &amp;img_2,
  std::vector&lt;KeyPoint&gt; &amp;keypoints_1,
  std::vector&lt;KeyPoint&gt; &amp;keypoints_2,
  std::vector&lt;DMatch&gt; &amp;matches);

void pose_estimation_2d2d(
  std::vector&lt;KeyPoint&gt; keypoints_1,
  std::vector&lt;KeyPoint&gt; keypoints_2,
  std::vector&lt;DMatch&gt; matches,
  Mat &amp;R, Mat &amp;t);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &amp;p, const Mat &amp;K);

int main(int argc, char **argv) {
  if (argc != 3) {
    cout &lt;&lt; "usage: pose_estimation_2d2d img1 img2" &lt;&lt; endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  assert(img_1.data &amp;&amp; img_2.data &amp;&amp; "Can not load images!");

  vector&lt;KeyPoint&gt; keypoints_1, keypoints_2;
  vector&lt;DMatch&gt; matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout &lt;&lt; "一共找到了" &lt;&lt; matches.size() &lt;&lt; "组匹配点" &lt;&lt; endl;

  //-- 估计两张图像间运动
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

  //-- 验证E=t^R*scale
  Mat t_x =
    (Mat_&lt;double&gt;(3, 3) &lt;&lt; 0, -t.at&lt;double&gt;(2, 0), t.at&lt;double&gt;(1, 0),
      t.at&lt;double&gt;(2, 0), 0, -t.at&lt;double&gt;(0, 0),
      -t.at&lt;double&gt;(1, 0), t.at&lt;double&gt;(0, 0), 0);

  cout &lt;&lt; "t^R=" &lt;&lt; endl &lt;&lt; t_x * R &lt;&lt; endl;

  //-- 验证对极约束
  Mat K = (Mat_&lt;double&gt;(3, 3) &lt;&lt; 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_&lt;double&gt;(3, 1) &lt;&lt; pt1.x, pt1.y, 1);
    Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    Mat y2 = (Mat_&lt;double&gt;(3, 1) &lt;&lt; pt2.x, pt2.y, 1);
    Mat d = y2.t() * t_x * R * y1;
    cout &lt;&lt; "epipolar constraint = " &lt;&lt; d &lt;&lt; endl;
  }
  return 0;
}

void find_feature_matches(const Mat &amp;img_1, const Mat &amp;img_2,
                          std::vector&lt;KeyPoint&gt; &amp;keypoints_1,
                          std::vector&lt;KeyPoint&gt; &amp;keypoints_2,
                          std::vector&lt;DMatch&gt; &amp;matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr&lt;FeatureDetector&gt; detector = ORB::create();
  Ptr&lt;DescriptorExtractor&gt; descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr&lt;FeatureDetector&gt; detector = FeatureDetector::create ( "ORB" );
  // Ptr&lt;DescriptorExtractor&gt; descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr&lt;DescriptorMatcher&gt; matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector-&gt;detect(img_1, keypoints_1);
  detector-&gt;detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor-&gt;compute(img_1, keypoints_1, descriptors_1);
  descriptor-&gt;compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector&lt;DMatch&gt; match;
  //BFMatcher matcher ( NORM_HAMMING );
  matcher-&gt;match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i &lt; descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist &lt; min_dist) min_dist = dist;
    if (dist &gt; max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

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

Point2d pixel2cam(const Point2d &amp;p, const Mat &amp;K) {
  return Point2d
    (
      (p.x - K.at&lt;double&gt;(0, 2)) / K.at&lt;double&gt;(0, 0),
      (p.y - K.at&lt;double&gt;(1, 2)) / K.at&lt;double&gt;(1, 1)
    );
}

void pose_estimation_2d2d(std::vector&lt;KeyPoint&gt; keypoints_1,
                          std::vector&lt;KeyPoint&gt; keypoints_2,
                          std::vector&lt;DMatch&gt; matches,
                          Mat &amp;R, Mat &amp;t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_&lt;double&gt;(3, 3) &lt;&lt; 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

  //-- 把匹配点转换为vector&lt;Point2f&gt;的形式
  vector&lt;Point2f&gt; points1;
  vector&lt;Point2f&gt; points2;

  for (int i = 0; i &lt; (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 &lt;&lt; "fundamental_matrix is " &lt;&lt; endl &lt;&lt; fundamental_matrix &lt;&lt; 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 &lt;&lt; "essential_matrix is " &lt;&lt; endl &lt;&lt; essential_matrix &lt;&lt; endl;

  //-- 计算单应矩阵
  //-- 但是本例中场景不是平面，单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);
  cout &lt;&lt; "homography_matrix is " &lt;&lt; endl &lt;&lt; homography_matrix &lt;&lt; endl;

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

}

五、总结

旋转矩阵和平移矩阵有着尺度不变性，可以进行归一化，对平移矩阵的归一化会导致单目视觉的尺度不确定，所以需要初始化。通常情况下，对于本质矩阵可以通过随机采样一致性去求解。

Carlos橘子柠檬

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
1
评论
【SLAM】对极几何学习与实践

一、对极约束1.前言问：对极约束是用来干嘛的？什么是对极约束？答：对于已经知道的图像2D点，通过若干点来匹配得出相机的运动情况，什么是对极约束呢，接下来我会介绍一下。如上图所示，可以简单的了解了什么是对极约束。对极约束是一种约束条件，是在我们不知道三维点P的真实空间三维位置的时候，通过相关的几何关系将三维P点在第二帧的图像上有可能的投影位置在约束条件下为一条直线上（就是极线）。然后，通过相关特征匹配，我们确定两个三维点p2与p1一样，都是空间点P的投影。2.约束我们通过相关计算，经过相机
复制链接

扫一扫