ORB-SLAM3中的3d-2d匹配

作者丨卢涛@知乎

编辑丨3D视觉工坊

非完整版注释：https://github.com/smilefacehh/ORB-SLAM3-Note

3d-2d匹配解决的问题是，已知世界坐标系下的3D点，与之对应的像素坐标系下的2d点，如何求解相机位姿R、t。引出PnP（Perspective-n-Point）,与2d-2d匹配不同的是，它的尺度已知，通常用于前端跟踪模块，2d-2d匹配用于系统初始化。

本文介绍PnP的DLT（Direct Linear Transform，直接线性变换）解法，SVD求解Ax=0的证明，EPnP算法，以及PnP-BA解法。如果有理解上的错误，请您指正。

PnP-DLT解法

记世界坐标系3D点为  ，在当前帧下投影的2d像素点为  ，当前帧位姿记为  ，根据投影关系

其中  为深度值，将  当做整体，一共12个参数，展开矩阵形式

化简运算一下

再将第三行代入前两行，消掉s，整理之后得到

然后写成  的形式，  就是我们要求的参数

每对3d-2d点可以提供2个等式，一共12个参数，超过6对点，就可以计算  了。在  约束下，矩阵A奇异值分解，右奇异矩阵  的最后一列（如果是  就取最后一行）近似等于解  ，后面会证明。

这个右奇异矩阵实际是  的特征向量矩阵，如下

好了，我们通过对  奇异值分解，得到了  约束下方程的解。当然，乘上任意一个系数  依然满足  ，所以上面得到的这个  还要再看看有没有什么约束，可以最终确定下来。我们知道旋转矩阵有个性质  ，正交且行列式为1，这就好办了，把  对应的  部分，近似成一个正交矩阵，行列式化为1就可以了。然后算出来变换需要的系数，  对应的  部分也乘上这个系数就ok了。

记上面得到的  ，我们要找到  的近似正交矩阵，还是SVD分解

要让  近似变成单位矩阵 ，这样就得到的近似正交矩阵了

其中 都是正交矩阵，相乘也是正交矩阵，系数  ，那么

系数正负号通过计算点的投影到相机坐标系下的深度值是否为正来确定

SVD求解Ax=0的证明

命题：  的（近似）解是矩阵  最小奇异值对应的右奇异向量。

由于  实际中不为0（为0没啥用），导致  不为0，只能让尽可能接近0，此种情况下得到一个。那么目标变成一个最小二乘问题

对矩阵  进行SVD分解

计算一下 

其中  依次从大到小，  为  对应的列向量，相互正交，用这组正交向基来表示一下 

再来计算  ，要用上正交基的性质，  ，化简之后就是这样

前面有  的限制，那么  ，想要上面这个式子值最小，令  即可，因为  是最小的，这时候就得到了最终解

 的（近似）解是矩阵  最小奇异值对应的右奇异向量。

EPnP算法

这里葡萄同学有篇介绍，写的很清晰明了，不再重写了。

[PnP]PnP问题之EPnP解法：https://zhuanlan.zhihu.com/p/59070440

PnP-BA解法

还是下面这个投影公式，记世界坐标系3D点为  ，在当前帧下投影的2d像素点为  ，深度值为，当前帧位姿记为 

BA要做的事情是最小化重投影像素误差（可参考《视觉SLAM十四讲》，P162），如下

这是一个典型的非线性最小二乘优化问题

我们没办法一次计算出最优  ，可行的策略是给一个初始值，不断迭代  ，期望每次可以使上面式子的值变小，收敛的时候，就得到最终的结果。问题变成了每次  选多大，可以让式子的值逐渐变小。牛顿告诉我们沿着梯度方向减去一个步长，最终会到达最小值的地方（梯度为0，局部最小值）。（下面的公式中向量没有加粗，见谅）

说说常用的高斯牛顿法，对  在当前  处一阶泰勒展开，  (Jacobian) 是一阶导数

计算损失函数

我们现在希望上面这个式子最小，注意现在是在求最优的  ，是自变量。梯度为0的时候，式子值最小。上面式子对求一阶导数，令导数=0，得到

这就是熟悉的正规方程（normal equation），这样对于每一步的  ，都按照这个式子计算 ，然后迭代更新 直到误差收敛为止。

现在问题转化成了怎么计算雅克比  ，雅克比其实就是一阶导数，还有熟知的Hessian矩阵，是二阶导数。  是残差函数  对变量  的导数。在PnP问题中，变量  就是位姿  ，残差函数  就是重投影像素误差。

其中  为对应的相机坐标系点，关系为

根据链式法则计算  ，其中  是R、t李代数上的表示

前半部分

后半部分（参考李代数求导）

综合起来

再总结一下流程

给定3d-2d匹配点，构建最小化重投影像素误差问题 ，采用高斯牛顿法迭代优化

构造雅克比矩阵 

根据正规方程  ，计算 

更新  ，并计算误差，重复上面步骤

当足够小或者误差收敛，结束，得到最终 

参考：

1.小葡萄：[PnP] PnP问题之DLT解法：https://zhuanlan.zhihu.com/p/58648937

2.小葡萄：[PnP]PnP问题之EPnP解法：

​​https://zhuanlan.zhihu.com/p/59070440​​

3.如何理解PnP问题的DLT解法以及Python编程实践_@司南牧|知乎|博客|易懂教程|李韬

​​https://zhuanlan.zhihu.com/p/355848913​​

4.解 Ax = 0 - nowgood -

5.漫漫成长：奇异值分解（SVD）：

​​https://zhuanlan.zhihu.com/p/29846048​​

6.SVD之最小二乘【推导与证明】 - Brad_Lucas -

7.高翔博士《视觉SLAM十四讲 》

本文仅做学术分享，如有侵权，请联系删文。

▲长按关注公众号