特征点法

1. 特征点法
特征点法的思路，是先从图像当中提取许多特征，然后在图像间进行特征匹配，这样就得到许多匹配好的点，再根据这些点进行相机位姿的求解。相机位姿求解部分则和图像本身关系不大了。比方说下图是ORB特征匹配的结果。
特征匹配之后，你得到了一组配对点，以及它们的像素坐标。剩下的问题是说，怎么用这组配对点去计算相机的运动。这里，根据传感器形式的不同，分成三种情况：

你用的是单目相机，于是只有2D-2D的配对点；
你用的是RGBD或双目相机，于是你有3D-3D的配对点；
你只知道一张图中3D信息，另一张图只有2D信息，于是有3D-2D的配对点。

第三种情况可能出现在单目SLAM中，你通过之前的信息计算了3D的地图点，现在又来了一张2D图像，所以会有3D-2D的情况。或者，在RGBD和双目中，你也可以忽略其中一张图的3D信息，使用3D-2D的配对点。无论如何，这三种情况都是现实存在的，所以处理方式也分为三种。

为保持行文清楚，先来把变量设一下。假设某个左边的特征点叫 $p_1$ ，右边配对的点叫 $p_2$ ，它们都以像素坐标给出。同时，以左边图为参考系，设右边图相对它的运动由旋转和平移 $(R,t)$ 描述，相机内参为 $K$ 。由于这两个点肯定是同一个空间点P的投影，那么显然（哎百姓知道还得手敲公式编号）：

$d_1 {p_1} = KP,d_2 p_2 = K\left( {RP + t} \right)$ (1)
这里 $d_1,d_2$ 是两个点的距离， $p_1,p_2$ 要取齐次坐标， $P$ 取非齐次坐标。又说，既然左边都取齐次了，干脆齐次到底吧，于是去掉那俩深度：

${p_1} = KP, p_2 = K\left( {RP + t} \right)$ (2)

该等式在齐次意义下成立，也就是说乘以任意非零常数仍然是等的。不懂的同学请去学习小孔成像原理。我们觉得右边的内参挺烦人的，于是记

${x_1} = {K^{ - 1}}{p_1},{x_2} = {K^{ - 1}}{p_2}$ (3)

这俩货叫归一化相机坐标，也就是去掉内参之后的东西，剩下的就简单了：

${x_2} = R{x_1} + t$ (4)

就这样。所以相机位姿估计问题变为：你有很多个 $x_1,x_2$ ，怎么去算这里的 $R,t$ ？

------------------------------------------------

1.1 2D-2D，分解E和F的情况：

在2D-2D情况下，你只有两个点的2D坐标，这种情况出现在单目SLAM的初始化过程中。这时我们只有一个 (4) 式，还是乘任意常数都成立的操蛋情形。没办法，两边叉乘 $t$ 吧：

$t \times {x_2} = t \times R{x_1} + t \times t = t \times R{x_1}$ (5)

这东西右边的两个 $t$ ，自己叉自己就没了。然后，再同时两边左乘一个 $x_2^T$ ：

$x_2^T\left( {t \times {x_2}} \right) = 0 = x_2^Tt \times R{x_1}$ (6)

发现左边的 $x_2^T$ 乘了一个和自己垂直的东西，当然是零了，于是就只剩下：

$x_2^Tt \times R{x_1} =0$ (7)

一个东西等于零，看起来很帅哦。这个牛逼的玩意叫做对极约束（Epipolar Constraint）。简而言之，随便你出一组匹配点，都会有这么个约束成立。

对极约束这东西在几何上的意思就是这货的混合零（从图像角度来看），所以它们是共面的向量。既然两个匹配点是同一个点的投影，那不共面还能上天么？当然是共面的了。于是，为了好看，又把中间那俩定义成一个：

$E = t \times R$ (8)

这个E叫做Essential（本质）矩阵（别问我为什么叫Essential，就是这么叫的）。所以（7）变为：

$x_2^TE{x_1} = 0$ (9)

这个约束只有E，但我们的目标是求 $R,t$ 呀，于是求解变成了两步：

用一坨配对点算E；
用E算R,t

不妨再说说这两步怎么算。

从配对点算E：

最简单的方式是把E看成单纯的一个数值矩阵，忽略它里面各元素的内在联系。当然这么做的时候你实际要清楚E是有内在性质的，我就直接告诉你这货的奇异值是一个零加俩一样的数就完了，证明不写了。E由t和R的叉积组成，t是仨自由度，R是仨自由度，一共六个。又由于等式为零这样的约束，乘任意非零常数都成立，也就是对E随便乘个数，对极约束还是成立的，所以自由度减一，一共五个。因为E有五个自由度，所以最少拿五对匹配点可以把它算出来，这个乃是“五点法”（这帮搞CV的人脑子真朴实都不取个帅点的名字……）。

又，五点法用了E的奇异值这种奇怪的性质，对E引入了非线性约束，解起来麻烦。所以另一个法子是把E看作数值矩阵，然后解它每一个元素就行了呗。E一个九个数，去掉一个非零常数的因子，还剩八个自由度，所以最少拿八对匹配点就可以算出E，粗暴地把E拉成长条即可。比方说对极约束展开后是这样的：

$\begin{pmatrix} u_{1},v_{1},1 \end{pmatrix} \begin{pmatrix} e_{1} & e_{2} & e_{3}\\ e_{4} & e_{5} & e_{6}\\ e_{7} & e_{8} & e_{9} \end{pmatrix} \begin{pmatrix} u_{2}\\v_{2}\\1 \end{pmatrix} =0.$ (10)

把E拉成一个向量扔到右边：

$[u_{1}u_{2},u_{1}v_{2},u_{1},v_{1}u_{2},v_{1}v_{2},u_{2},v_{2},1] \cdot \bm{e}=0.$ (11)

这里：

$\bm{e}= [e_{1},e_{2},e_{3},e_{4},e_{5},e_{6},e_{7},e_{8},e_{9}]^{T}$ (12)

简单吧，现在你搞出了一个线性方程。当你有八对点时，就变成了方程组，磊起来是这样的：

$\begin{pmatrix} 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\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 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{pmatrix} \begin{pmatrix} e_{1}\\ e_{2}\\ e_{3}\\ e_{4}\\ e_{5}\\ e_{6}\\ e_{7}\\ e_{8}\\ e_{9} \end{pmatrix} =0.$ (13)

然后就是爱怎么解就怎么解了。可逆时求逆，不可逆时求伪逆和最小二乘解，矩阵论里都有，不说了。这个方法最少用八对点，所以叫什么？对，八点法（你倒是取个好听点的名字啊喂）。

然后就是用E算R,t的问题了。

这个推导也没啥好说的，直接给答案吧，推起来太麻烦。先把E给奇异值分解了：

$\bm{E} = \bm{U} \bm{\Sigma} \bm{V}^T$ (14)

完了之后这么一算就得到了R,t：

$\begin{array}{l} \bm{t}_1^ \wedge = \bm{U}{\bm{R}_Z}(\frac{\pi }{2}) \bm{\Sigma} {\bm{U}^T}, \quad {\bm{R}_1} = \bm{U} \bm{R}_Z^T(\frac{\pi }{2}){ \bm{V}^T}\\ \bm{t}_2^ \wedge = \bm{U}{\bm{R}_Z}( - \frac{\pi }{2})\bm{\Sigma} {\bm{U}^T}, \quad {\bm{R}_2} = \bm{U} \bm{R}_Z^T( - \frac{\pi }{2}){\bm{V}^T}. \end{array}$ (15)

这里对任意一个t加个相反号，对极约束仍然满足，所以你会得到四个解。这四个解画出来是这样的：

怎么看这个图呢？两个小红点是我们找的配对点，它们都是P的投影。你会看到这四个解里小红点的位置都是不变的。那么哪种情况是真实的呢？废话，当然是第一种。因为只有第一种情况里，P出现在相机的前面。什么？你的相机还能看到身后的东西？你确实不是在逗我？

于是，在验证之后，就能确定唯一的解了。另外再啰嗦一句，当你不知道内参时，只有像素坐标，那么对极约束为：

$\bm{p}_2^T \bm{K}^{-T} \bm{t} \times \bm{R} \bm{K}^{-1} \bm{p}_1 = 0.$ (16)

这时中间那货叫做F（Fundamental，基本矩阵），和E大同小异但是性质比E麻烦点。因为SLAM里通常认为相机已经标定好了所以也用不着它了。

------------------------------------------------

1.2 2D-2D，分解H的情况
另一种情况是你找的那些点都位于一个平面上，比如说你的相机是朝天花板或地板看的，这时候分解E和F会出现退化，要用另一种方式来解。

这图来自wikipedia.

你们不是在平面上吗？来啊，我们就把平面搞出来。平面方程为：

$\bm{n}^T \bm{P} + d = 0.$ (17)

然后对两个点，有：

$\begin{align*} \bm{p}_2 &= \bm{K} ( \bm{R} \bm{P} + \bm{t} ) \\ &= \bm{K} \left( \bm{R} \bm{P} + \bm{t} \cdot (- \frac{\bm{n}^T \bm{P} }{d}) \right) \\ &= \bm{K} \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{P} \\ &= \bm{K} \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{K}^{-1} \bm{p}_1. \end{align*}$ (18)

这个式子的好处是直接推出了两个坐标之间的关系。把中间那坨东西记为 $\bm{H}$ （Homography，单应矩阵），于是：

$\bm{p}_2 = \bm{H} \bm{p}_1.$ (19)

这货也没啥大不了的。和之前一样，问题变为：

怎么用给定的一堆匹配点算H；
怎么用H算出R,t,n,d

讲起来又是一堆麻烦事。总之第一步比较容易，把H拉成一长条扔到一边求个线性方程组就行了；第二步比较麻烦，要用到SVD和QR分解。最后你会得到八组解，然后有一串步骤告诉你如何从这八组解里选出最好的。步骤实在是比较长我就懒得写了。总之你要知道，在特征点位于平面上时，分解H；否则分解E或F。就这样.

------------------------------------------------

1.2 2D-2D，讨论

稍微说几句。2D-2D的情况出现在单目SLAM的初始化中，你没有别的信息，只能这样子做。其中，分解E或F的过程中存在几个问题。E这个东西具有尺度等价性，随便乘个数仍是同一个。所以拿它分解得到的R,t也有一个尺度等价性，特别是t上有一个因子，而 $\bm{R} \in SO(3)$ 自身具有约束，没有关系。换言之，在分解过程中，对 $\bm{t}$ 乘以任意非零常数，分解都是成立的，这个叫做单目SLAM的尺度不确定性。因此，我们通常把t进行归一化，让它的长度等于1。或者让场景中特征点的平均深度等于1，总之是有个比♂例的。

因为尺度不确定性的问题，对t长度归一化后，导致了t的单位问题，不知道其输出的数值单位是多少，虽然不知道它的实际长度为多少，但我们这时以t为单位1，计算相机运动和特征点的3D位置，称为单目SLAM初始化。

在初始化之后，可以用3D-2D来计算运动，初始化后的轨迹和地图的单位，就是初始化时固定的尺度。初始化化后的轨迹和地图的单位，就是初始化时固定的尺度。单目SLAM一定要初始化，初始化的俩张图像必须有一定程度的平移，而后的轨迹和地图都以此步的平移为单位。

此外，分解E的过程中，如果相机发生的是纯旋转，导致t为零，那么，得到的E也将为零。零分个毛线！于是，另一个结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移！必须要有一定程度的平移！必须要有一定程度的平移！

------------------------------------------------

1.3 3D-3D，ICP：

下面来讨论简单点的情况：你不光得到了匹配点，还知道这两组匹配点的深度，于是有了3D-3D的匹配。因为你知道匹配，这种情况下 R,t 的估计是有解析解（闭式解）的。否则，如果只有两堆点而不知道匹配，则要用迭代最近点（Iterative Closest Point, ICP）求解。闭式解可以稍加推导，不喜欢看推导的同学可以跳过。

假定你找的两组点是这样的：

$\bm{P} = \{ \bm{p}_1, \ldots, \bm{p}_n \}, \quad \bm{P}' = \{ \bm{p}_1', \ldots, \bm{p}_n'\}$

配对好之后，每个点满足关系：

$\forall i, \bm{p}_i = \bm{R} \bm{p}_i' + \bm{t}.$

一开始不知道R,t，所以算一个误差再求它最小化。误差为：

$\bm{e}_i = \bm{p}_i - (\bm{R} \bm{p}_i' + \bm{t} ) .$

最小化它：

$\mathop {\min }\limits_{\bm{R}, \bm{t}} J = \frac{1}{2} \sum\limits_{i = 1}^n\| {\left( {{\bm{p}_i} - \left( {\bm{R}{\bm{p}_i}' + \bm{t}} \right)} \right)} \|^2_2.$

简单吧。这里可以用一个技巧，先把两组点的质心设出来，记住不带下标的是质心：

$\bm{p} = \frac{1}{n}\sum_i^n ( \bm{p}_i ), \quad \bm{p}' = \frac{1}{n} \sum_i^n ( \bm{p}_i' ).$

然后处理一下目标函数：

$\begin{align*} \begin{array}{ll} \frac{1}{2}\sum\limits_{i = 1}^n {{{\left\| {{\bm{p}_i} - \left( {\bm{R}{ \bm{p}_i}' + \bm{t}} \right)} \right\|}^2}} & = \frac{1}{2}\sum\limits_{i = 1}^n {{{\left\| {{\bm{p}_i} - \bm{R}{\bm{p}_i}' - \bm{t} - \bm{p} + \bm{Rp}' + \bm{p} - \bm{Rp}'} \right\|}^2}} \\ & = \frac{1}{2}\sum\limits_{i = 1}^n {{{\left\| {\left( {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right) + \left( {\bm{p} - \bm{Rp}' - \bm{t}} \right)} \right\|}^2}} \\ & = \frac{1}{2}\sum\limits_{i = 1}^n {{\left\| {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right\|}^2} + {{\left\| {\bm{p} - \bm{Rp}' - \bm{t}} \right\|}^2} +\\ & \quad \quad 2{{\left( {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right)}^T}\left( {\bm{p} - \bm{Rp}' - \bm{t}} \right). \end{array} \end{align*}$

这里的技巧无非是先加一项再减一项而已。注意到交叉项部分中， $\left( {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right)$ 在求和之后是为零的，因此优化目标函数可以简化为：

$\mathop {\min }\limits_{\bm{R}, \bm{t}} J = \frac{1}{2}\sum\limits_{i = 1}^n {{\left\| {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right\|}^2} + {{\left\| {\bm{p} - \bm{Rp}' - \bm{t}} \right\|}^2} .$

嘛，这两项里，左边只和旋转矩阵R相关，而右边既有R也有t，但只和质心相关。因此，只要我们获得了R，令第二项为零就能得到t。于是，ICP可以分为以下几个步骤求解：

计算两组点的质心；
计算去质心坐标： $\bm{q}_i = \bm{p}_i - \bm{p}, \quad \bm{q}_i' = \bm{p}_i' - \bm{p}'.$
求解旋转R；
根据旋转和质心解t： $\bm{t}^* = \bm{p} - \bm{R} \bm{p}'.$

t很简单，问题是R怎么解？这东西的平方误差展开为：

$\frac{1}{2}\sum\limits_{i = 1}^n \left\| {{\bm{q}_i} - \bm{R} \bm{q}_i' } \right\|^2 = \frac{1}{2}\sum\limits_{i = 1}^n \bm{q}_i^T \bm{q}_i + \bm{q}_i^{ \prime T} \bm{R}^T \bm{R} \bm{q}^\prime_i - 2\bm{q}_i^T \bm{R} \bm{q}^\prime_i.$

注意到第一项和R无关，第二项由于 $\bm{R}^T\bm{R}=\bm{I}$ ，亦与R无关。因此，实际上优化目标函数变为：

$\sum\limits_{i = 1}^n - \bm{q}_i^T \bm{R} \bm{q}^\prime_i = \sum\limits_{i = 1}^n -\mathrm{tr} \left( \bm{R} \bm{q}_i^{\prime} \bm{q}_i^T \right) = - \mathrm{tr} \left( \bm{R} \sum\limits_{i = 1}^n \bm{q}_i^{\prime} \bm{q}^{ T}_i \ \right).$

这个优化问题的解法见文献[1]，这里只给结果。首先定义：

$\bm{W} = \sum\limits_{i = 1}^n \bm{q}_i \bm{q}^{\prime T}_i.$

对W进行SVD分解，然后令：

$\bm{R} = \bm{U} \bm{V}^T.$

于是就得到了旋转。

总之就是有闭式解，很简单，因为有匹配。在不知道匹配的时候，情况比较麻烦，通常你要假设最近点是配对点，所以叫迭代最近点。但是既然我在讲特征点法，匹配就是知道的，什么迭代最近见鬼去吧。

------------------------------------------------

1.4 3D-2D，PnP

PnP（Perspective n Points）就是你有n个点的3D位置和它们的投影，然后要算相机的位姿。这个倒是SLAM里最常见的情况，因为你会有一堆的地图点和像素点等着你算。PnP的做法有好多种：直接线性变换，P3P，EPnP，UPnP等等，基本你去找OpenCV的SolvePnP中的参数即可，好用的都在那里。除此之外，通常认为线性方程解PnP，在有噪声的情况下表现不佳，所以一般以EPnP之类的解为初始值，构建一个Bundle Adjustment（BA）去做优化。上面那堆算法题主自己查文献比较好，有大量的实现细节。当然你也可以完全不鸟他们，直接调cv的函数，反正人家早实现好
了……

扯到BA不妨多说几句，BA其实蛮容易理解的，只是名字听上去不那么直观。首先，你有3D点：

$\bm{P}_i=[X_i,Y_i,Z_i]^T$

然后你又知道了投影：

$d_i \bm{p}_i = \bm{K} (\bm{RP}_i + \bm{t})$

于是算一个误差：

$\bm{e}_i = \bm{p}_i - \frac{1}{d_i} \bm{K} (\bm{RP}_i+\bm{t})$

然后让它们最小化：

${\bm{T} ^*} = \arg \mathop {\min }\limits_{\bm{T}} \frac{1}{2}\sum\limits_{i = 1}^n {\left\| {{{\bm{p}}_i} - \frac{1}{s_i} \bm{K} (\bm{R}{{\bm{P}}_i}}+\bm{t}) \right\|_2^2} .$

就行了。这就叫最小化重投影误差，也叫BA。当然实际算的时候，由于R,t自身带有约束，所以要转到李代数上算，这里不展开。

直观的解释如上图。我们通过特征匹配，知道了 $p_1$ 和 $p_2$ 是同一个空间点P的投影，但是不知道相机的位姿。在初始值中，P的投影 $\hat{p}_2$ 与实际的 $p_2$ 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。总之，我们就寄希望于这个误差会越调越小了。为什么越调越小呢？因为我们往往会沿着负梯度方向去调呗。当然解释起来又得涉及一些非线性优化的东西，什么高斯牛顿之类的，请查非线性优化教材。

BA是万金油，你看哪个问题不爽就把它扔到优化目标里，然后让计算机帮你优化就行。当然这东西非凸的时候要当心初值，否则一不小心就掉在局部坑里爬不出来……

猜你喜欢