09_对极约束与单应矩阵

前言

根据上一节的介绍，我们已经有了匹配好的点对，然后我们就可以根据点对估计相机的运动。这里由于相机的原理不同，我们要分情况讨论：

如果是单目相机，此时我们仅知道2D的像素坐标，要根据两组2D点估计相机运动，这种情况要用对极几何来解决。
如果是双目或者RGB-D相机，此时我们知道深度信息，也就是我们可以通过3D点来估算运动，这种情况用ICP解决。
如果我们知道一些3D点和相机的投影位置，此时我们用PnP解决。

2D-2D：对极几何

对于单目相机，假设我们已经从两张图像中得到了一张匹配正确的点对，将问题可以抽象成下图所示。我们希望求取两帧图像为 I 1 I_{1} I1， I 2 I_{2} I2之间的运动，设第一帧到第二帧的运动为 R R R， t t t，两个相机的中心为 O 1 O_{1} O1， O 2 O_{2} O2。点 P P P在 I 1 I_{1} I1中的特征点为 p 1 p_{1} p1，点 P P P在 I 2 I_{2} I2中的特征点为 p 2 p_{2} p2。

对极几何主要涉及以下几个元素：

基线（baseline）：两个相机中心的连线 O 1 O 2 O_{1}O_{2} O1O2称为基线。
对极点（epipolar）： e 1 , e 2 e_{1},e_{2} e1,e2是对极点，是基线和两个成像平面的交点，也就是对极点在一幅视图中另一个相机中心的像；这里在解释一下， e 1 e_{1} e1是右边的相机中心 O 2 O_{2} O2在左边相机的像点，同样 e 2 e_{2} e2是左边相机中心 O 1 O_{1} O1在右边相机的像点。
对极平面（epipolar plane）：任何过基线的平面都被才称为对极平面，两个相机的中心 O 1 O_{1} O1和 O 2 O_{2} O2，三维点 P P P，以及其在两个相机的像点 p 1 , p 2 p_{1},p_{2} p1,p2，这5点必定在同一个对极平面上。当三维点 P P P变化时，对极平面绕着基线旋转，形成对极平面束。
对极线（epipolar line）：对极平面和成像平面的交线 l 1 l_{1} l1， l 2 l_{2} l2，所有的对极线相交于极点。

上面我们了解了一些简单的几何知识，但有一个地方要注意一下，如果没有特征匹配，我们就无法确定 p 2 p_{2} p2在具体在极线的哪个位置。遇到这种的情况此时就需要在极线上搜索以获取正确的匹配，这种情况我们后面在讨论。

我们用从代数的方法来分析上图的几何关系。设在第一帧的坐标系下，设 P P P的空间位置为

P = [ X , Y , Z ] T . P =[X, Y, Z]^{\mathrm{T}}. P=[X,Y,Z]T.

根据之前我们讲解的相机模型，可知像素点 p 1 p_{1} p1， p 2 p_{2} p2的像素坐标为

s 1 p 1 = K P , s 2 p 2 = K ( R P + t ) . s_{1} p_{1}=K P, \quad s_{2} p_{2}=K(R P+t). s1p1=KP,s2p2=K(RP+t).

其中 K K K为相机的内参矩阵， R R R， t t t为两个坐标系的相机运动。

我们使用齐次坐标来表示像素点。使用齐次坐标主要是为了表达投影关系，例如， s 1 p 1 s_{1} p_{1} s1p1和 p 1 p_{1} p1成投影关系，它们在齐次坐标的意义下相等。这种关系成为尺度意义下相等，则有：

s p ≃ p . s p \simeq p . sp≃p.

根据投影关系可有

p 1 ≃ K P , p 2 ≃ K ( R P + t ) . p_{1} \simeq K P, \quad p_{2} \simeq K(R P+t). p1≃KP,p2≃K(RP+t).

我们使用归一化平面坐标，则有

x 1 = K − 1 p 1 , x 2 = K − 1 p 2 . x_{1}=K^{-1} p_{1}, \quad x_{2}=K^{-1} p_{2}. x1=K−1p1,x2=K−1p2.

继续整理，带入化简可有

x 2 ≃ R x 1 + t . x_{2} \simeq R x_{1}+t . x2≃Rx1+t.

两边同时左乘 t ∧ t^{\wedge} t∧（简单回忆一下 a ∧ a = 0 a^{\wedge} a=0 a∧a=0）,则有

t ∧ x 2 ≃ t ∧ R x 1 . t^{\wedge} x_{2} \simeq t^{\wedge} R x_{1}. t∧x2≃t∧Rx1.

两边同时左乘 x 2 T x_{2}^{\mathrm{T}} x2T，则有

x 2 T t ∧ x 2 ≃ x 2 T t ∧ R x 1 . x_{2}^{\mathrm{T}} t^{\wedge} x_{2} \simeq x_{2}^{\mathrm{T}} t^{\wedge} Rx_{1}. x2Tt∧x2≃x2Tt∧Rx1.

到了这里要注意一下，向量 t ∧ x 2 t^{\wedge} x_{2} t∧x2与向量 t t t、 x 2 x_{2} x2都垂直。所以在和 x 2 x_{2} x2做内积，将严格为零。所以有

x 2 T t ∧ R x 1 = 0. x_{2}^{\mathrm{T}} t^{\wedge} Rx_{1}=0. x2Tt∧Rx1=0.

将 p 1 p_{1} p1， p 2 p_{2} p2带入可得

p 2 T K − T t ∧ R K − 1 p 1 = 0. p_{2}^{T} K^{-T} t^{\wedge} R K^{-1} p_{1}=0. p2TK−Tt∧RK−1p1=0.

上面两个式子称为对极约束。它的几何意义为 O 1 O_{1} O1， P P P， O 2 O_{2} O2三者共面。我们令中间部分两个矩阵为基础矩阵（Fundamental Matrix）和本质矩阵（Essential Matrix），进一步化简则有

E = t ∧ R , F = K − T E K − 1 , x 2 T E x 1 = p 2 T F p 1 = 0. E=t^{\wedge} R, \quad F=K^{-\mathrm{T}} E K^{-1}, \quad x_{2}^{\mathrm{T}} Ex_{1}=p_{2}^{\mathrm{T}} F p_{1}=0. E=t∧R,F=K−TEK−1,x2TEx1=p2TFp1=0.

我们根据上面的代数推导，所以相机位姿估计步骤为：

1．根据配对点的像素位置求出 E E E或者 F F F。

2．根据 E E E或者 F F F求出 R R R， t t t。

本质矩阵

本质矩阵 E E E有如下特点：

它是 3 × 3 3 \times 3 3×3的矩阵，有9个未知数；
由于对极约束是等式为零的约束，所以对 E E E乘以任意非零常数后，对极约束依然满足。也就是我们前面提到的尺度等价性。
可以证明，本质矩阵的奇异值一定是 [ σ , σ , 0 ] T [\sigma, \sigma, 0]^{\mathrm{T}} [σ,σ,0]T的形式。
由于平移和旋转各有3个自由度，故 E E E共有6个自由度。但由于尺度的等价性，故 E E E实际共有5个自由度。

我们通常使用八点法来求解 E E E。设一对匹配点为 x 1 x_{1} x1， x 2 x_{2} x2，它们的归一化坐标为 x 1 = [ u 1 , v 1 , 1 ] T x_{1}=\left[u_{1}, v_{1}, 1\right]^{T} x1=[u1,v1,1]T， x 2 = [ u 2 , v 2 , 1 ] T x_{2}=\left[u_{2}, v_{2}, 1\right]^{T} x2=[u2,v2,1]T。由对极约束，则有

( u 2 , v 2 , 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) ( u 1 v 1 1 ) = 0. \left(u_{2}, v_{2}, 1\right)\left(\begin{array}{ccc} e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9} \end{array}\right)\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)=0. (u2,v2,1)⎝⎛e1e4e7e2e5e8e3e6e9⎠⎞⎝⎛u1v11⎠⎞=0.

我们把矩阵 E E E展开，写成向量形式：

e = [ e 1 , e 2 , e 3 , e 4 , e 5 , e 6 , e 7 , e 8 , e 9 ] T , e =\left[e_{1}, e_{2}, e_{3}, e_{4}, e_{5}, e_{6}, e_{7}, e_{8}, e_{9}\right]^{\mathrm{T}}, e=[e1,e2,e3,e4,e5,e6,e7,e8,e9]T,

将其整理成关于 e e e的线性形式，则有

[ u 2 u 1 , u 2 v 1 , u 2 , v 2 u 1 , v 2 v 1 , v 2 , u 1 , v 1 , 1 ] ⋅ e = 0. \left[u_{2} u_{1}, u_{2} v_{1}, u_{2}, v_{2} u_{1}, v_{2} v_{1}, v_{2}, u_{1}, v_{1}, 1\right] \cdot e=0. [u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1]⋅e=0.

同理可得，我们将上面线性形式扩展成8个特征点的形式，其中 u i , v i u^{i}, v^{i} ui,vi表示第 i i i个特征点，整理可得：

( u 2 1 u 1 1 u 2 1 v 1 1 u 2 1 v 2 1 u 1 1 v 2 1 v 1 1 v 2 1 u 1 1 v 1 1 1 u 2 2 u 1 2 u 2 2 v 1 2 u 2 2 v 2 2 u 1 2 v 2 2 v 1 2 v 2 2 u 1 2 v 1 2 1 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ u 2 8 u 1 8 u 2 8 v 1 8 u 2 8 v 2 8 u 1 8 v 2 8 v 1 8 v 2 8 u 1 8 v 1 8 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) = 0. \left(\begin{array}{ccccccccc} u_{2}^{1} u_{1}^{1} & u_{2}^{1} v_{1}^{1} & u_{2}^{1} & v_{2}^{1} u_{1}^{1} & v_{2}^{1} v_{1}^{1} & v_{2}^{1} & u_{1}^{1} & v_{1}^{1} & 1 \\ u_{2}^{2} u_{1}^{2} & u_{2}^{2} v_{1}^{2} & u_{2}^{2} & v_{2}^{2} u_{1}^{2} & v_{2}^{2} v_{1}^{2} & v_{2}^{2} & u_{1}^{2} & v_{1}^{2} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{2}^{8} u_{1}^{8} & u_{2}^{8} v_{1}^{8} & u_{2}^{8} & v_{2}^{8} u_{1}^{8} & v_{2}^{8} v_{1}^{8} & v_{2}^{8} & u_{1}^{8} & v_{1}^{8} & 1 \end{array}\right)\left(\begin{array}{c} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{array}\right)=0. ⎝⎜⎜⎜⎛u21u11u22u12⋮u28u18u21v11u22v12⋮u28v18u21u22⋮u28v21u11v22u12⋮v28u18v21v11v22v12⋮v28v18v21v22⋮v28u11u12⋮u18v11v12⋮v18111⎠⎟⎟⎟⎞⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛e1e2e3e4e5e6e7e8e9⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞=0.

根据上面的线性方程组，这样就解出了本质矩阵 E E E。我们接下来的工作就是通过本质矩阵 E E E，恢复出相机的运动 R R R， t t t。

设 E E E的SVD的分解为

E = U Σ V T E=U \Sigma V^{T} E=UΣVT

其中 U , V U, V U,V为正交阵， Σ \Sigma Σ为奇异值矩阵。对于任意一个 E E E，分解到 t t t， R R R有四种可能的解。我们把任意一个点带入到四种解中，如果两个相机都为正的深度，即为正确的解。

单应矩阵

单应矩阵（Homography） H H H描述的是两个平面之间的映射关系。若场景中的特征点都落在同一平面上，则可以通过单应性进行运动估计。我们还是和前面一样，进行简单的数学推导。假设我们使用同一相机在不同的位姿下拍摄同一平面，如图所示：

其中平面 π \pi π表示两个相机的成像平面，设平面 π \pi π在第一个相机坐标系下的单位法向量为 N N N，第一个相机中心到平面 π \pi π的距离为 d d d，则

N T X 1 = d . N^{T} X_{1}=d. NTX1=d.

即为

1 d N T X 1 = 1 , ∀ X 1 ∈ π . \frac{1}{d} N^{T} X_{1}=1, \forall X_{1} \in \pi. d1NTX1=1,∀X1∈π.

同样使 X 1 X_{1} X1( X 1 X_{1} X1是三维点 P P P在第一相机坐标系下的坐标)经过旋转和平移到第二个相机坐标系下为 X 2 X_{2} X2，则

X 2 = R X 1 + t . X_{2}=R X_{1}+t. X2=RX1+t.

将上式整理化简，则

X 2 = R X 1 + t 1 d N T X 1 = ( R + t 1 d N T ) X 1 = H ′ X 1 . X_{2}=R X_{1}+t \frac{1}{d} N^{T} X_{1}=\left(R+t \frac{1}{d} N^{T}\right) X_{1}=H^{\prime} X_{1}. X2=RX1+td1NTX1=(R+td1NT)X1=H′X1.

故我们就得到了同一平面在不同相机坐标系下的单应矩阵

H ′ = R + t 1 d N T . H^{\prime} =R+t \frac{1}{d} N^{T}. H′=R+td1NT.

H ′ H^{\prime} H′矩阵为第一个相机坐标系变换到第二个相机坐标所得，我们还需将其转化为成像平面 π \pi π。设 x 1 x_{1} x1， x 2 x_{2} x2为 P P P在两图像的像点坐标，

x 1 = K X 1 , x 2 = K X 2 . x_{1}=K X_{1}, x_{2}=K X_{2}. x1=KX1,x2=KX2.

其中 K K K为相机内参矩阵，整理上式进行单应变换则有

x 2 = K H ′ K − 1 x 1 = K ( R + t 1 d N T ) K − 1 x 1 . x_{2}=K H^{\prime} K^{-1} x_{1}=K\left(R+t \frac{1}{d} N^{T}\right) K^{-1} x_{1}. x2=KH′K−1x1=K(R+td1NT)K−1x1.

故同一平面得到的两个图像间的单应矩阵 H H H为

H = K ( R + t 1 d N T ) K − 1 . H=K\left(R+t \frac{1}{d} N^{T}\right) K^{-1}. H=K(R+td1NT)K−1.

关于 H H H矩阵的求解，它和本质矩阵的方式类似，最后都是将 H H H矩阵分解为 R R R和 t t t，通过先验信息得到正确的解。

当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声，这时候如果继续使用过八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 F F F和单应矩阵 H H H，选择重投影误差比较小的那个座位最终的运动估计矩阵。

我们简单总结一下基础矩阵 F F F，本质矩阵 E E E和单应矩阵之间的关系：

E E E矩阵表示空间中的一个三维点在不同相机视角下投影点在相机坐标系下的约束关系。
F F F矩阵表示空间中的一个三维点在不同相机视角下投影点在图像坐标系下的约束关系。
F F F矩阵比 E E E矩阵多了内参矩阵 K K K的信息， F F F矩阵自由度更高，求解 R R R， t t t使用 E E E更准确。
若相机只有旋转而没有平移的情况下，此时 E E E矩阵和 F F F矩阵为零矩阵，无法求出 R R R矩阵，这是可以使用 H H H矩阵描述两个图像像点的对应关系。

实践：对极约束求解相机运动

我们还是使用OpenCV提供的接口，最后分解出 R R R和 t t t。代码如下：

#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;void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches);void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,std::vector<KeyPoint> keypoints_2,std::vector<DMatch> matches,Mat &R, Mat &t);// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);int main(int argc, char **argv) {if (argc != 3) {cout << "usage: pose_estimation_2d2d img1 img2" << 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 && img_2.data && "Can not load images!");vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);cout << "一共找到了" << matches.size() << "组匹配点" << endl;//-- 估计两张图像间运动Mat R, t;pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);//-- 验证E=t^R*scaleMat t_x =(Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),t.at<double>(2, 0), 0, -t.at<double>(0, 0),-t.at<double>(1, 0), t.at<double>(0, 0), 0);cout << "t^R=" << endl << t_x * R << endl;//-- 验证对极约束Mat K = (Mat_<double>(3, 3) << 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_<double>(3, 1) << pt1.x, pt1.y, 1);Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);Mat d = y2.t() * t_x * R * y1;cout << "epipolar constraint = " << d << endl;}return 0;
}void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches) {//-- 初始化Mat descriptors_1, descriptors_2;// used in OpenCV3Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// use this if you are in OpenCV2// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//-- 第一步:检测 Oriented FAST 角点位置detector->detect(img_1, keypoints_1);detector->detect(img_2, keypoints_2);//-- 第二步:根据角点位置计算 BRIEF 描述子descriptor->compute(img_1, keypoints_1, descriptors_1);descriptor->compute(img_2, keypoints_2, descriptors_2);//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离vector<DMatch> match;//BFMatcher matcher ( NORM_HAMMING );matcher->match(descriptors_1, descriptors_2, match);//-- 第四步:匹配点对筛选double min_dist = 10000, max_dist = 0;//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for (int i = 0; i < descriptors_1.rows; i++) {double dist = match[i].distance;if (dist < min_dist) min_dist = dist;if (dist > 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 < descriptors_1.rows; i++) {if (match[i].distance <= max(2 * min_dist, 30.0)) {matches.push_back(match[i]);}}
}Point2d pixel2cam(const Point2d &p, const Mat &K) {return Point2d((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,std::vector<KeyPoint> keypoints_2,std::vector<DMatch> matches,Mat &R, Mat &t) {// 相机内参,TUM Freiburg2Mat 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;}