3D视觉(五)：对极几何和三角测量

3D视觉(五)：对极几何和三角测量

对极几何（Epipolar Geometry）描述的是两幅视图之间的内在射影关系，与外部场景无关，只依赖于摄像机内参数和这两幅试图之间的的相对姿态。

一、对极几何

假设我们从两张图像中得到了一对配对好的点对，如果有若干对这样的匹配点对，就可以通过这些二维图像点的对应关系，恢复出在两帧之间的摄像机的运动。

从代数角度来分析这里的几何关系。在第1帧的坐标系下，设P的空间位置为：
P = [X, Y, Z]

根据针孔相机模型，可以得到两个像素点p1、p2的像素位置坐标。这里K为相机内参矩阵，R、t为两个坐标系之间的相机运动。
s1p1 = KP
s2p2 = K（RP + t）

s1p1和p1成投影关系，它们在齐次坐标的意义下是相等的，我们称这种相等关系为尺度意义下相等（equal up to a scale），记作：s1p1～p1。于是上述两个投影关系可写为：
p1 ～ KP
p2 ～ K（RP + t）

将像素坐标系下的坐标转化成归一化平面坐标系下的坐标。方程左右两边同时左乘inv(K)，并记x1 = inv(K) p1、x2 = inv(K) p2，这里x1、x2是两个像素点的归一化平面上的坐标，代入上式得：
x2 ～ R x1 + t

方程两边同时左乘t ^，相当于两侧同时与t做外积，可以消去等式右边的第二项：
t ^ x2 ～ t ^ R x1

然后两侧再同时左乘x2.T，由于t ^ x2是一个与t、x2都垂直的向量，它再与x2做内积时将得到0。化零之后尺度意义下的相等被转化为了严格等号，便能得到一个简单的恒等式：
x2.T t ^ R x1 = 0

重新代入p1、p2，有：
p2.T inv(K).T t ^ R inv(K) p1 = 0

这两个式子都称为对极约束，它以形式简洁著名。它的几何意义是O1、P、O2三点共面，对极几何约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵：基础矩阵F（Fundamental Matrix）、本质矩阵E（Essential Matrix），于是可以进一步化简对极约束：

E = t ^ R
F = inv(K).T E inv(K)
x2.T E x1 = 0
p2.T F p1 = 0

对极约束简洁地给出了两个匹配点的空间位置关系，于是相机位姿估计问题转变为如下两步：
第1步：根据配对点的像素位置，解线性方程组，求出E或者F。
第2步：基于旋转矩阵特有的性质，基于矩阵分析理论，从E或者F中分解出R和t。

二、三角测量

三角测量是指，通过不同位置对同一个路标点进行观察，从观察到的位置推断路标点的距离。三角测量最早由高斯提出并应用于测量学中，它在天文学、地理学的测量中都有应用。例如：在天文学中，我们可以通过不同季节观测到的星星角度，来估计它与我们的距离。在计算机视觉中，我们可以通过观测不同图片中标志点的位置，利用三角化来估计标志点的距离。

按照对极几何中的定义，设x1、x2为两个特征点的归一化坐标，它们满足：
s2 x2 = s1 R x1 + t

利用对极几何，我们已经求解得到了R和t，现在想进一步得到两个特征点的深度信息s1、s2。我们对上式两侧左乘一个x2 ^，得：
s2 x2 ^ x2 = 0 = s1 x2 ^ R x1 + x2 ^ t

该式左侧为零，右侧可看成s2的一个方程，根据它可以直接求得s2。一旦求解得到s2，s1也可非常容易求出，于时我们就得到了两帧下的点的深度，即确定了它们的3D空间坐标。

三、实验过程

利用双目摄像头拍摄得到左右目图片：

提取ORB特征，进行特征点配对：

利用对极几何，求得两幅图像位姿变换关系为：

利用三角测量，以第一幅图像的相机视角为3D坐标系，还原得到每个landmark标志点的3D坐标：

我们在图片对应的标志点位置，绘图显示深度信息，绿色代表距离相机越近，红色代表距离相机越远，中间以渐变颜色显示，可视化效果如下：

根据实验结果，三角测量的确能还原出landmark标志点距离相机的深度，但计算结果较为粗糙，只能得到大致的远近度量，难以用米来精确刻画，距离误差很大。另外对极几何算法鲁棒性不强，一旦特征点配对错误，还原出的位姿变换矩阵误差很大，从而导致三角测量还原出的深度信息误差很大，根本无法使用。

四、源码

对极几何：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>


using namespace std;
using namespace cv;


// 利用匹配好的特征点对、landmark位置，基于对极几何方法，估计旋转矩阵R、平移向量t

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          const Mat &K, Mat &R, Mat &t) {

  // 把匹配点对转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  // 将对应landmark标志点的像素坐标，分别push到points1、points2容器中
  
  cout << "匹配点对的像素位置: " << endl;
  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);
    
    cout << keypoints_1[matches[i].queryIdx].pt << "  " << keypoints_2[matches[i].trainIdx].pt << endl;
  }

  // 计算基础矩阵 F
  Mat fundamental_matrix;
  fundamental_matrix = findFundamentalMat(points1, points2);
  cout << endl << "基础矩阵 F: " << endl << fundamental_matrix << endl << endl;

  // 计算本质矩阵 E
  Point2d principal_point(K.at<double>(0, 2), K.at<double>(1, 2));  //相机光心
  double focal_length = K.at<double>(0, 0);      //相机焦距
  
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
  cout << "本质矩阵 E: " << endl << essential_matrix << endl << endl;

  // 计算单应矩阵 H，但在本例中场景中，特征点并不都落在同一个平面上，单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);
  cout << "单应矩阵 H: " << endl << homography_matrix << endl << endl;

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

}

三角测量：

#include <iostream>
#include <opencv2/opencv.hpp>


using namespace std;
using namespace cv;


// 保留指定区间的深度信息，此范围内深度估计值较为精确，深度估计值小于low_th的会被赋值成low_th，深度估计值大于up_th的会被赋值成up_th
// 借助rgb颜色可视化像素点位置的深度，绿色表示距离近，红色表示距离远

cv::Scalar get_color(float depth) {
    
  float up_th = 21, low_th = 16, th_range = up_th - low_th;
  
  if (depth > up_th) depth = up_th;
  if (depth < low_th) depth = low_th;
  
  // 如果距离较近，会接近绿色；如果距离较远，会接近红色
  return cv::Scalar(0, 255 * (1 - (depth - low_th) / th_range), 255 * (depth - low_th) / th_range);
  
}


// 将像素坐标系下的坐标，转换到归一化坐标系下的坐标，(u, v) - (x, y, 1)
// x = (u - cx) / fx
// y = (v - cy) / fy

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


// 三角测量，计算对极几何尺度s1、s2，还原每个landmark标志点的深度信息，landmark 3d坐标信息存储于points容器中

void triangulation(const vector<KeyPoint> &keypoint_1, 
                   const vector<KeyPoint> &keypoint_2,
                   const std::vector<DMatch> &matches,
                   const Mat &K, const Mat &R, const Mat &t,
                   vector<Point3d> &points) {
  
  // 默认以第一幅图像的相机坐标系为基准，还原出来的深度信息，就是相对于第一幅图像视角下的深度
    
  Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);
  Mat T2 = (Mat_<float>(3, 4) <<
    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));

  vector<Point2f> pts_1, pts_2;
  
  // 将像素坐标转换成归一化平面坐标，存储于pts_1、pts_2
  for (DMatch m:matches) {
    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
  }
  
  // cv::triangulatePoints函数，输出的3D坐标是齐次坐标，共四个维度，因此需要将前三个维度除以第四个维度，得到非齐次坐标x、y、z
  Mat pts_4d;
  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

  // 转换成非齐次坐标
  for (int i = 0; i < pts_4d.cols; i++) {
      
    Mat x = pts_4d.col(i);
    x /= x.at<float>(3, 0); // 归一化，四个分量全除以最后一位，将第四位数值转化为1
    
    Point3d p(x.at<float>(0, 0), x.at<float>(1, 0), x.at<float>(2, 0));
    points.push_back(p);
    
  }
}

五、项目链接

如果代码跑不通，或者想直接使用我自己制作的数据集，可以去下载项目链接：
https://blog.csdn.net/Twilight737