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

作者丨卢涛@知乎

来源丨https://zhuanlan.zhihu.com/p/355848913

编辑丨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编程实践_@司南牧|知乎|博客|易懂教程|李韬-CSDN博客：

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

4.解 Ax = 0 - nowgood - 博客园：https://www.cnblogs.com/cmt/p/14553189.html

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

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

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

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

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

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近3000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~