极线几何&单应性矩阵 理论+实践

前期预备知识

在开始阅读下面文章之前，首先需要掌握SVD分解数学知识、相机成像模型、向量的内积和外积相关知识，然后在进行后续阅读才会事半功倍。下面我们先来详细讲解一下相关知识：

• SVD分解数学知识

在讲解SVD分解之前，我们先看一下特征值分解（EVD），这里我们假设一个实对称$B_{m\ast m}$矩阵（$B_{m\ast m}^T=B_{m\ast m}$），可将其分为：

$$B_{m\ast m}=Q\Sigma Q^T=Q\left(\begin{array}{cccc}{\lambda }_1& \cdots & \cdots & \cdots \\ \cdots & {\lambda }_2& \cdots & \cdots \\ ⋮& ⋮& \ddots & ⋮\\ \cdots & \cdots & \cdots & {\lambda }_m\end{array}\right)Q^T$$
这里的矩阵$Q_{m\ast m}$为正交矩阵（$Q_{m\ast m}{Q}_{m\ast m}^T=I$，$I$为单位阵），数值${\lambda }_i$对应特征值，并且每个特征值对应一个特征向量$q_i$。特征值和特征向量具有的性质为：$B{q}_i={\lambda }_i{q}_i$。

上面的矩阵能通过特征分解，是因为矩阵为是实对称矩阵。但是我们实际应用过程中，很多矩阵并不一定满足此性质，并且很有可能不是方阵（$B_{m\ast n},m\ne n$，矩阵中行列数不相等）。在这种情况下，要进行分解就需要引入一个概念奇异值分解（Singular Value Decomposition，SVD）。下面来看一下SVD的定义：
对于矩阵$B_{m\ast n}$，可以将其分解为：
$$B_{m\ast n}=U\Sigma V^T=U\left(\begin{array}{cccc}{\sigma }_1& 0& \cdots & 0\\ 0& {\sigma }_2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 0\end{array}\right)V^T$$
这里的$U_{m\ast m}$和$V_{n\ast n}$均为正交矩阵（$U{U}^T=I,V{V}^T=I$），一般称$U$为左奇异矩阵，$V$为右奇异矩阵。$\Sigma$仅在主对角线上有值，其它位置处元素均为0。
知道了什么是SVD，下面聊一聊在实际应用中该如何求解呢？
$$\begin{gathered} B{B}^T=U{\Sigma }_1{V}^{T}V{\Sigma }_1^T{U}^T=U{\Sigma }_1{\Sigma }_1^T{U}^T \\ {B}^{T}B=V{\Sigma }_2^T{U}^{T}U{\Sigma }_2{V}^T=V{\Sigma }_2^T{\Sigma }_2{V}^T \end{gathered}$$
上面求解公式之所以能够进一步化简，是因为$U_{m\ast m}$和$V_{n\ast n}$均为正交矩阵。还需要说明的是 ${\Sigma }_1$和${\Sigma }_2$的维度是不相同的，${\Sigma }_1$为$m\ast m$方阵，${\Sigma }_2$为$n\ast n$方阵。
$$\begin{gathered} {\Sigma }_1={\left(\begin{array}{cccc}{\sigma }_1^2& 0& \cdots & 0\\ 0& {\sigma }_2^2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 0\end{array}\right)}_{m\ast m} \\ {\Sigma }_2={\left(\begin{array}{cccc}{\sigma }_1^2& 0& \cdots & 0\\ 0& {\sigma }_2^2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 0\end{array}\right)}_{n\ast n} \end{gathered}$$

虽然方阵${\Sigma }_1$和${\Sigma }_2$的维度不同，但是它们主对角线上的奇异值是相同的。通过上面的式子，我们也很轻易的得到$U$和$V$的对应值。

• SVD分解的应用
  我们知道了SVD分解的相关理论知识，现在简单聊一聊它的一些具体应用。估计很多玩图像的人，肯定会知道一个就是使用SVD来对数据进行降维（关于这块的知识大家查看SVD（奇异值分解）小结，这里实例说明了SVD的数据降维）；对于搞深度学习算法的，应该知道SVD可以对网络模型进行压缩（SVD压缩深度模型的全连接层）；经常玩SLAM的人，肯定知道就是使用SVD求解超定方程，来得到相机的运动姿态等等。本篇文章主要是讲极线几何来求解相机之间的位姿，所以这里将详讲一下SVD如何求解超定方程，来得到未知变量的值。
• SVD解超定方程
  这里说最简单的求解齐次方程$BX=0$，因为在求解极线几何的时候就需要求解齐次方程。这里我们很明显的会发现一个特解就是$X=0$的情况，所以这里需要对$X$的解进行约束（因为$X=0$的情况并不是我们想要的解）。我们添加一个约束$||X|{|}_2=1$，即假设$X$的范数为1。当然可以把它约束成任意的正整数，因为齐次方程乘以任意常数，还是满足方程的，毕竟等式右边为0。可能大家会很长时间没有接触范数啦，这里简单说明一下本篇用到的二范数定义：$||X|{|}_2=(|x_1^2|+|x_2^2|+\cdots +|x_n^2|{)}^{\frac{1}{2}}$。好啦，这里就可以将解齐次方程组的问题转化为：
  $$min(||BX|{|}_2),||X|{|}_2=1$$
  在求解之前，我们还需要知道正交矩阵$M$的一个性质：
  $$||MX||=||X||$$
  考虑到要使用这个性质，我们这里将优化问题的函数变为
  $$||BX|{|}_2=||U{\Sigma }_1{V}^{T}X|{|}_2=||{\Sigma }_1{V}^{T}X|{|}_2$$
  这里我们假设$Y=V^{T}X$，可以将问题转化为$min(||{\Sigma }_{1}Y|{|}_2),||Y|{|}_2=1$（这里需要说明一下，$||Y|{|}_2=1$是根据正交矩阵的性质推导过来的）。当${\Sigma }_1$ 的对角阵按照元素递减的顺序排列的时候，我们知道最优解的位置为：$Y=(0,0,\cdots ,1)$。因为$V$为正交矩阵，所以$X=VY$。即最优解在矩阵$V$最小奇异值的列向量。（关于解非齐次线性方程，请看：SVD解线性方程组——秘密大起底）

极线几何

在讲解之前我们还是要先了解一下，这个有什么作用？毕竟如果不是很重要，我们就不要学啦。其实，极线几何在SLAM中应用性是非常大。它可以根据相邻图像对应的特征点，来求解出相机之间的位姿变换。好啦，知道了有什么作用，我们来看一下极线几何中的一些定义。这里主要包括：极点、极线、基线、极平面。

我们以上面的图为例，来说明一下上述几个定义表示的什么。上图中$Q_1$表示图像$I_1$处的光心，$Q_2$表示图像$I_2$处的光心。$P$为三维空间中的一点，从每张图像光心与空间点连线，会与图像平面相交于一点$p_1$和$p_2$。其中，相交点$p_1$和$p_2$则就定义为极点。两张图像光心$Q_1$和$Q_2$的连线，构成基线。基线会与两张图像平面相交，得到交点$e_1$和$e_2$。在一个图像平面上，极点与交点的连线，构成极线$l_1$和$l_1$。两个光心与空间点构成一个平面$Q_{1}P{Q}_2$，此平面称为极平面。
好啦，知道了相关定义，下面我们就来看看，如何将其转化数学形式并进行求解。其实，这里的公式推导都是按照《SLAM十四讲》里面来的，只不过这里加入了一些自己的理解。这里我们假设图像$I_1$相机位置为世界起始坐标点，对应空间中一点$P=[X,Y,Z{]}^T$，相机的内参矩阵为$K$。相机$Q_1$到相机$Q_2$可由旋转矩阵$R$和平移矩阵$T$表示。根据相机成像模型（这块可参考：相机成像模型），可得变换矩阵：
$$\begin{gathered} s_1{p}_1=KP \\ {s}_2{p}_2=K(RP+t) \end{gathered}$$
这里$s_1$和$s_2$表示尺度因子，我们可以使用齐次坐标来表示，消去尺度因子得到：
$$\begin{gathered} p_1=KP \\ {p}_2=K(RP+t) \end{gathered}$$
在这里需要插一句，就是说明一下什么是齐次坐标，以及它有什么作用（我这里只是简单说，详细请看：为什么要引入齐次坐标，齐次坐标的意义）。齐次坐标最直观的定义就是：用N+1维来代表N维坐标。对应表示为：
$$(x,y,w)=(\frac{x}{w},\frac{y}{w})$$
使用齐次坐标，可以表示平行线在透视空间的无穷远处交于一点。在欧氏空间这将变得没有意义，所以欧式坐标不能表示。齐次坐标可以表示无穷远处的点， 齐次坐标为$(1,3,0)$，而笛卡尔坐标表示为$(\frac{1}{0},\frac{3}{0})=(\infty ,\infty )$
接着上面继续讲，这里假设$x_1$和$x_2$为像素点归一化平面上的坐标（其实就是，三维空间点的坐标都除以$Z$：什么是归一化的平面坐标）。这里补充一下，就是相机坐标系下，三维点坐标同时除以$Z$值：
$$\begin{gathered} x_1=K^{-1}{p}_1 \\ {x}_2=K^{-1}{p}_2 \end{gathered}$$
将$x_1$和$x_2$表示的方程带入到$p_1$和$p_2$表示的方程，可以得到：
$$x_2=R{x}_1+t$$
将上面方程同时乘以矩阵$t$的外积，这里使用$t^{\wedge }$来表示（这里是《SLAM十四讲》前几章讲解的内容）。我们都知道向量叉乘的数值表示为：$\mathbf{a}\times \mathbf{b}=|\mathbf{a}||\mathbf{b}|sin(\theta )$，其中$\theta$表示向量$\mathbf{a}$和向量$\mathbf{b}$的夹角。向量叉乘的方向符合右手定则（具体看：向量积）。知道了向量外积的定义，上述等式同时乘以$t^{\wedge }$可以得到：
$$t^{\wedge }{x}_2=t^{\wedge }R{x}_1$$
然后，上面等式同时乘以$x_2^T$得到：
$$x_2^T{t}^{\wedge }{x}_2=x_2^T{t}^{\wedge }R{x}_1$$
因为矩阵满足结合律，所以我们可以先计算$t^{\wedge }{x}_2$，我们知道得到的向量方向会与向量x_{2} 垂直，所以$x_2^T{t}^{\wedge }{x}_2=0$。因此，可以得到等式：
$$x_2^T{t}^{\wedge }R{x}_1=0$$
将$x_1$和$x_2$表示的等式带入到上述方程得到：
$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$$
我们在上面这个矩阵就可以得到两个概念一个为基础矩阵$F=K^{-T}{t}^{\wedge }R{K}^{-1}$，另外一个为本质矩阵$E=t^{\wedge }R$。它们之间最本质的区别在于，一个加入了内参矩阵$K$，另外一个没有加入。因此可以得到最终矩阵为：
$$p_2^{T}F{p}_1=p_2^T{K}^{-T}E{K}^{-1}{p}_1=0$$

本质矩阵的求解

本质矩阵和基础矩阵就相差了一个内参矩阵。所以这里选择本质矩阵来讲解求解的过程。在求解之前我们需要了解一下，本质矩阵$E$具有的三个性质：

• 不同尺度下等价： 因为本质矩阵$E$无论乘以什么值，由于等式右边为零，所以总是满足极线约束；
• $E$的奇异值必定为$[\delta ,\delta ,0]$：这个要证明的话，需要看论文，可以参考（《SLAM十四讲》论文目录里面的第三篇）
• 自由度：我们知道本质矩阵是由旋转和平移组成，旋转在空间中有3个自由度（绕$x,y,z$轴旋转），平移在空间中也有3个自由度（沿$x,y,z$轴平移）。但由于尺度等价性，自由度实际上有5个自由度。这里怎么理解5个自由度呢！其实很简单，我们本质矩阵乘以任意数值都满足极线约束，所以我们可乘以沿$z$轴平移数值的倒数。这样平移就变成了2个自由度，当然也可以将其它自由度化为1，主要是理解这个操作。
  好啦，知道了性质，我们就可以求解本质矩阵啦。这里假设两张图像匹配点，对应的归一化坐标为$x_1=[u_1,v_1,1{]}^T$和$x_2=[u_2,v_2,1{]}^T$，可以得到极线约束方程为：
  $$\left(\begin{array}{ccc}{u}_1& v_1& 1\end{array}\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}_2\\ v_2\\ 1\end{array}\right)=0$$
  这里将本质矩阵$E$转化为向量的形式，目的是为了后面构造超定方程：
  $$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$$
  因此可以写成线性形式：
  $$[u_1{u}_2,u_1{v}_2,u_1,v_1{u}_2,v_1{v}_2,v_1,u_2,v_2,1]\cdot e=0$$
  上面等式的转化就是最简单矩阵程序，把前面的系数提取出来即可。其它的对应点也同样表示出来得到,因为后面会说到八点法求解，所以这里写出八组对应点得到的超定方程：
  $$\left(\begin{array}{ccccccccc}{u}_1^1{u}_2^1& u_1^1{v}_2^1& u_1^1& v_1^1{u}_2^1& v_1^1{v}_2^1& v_1^1& u_2^1& v_2^1& 1\\ u_1^2{u}_2^2& u_1^2{v}_2^2& u_1^2& v_1^2{u}_2^2& v_1^2{v}_2^2& v_1^2& u_2^2& v_2^2& 1\\ u_1^1{u}_2^1& u_1^1{v}_2^1& u_1^1& v_1^1{u}_2^1& v_1^1{v}_2^1& v_1^1& u_2^1& v_2^1& 1\\ u_1^3{u}_2^3& u_1^3{v}_2^3& u_1^3& v_1^3{u}_2^3& v_1^3{v}_2^3& v_1^3& u_2^3& v_2^3& 1\\ u_1^4{u}_2^4& u_1^4{v}_2^4& u_1^4& v_1^4{u}_2^4& v_1^4{v}_2^4& v_1^4& u_2^4& v_2^4& 1\\ u_1^5{u}_2^5& u_1^5{v}_2^5& u_1^5& v_1^5{u}_2^5& v_1^5{v}_2^5& v_1^5& u_2^5& v_2^5& 1\\ u_1^6{u}_2^6& u_1^6{v}_2^6& u_1^6& v_1^6{u}_2^6& v_1^6{v}_2^6& v_1^6& u_2^6& v_2^6& 1\\ u_1^7{u}_2^7& u_1^7{v}_2^7& u_1^7& v_1^7{u}_2^7& v_1^7{v}_2^7& v_1^7& u_2^7& v_2^7& 1\\ u_1^8{u}_2^8& u_1^8{v}_2^8& u_1^8& v_1^8{u}_2^8& v_1^8{v}_2^8& v_1^8& u_2^8& v_2^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$$
  上面系数矩阵构成了$8\times 9$的矩阵，当系数矩阵矩阵为满秩矩阵时（秩为8），则零空间维度（定义可参考：零空间概念，Ax=0）为1。也就是$\mathbf{e}$构成了一条直线，满足尺度等价性。这样我们可以使用SVD分解系数矩阵（定义已经在上面讲解），可以得到下面解：
  $$\begin{gathered} t_1^{\wedge }=U{R}_{Z(\frac{\pi }{2})}\Sigma U^T,R_1=U{R}_{Z(\frac{\pi }{2})}^T{V}^T \\ {t}_2^{\wedge }=U{R}_{Z(-\frac{\pi }{2})}\Sigma U^T,R_2=U{R}_{Z(-\frac{\pi }{2})}^T{V}^T \end{gathered}$$
  $R_{Z(\frac{\pi }{2})}$表示沿$z$轴旋转90度。因为$-E$和$E$是等价矩阵（$-B=QEP$,$Q$和$P$为可逆矩阵），所以任意$t$取负号是相同。所以我们可以得到四个解：
  $$(t_1^{\wedge },R_1),(t_2^{\wedge },R_1),(t_1^{\wedge },R_2),(t_2^{\wedge },R_2)$$

四个解对应分别对应的空间点位置以及投影点位置。我们知道深度值肯定为正值所以$(2)$和$(3)$就不考虑啦，$(4)$不满足相机的成像模式，所以我们会根据空间位置和投影位置从四个解里面选择出一个正解。

在这里还需要指出一点，我们在前面已经说过啦。本质矩阵的奇异值为$[\delta ,\delta ,0]$，但是由于求解过程中的误差，我们往往求解出来的奇异值为$[{\delta }_1,{\delta }_2,{\delta }_3]$。这种情况下，该如何解决呢。其实，我们可以转化为求最优解问题：
$$min||E-E^{{}^{\prime }}|{|}_2=min||U(\Sigma -{\Sigma }^{{}^{\prime }})V^T|{|}_2=min||\Sigma -{\Sigma }^{{}^{\prime }}|{|}_2$$
我们假定$\Sigma =\left(\begin{array}{ccc}\delta & 0& 0\\ 0& \delta & 0\\ 0& 0& 0\end{array}\right)=0$，${\Sigma }^{{}^{\prime }}=\left(\begin{array}{ccc}{\delta }_1& 0& 0\\ 0& {\delta }_2& 0\\ 0& 0& {\delta }_3\end{array}\right)=0$。所以可以将最小化问题转化为：
$$min(({\delta }_1-\delta {)}^2+({\delta }_2-\delta {)}^2+({\delta }_3-0{)}^2{)}^{\frac{1}{2}}$$
求解上述方程可以得到$\delta =\frac{({\delta }_1+{\delta }_2)}{2}$，所以在实际应用中，我们可以得到奇异值$[\frac{({\delta }_1+{\delta }_2)}{2},\frac{({\delta }_1+{\delta }_2)}{2},0]$

单应性矩阵

知道了极线几何，再看单应性矩阵，那就太简单啦。我们还是先讨论一下，什么时候需要使用单应性矩阵。对于场景中特征特征点几乎全都分布在同一平面上（飞机俯视拍摄的场景等），这时候使用单应性矩阵来估计相机的运行最为合适。
知道了上面应用的背景，下面我们来看看它对应的数学推导，这个主要就涉及一个法向量表示平面方程，其它的就没有啥啦：
$$n^{T}P+d=0$$
上面为构建的特征点所落在平面对应的平面方程。后面就是化简带入：
$$-\frac{n^{T}P}{d}=1$$
$$\begin{gathered} p_2=K(RP+t) \\ {p}_2=K(RP+t\cdot (-\frac{n^{T}P}{d}))=K(R-\frac{t{n}^T}{d})K^{-1}{p}_1 \end{gathered}$$
我们使用$H$表示$K(R-\frac{t{n}^T}{d})K^{-1}$，因此$p_2=H{p}_1$。下面在求解的时候，就是我们在SLAM中经常使用到的DLT（Direct Linear Transform）求解。我们可以把等式转化为（和上面本质矩阵求解相似）：
$$\left(\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right)=\left(\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)$$
在实际应用中，往往会乘以一个非零因子使$h_9=1$，然后在去除这个非零因子，在这里我们写详细一点：
$$m\left(\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right)=\left(\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& 1\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)$$
我们可以得到对应解为：
$$\begin{gathered} m{u}_2=h_1{u}_1+h_2{v}_1+h_3 \\ m{v}_2=h_4{u}_1+h_5{v}_1+h_6 \\ m=h_7{u}_1+h_8{v}_1+1 \end{gathered}$$
在进行化简，分别除以$m$得到：
$$\begin{gathered} u_2=\frac{h_1{u}_1+h_2{v}_1+h_3}{h_7{u}_1+h_8{v}_1+1} \\ {v}_2=\frac{h_4{u}_1+h_5{v}_1+h_6}{h_7{u}_1+h_8{v}_1+1} \end{gathered}$$
化简上述方程可以得到：
$$\begin{gathered} u_2=h_1{u}_1+h_2{v}_1+h_3-h_7{u}_1{u}_2-h_8{v}_1{u}_2 \\ {v}_2=h_4{u}_1+h_5{v}_1+h_6-h_7{u}_1{v}_2-h_8{v}_1{v}_2 \end{gathered}$$
也就是一组对应点可以得到两个方程组，我们需要四组点，来求解8个未知参数，因为我们将$h_9=1$。所以方程为：
$$\left(\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{v}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{u}_2^1& -v_1^1{v}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{v}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{u}_2^2& -v_1^2{v}_2^2\\ u_1^3& v_1^3& 1& 0& 0& 0& -u_1^3{u}_2^3& -v_1^3{v}_2^3\\ 0& 0& 0& u_1^3& v_1^3& 1& -u_1^3{u}_2^3& -v_1^3{v}_2^3\\ u_1^4& v_1^4& 1& 0& 0& 0& -u_1^4{u}_2^4& -v_1^4{v}_2^4\\ 0& 0& 0& u_1^4& v_1^4& 1& -u_1^4{u}_2^4& -v_1^4{v}_2^4\end{array}\right)\left(\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right)=0$$

代码实现

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
// #include "extra.h" // use this if in OpenCV2 
using namespace std;
using namespace cv;

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

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 );

    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*scale
    Mat 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 OpenCV3 
    Ptr<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 Freiburg2
    Mat 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;

    //-- 从本质矩阵中恢复旋转和平移信息.
    recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
    

OK!终于写完啦！有什么问题请在下方留言。其实主要还是《SLAM十四讲》内容，只不过我在此基础上加入了一些自己的理解和说明而已。
了解更多关于《计算机视觉与图形学》相关知识，请关注公众号：

下载我们视频中代码和相关讲义，请在公众号回复：计算机视觉课程资料

参考的链接：

《SLAM十四讲》
SVD（奇异值分解）小结
Cmd Markdown 公式指导手册
SVD解线性方程组——秘密大起底
SLAM十四讲代码