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

本文主要是介绍3D视觉(五)：对极几何和三角测量，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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

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

文章目录

3D视觉(五)：对极几何和三角测量
一、对极几何
二、三角测量
三、实验过程
四、源码
五、项目链接

一、对极几何

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

在这里插入图片描述
从代数角度来分析这里的几何关系。在第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

************************************************************************************************************** 补充：记向量a = [a1, a2, a3].T、b = [b1, b2, b3].T，则外积a x b为： a x b = [a2b3- a3b2, a3b1 - a1b3, a1b2 - a2b1] = [0, -a3, a2; a3, 0, -a1; -a2, a1, 0] b = a^ b。我们引入^符号，把向量a转化成一个反对称矩阵a ^，这样就把外积a x b的计算过程，转化成了矩阵与向量的乘法a ^ b，把它变成了线性运算。

方程两边同时左乘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、平移向量tvoid 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;}// 计算基础矩阵 FMat fundamental_matrix;fundamental_matrix = findFundamentalMat(points1, points2);cout << endl << "基础矩阵 F: " << endl << fundamental_matrix << endl << endl;// 计算本质矩阵 EPoint2d 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) / fyPoint2f 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_2for (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、zMat 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); // 归一化，四个分量全除以最后一位，将第四位数值转化为1Point3d p(x.at<float>(0, 0), x.at<float>(1, 0), x.at<float>(2, 0));points.push_back(p);}
}