https://mp.weixin.qq.com/s/2i5RwW5HxpQVdqQ25AqN1w?
欢迎大家在周日来到泡泡机器人讲堂，本次我们将为大家介绍相机位姿问题的求解，相机位姿估计是指给定若干图像，估计其中相机运动的问题。求解方法通常分特征点法和直接法两种，这也是视觉里程计的两类基本方法。本次主要为大家讲解特征点法。
特征点法的思路，是先从图像当中提取许多特征，然后在图像间进行特征匹配，这样就得到许多匹配好的点，再根据这些点进行相机位姿的求解。相机位姿求解部分则和图像本身关系不大了。比方说下图是ORB特征匹配的结果。
特征匹配之后，你得到了一组配对点，以及它们的像素坐标。剩下的问题是说，怎么用这组配对点去计算相机的运动。这里，根据传感器形式的不同，分成三种情况：
-
你用的是单目相机，于是只有2D-2D的配对点；
-
你用的是RGBD或双目相机，于是你有3D-3D的配对点；
-
你只知道一张图中3D信息，另一张图只有2D信息，于是有3D-2D的配对点。
扫描二维码关注公众号，回复： 3893501 查看本文章
第三种情况可能出现在单目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.