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视觉系统学习资源总结（附书籍、网址与****）
机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划
重磅！3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会（ICRA/IROS/ROBIO/CVPR/ICCV/ECCV等）、顶刊（IJCV/TPAMI/TIP等）、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号