LSD-SLAM解读——帧间追踪(详细推导)

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者：fishmarch

链接：https://zhuanlan.zhihu.com/p/47914361

本文转载自知乎，作者已授权，未经许可请勿二次转载。

直接法的帧间追踪就是用图像构建光度误差函数，优化位姿使误差最小。

LSD中因为是单目直接法，像素点的深度由深度滤波器给出，相比于RGBD等带有深度信息的直接法中，其深度误差较大且不同，因此构建误差函数时将该部分也考虑进来。

关于LSD的深度滤波，可以参考：

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

具体来说，LSD中的光度误差函数为：

这里的分子为对应点的光度差：

分母为对应方差：

也就是对每个像素点带来的光度误差，以深度不确定度做了加权求和。

误差函数分子就是基本的光度误差函数，我们重点看分母。分母中分为了两部分，第一部分是图像的光度方差，是一个与相机等有关的常数，因为有两帧图像，所以存在系数2。第二部分是深度相关方差，通过深度滤波器中得到的逆深度的方差传递获得。

我们简单来看下代码：

在代码中，这部分也比较容易理解，大致按照以下过程：

追踪是按图像金字塔逐层进行的，在每一层中重复上述优化过程。

点云生成

主要是在参考关键帧中找到有效追踪点，并获得这些点的参数信息

光度误差

将参考帧中的有效点投影到当前帧，若可观测到则计算光度误差等；并由误差定义了其权重，以此计算了仿射参数。

误差加权

这部分主要就是在计算权重：

代码中参数较多，乍一看很晕，并且该部分在论文中没有进一步推导。现在我们就来仔细看一下。

在整个式子中，比较麻烦的便是这个导数：

也就是光度误差对逆深度的导数。

其中为3D点到当前帧的投影函数。

上述方程中，我们将光度误差对逆深度的导数分类了两部分，第一部分是当前帧的像素梯度，第二部分是当前帧像素坐标对第帧中对应点的逆深度的导数，两者通过投影函数相关联。

我们再来看的具体表达：

在第帧中，根据相机模型我们知道：（其中 ,逆深度）

其中左侧是像素坐标，中间为相机内参数，记为，右侧是点在相机坐标系下的坐标。

两边乘上变换一下再乘上变换矩阵：

现在右侧为点在第帧中的坐标：

最右侧是在第帧中的像素坐标，我们记为

上述方程与论文中是不同的，论文中没有相机内参，作者把它包含到了像素坐标中，怪怪的，我在这就把它写出来了

所以最后我们获得：

其中（乘上相机参数后展开）：

所以现在求导：

现在我们可以来计算方程（1）了，中有两个方向，我们现在看一个方向：

第一项为方向像素梯度，

第二项：

后一项中的都与有关，需要分别求导：

我们往回看，方程（2）中有与的关系，再写到这里，同时我们把前面的常数矩阵：

因此对于：

其实如果计算的话，我们可以发现（2*）中的矩阵的第四列是 ,其中每个表示两帧的相对平移，所以：

其中就是逆深度，所以：

同理我们可得到：

我们把（12）,（13）代回到（10）：

同理我们可以获得：

方程(16),(17)代入到(9),(8)我们就获得了最终结果：

该结果是在源代码中对应的计算部分了。

定义好了误差函数后面就清楚了，非线性优化就可以了，程序中使用的是LM方法。

参考文献：

[1]Engel, J., Schöps, T., & Cremers, D. (2014). LSD-SLAM: Large-Scale Direct Monocular SLAM, 8690, 834–849.

推荐阅读：

吐血整理|3D视觉系统化学习路线

那些精贵的3D视觉系统学习资源总结（附书籍、网址与视频教程）

超全的3D视觉数据集汇总

大盘点|6D姿态估计算法汇总(上)

大盘点|6D姿态估计算法汇总（下）

机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划

汇总|3D点云目标检测算法

汇总|3D人脸重建算法

那些年，我们一起刷过的计算机视觉比赛

总结|深度学习实现缺陷检测

深度学习在3-D环境重建中的应用

汇总|医学图像分析领域论文

大盘点|OCR算法汇总

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

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会（ICRA/IROS/ROBIO/CVPR/ICCV/ECCV等）、顶刊（IJCV/TPAMI/TIP等）、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

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

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

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