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