相机位姿问题的特征点法求解——高翔（转载自泡泡机器人）

原创 2016-11-28 高翔 泡泡机器人SLAM

【泡泡机器人原创专栏-位姿求解】相机位姿问题的特征点法求解

（点开，进入微信，刷新下就可以看见了）

欢迎大家在周日来到泡泡机器人讲堂，本次我们将为大家介绍相机位姿问题的求解，相机位姿估计是指给定若干图像，估计其中相机运动的问题。求解方法通常分特征点法和直接法两种，这也是视觉里程计的两类基本方法。本次主要为大家讲解特征点法。

特征点法的思路，是先从图像当中提取许多特征，然后在图像间进行特征匹配，这样就得到许多匹配好的点，再根据这些点进行相机位姿的求解。相机位姿求解部分则和图像本身关系不大了。比方说下图是ORB特征匹配的结果。

特征匹配之后，你得到了一组配对点，以及它们的像素坐标。剩下的问题是说，怎么用这组配对点去计算相机的运动。这里，根据传感器形式的不同，分成三种情况：

• 你用的是单目相机，于是只有2D-2D的配对点；

• 你用的是RGBD或双目相机，于是你有3D-3D的配对点；

• 你只知道一张图中3D信息，另一张图只有2D信息，于是有3D-2D的配对点。

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

为保持行文清楚，先来把变量设一下。假设某个左边的特征点叫，右边配对的点叫，它们都以像素坐标给出。同时，以左边图为参考系，设右边图相对它的运动由旋转和平移描述，相机内参为。由于这两个点肯定是同一个空间点P的投影，那么显然:

(1)

这里是两个点的距离，要取齐次坐标，取非齐次坐标。又说，既然左边都取齐次了，干脆齐次到底吧，于是去掉那俩深度：

  (2)

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

(3)

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

  (4)

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

1.已知2D-2D配对点，求解位姿

---

---

 

1.1.分解E和F的情况

---

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

(5)

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

(6)

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

  (7)

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

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

 (8)

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

  (9)

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

• 用一坨配对点算E；

• 用E算R,t

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

1.1.1.从配对点算E

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

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

 (10)

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

  (11)

这里：

  (12)

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

  (13)

然后就是爱怎么解就怎么解了。可逆时求逆，不可逆时求伪逆和最小二乘解，矩阵论里都有，不说了。这个方法最少用八对点，所以叫什么？对，八点法。

1.1.2.用E算R,t

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

 (14)

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

 (15)

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

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

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

  (16)

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

 1.2.分解H的情况

---

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

这图来自wikipedia.

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

  (17)

然后对两个点，有：

  (18)

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

  (19)

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

• 怎么用给定的一堆匹配点算H；

• 怎么用H算出R,t,n,d

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

 1.3.讨论

---

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

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

程度的平移！

（手持单目时不能原地旋转，必须像结印那样有平移）

 

2.已知3D-3D配对点，求解位姿

---

---

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

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

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

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

最小化它：

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

然后处理一下目标函数：

 

这里的技巧无非是先加一项再减一项而已。注意到交叉项部分中，在求和之后是为零的，因此优化目标函数可以简化为：

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

• 计算两组点的质心；

• 计算去质心坐标：

• 求解旋转R；

• 根据旋转和质心解t：

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

注意到第一项和R无关，第二项由于，亦与R无关。因此，实际上优化目标函数变为：

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

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

于是就得到了旋转。

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

3.已知3D-2D配对点，求解位姿

---

---

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

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

然后你又知道了投影：

于是算一个误差：

然后让它们最小化：

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

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

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

 

参考文献： 

[1] Arun, K Somani and Huang, Thomas S and Blostein, Steven D, Least-squares fitting of two 3-D point sets, PAMI, 1987.

原文地址：

【泡泡机器人原创专栏-位姿求解】相机位姿问题的特征点法求解

（点开，进入微信，刷新下就可以看见了）

一起做RGBD-SLAM:(4)

一起做RGB-D SLAM (4) - 半闲居士 - 博客园