LSD SLAM 的tracking部分的代码解析

    首先还是放上LSD SLAM的整个代码框架，以至于读者可以清楚的知道tracking部分在整个SLAM框架中处于一个什么样的位置。

    LSD SLAM 的tracking部分主要做的工作就是解出keyframe与current frame之间的pose（R，t），已知keyframe上高梯度点的3D点，首先可以初始化一个pose（比如利用上一帧的pose作为这一帧的初始化的值），然后通过photometric error来构建一个最小二乘问题，利用优化的方式来优化出pose。

一、主函数调用tracking部分函数的代码：

如上图所示，初始化完成之后直接调用tracking部分中SE3Tracker这个类的成员函数进行track。

二、tracking部分的函数
    tracking部分主要有四个文件，分别是SE3Tracker.cpp，SE3Tracker.h，TrackingReference.cpp，TrackingReference.h。

    其中SE3Tracker是tracking部分最主要的一个类，其中的成员函数trackFrame()实现了一个参考帧和当前帧，以及一个初始的pose估计，输出一个优化好的pose。

具体的实现为：
1、首先调用trackingReference类中的makePointCloud()函数来计算参考帧有深度的点的3D坐标。这个函数主要记录了像素梯度，参考帧的点云个数。
2、然后调用calcResidualAndBuffers()函数来计算两张图对应点光度之间的残差。

这个函数主要输入参考帧上高梯度点的2d坐标和对应的3d点云，还有当前帧以及参考帧到当前帧的pose。首先是将参考帧上的点通过旋转矩阵rotMat 和平移向量transVec进行变换，然后计算出在当前帧下的u,v坐标，然后通过插值的方式找出u,v坐标对应的灰度值，计算出灰度的残差。最后把所以点的残差求个平均，输出一个平均的残差。

3、调用callOptimized()函数进行优化，求得最后一个优化好的SE3

参考文献：
[1] https://blog.csdn.net/lancelot_vim/article/details/51758870
[2] Engel, Jakob, Thomas Schöps, and Daniel Cremers. “LSD-SLAM: Large-scale direct monocular SLAM.” European    Conference on Computer Vision. Springer, Cham, 2014.
[3] https://github.com/tum-vision/lsd_slam