LSD_SLAM代码笔记

Lsd_slam的主函数位于main_live_odometry.cpp中，其中主要的SLAM功能由slamNode.Loop();执行。

Slam.Loop()主要为一个读取新图像并处理的死循环，由newImageCallback处理图像。

newImageCallback执行slam的随机初始化和图像跟踪。

1 随机初始化

随机初始化函数为SlamSystem::randomInit

1.1 currentKeyFrame设置为当前图像的Frame

1.1.1 Frame类及其创建（构造函数）

Frame类中主要用来存储数据的类是Data类，Data类中保存了id、内参金字塔、图像金字塔、梯度图像金字塔、像素点深度金字塔、像素点逆深度方差金字塔等信息。

当用构造函数创建一个新的Frame类时，首先给data一个新的id，然后初始化Data类中的内参金字塔数据，然后对其他数据进行对应的初始化，再然后对图像金字塔赋值，不过只赋值金字塔的第0层。

Frame创建完后赋给currentKeyFrame。

1.2 DepthMap初始化

主要函数为DepthMap::initializeRandomly(Frame* new_frame)

首先复制了new_frame中的一些指针：互斥锁、new_frame、图像数据、和一个标志位activeKeyFrameIsReactivated。

计算图像梯度并且根据图像梯度给currentDepthMap赋初始值，并且将传进来的new_frame的深度设置为currentDepthMap中的值(Frame::setDepth)。

Frame::setDepth主要是对data.idepth[0]，和data.idepthVar进行了内参分配和赋值。

再把当前帧添加到keyFrameGraph中。

2 图像跟踪

首先根据新的图像创建一个Frame类trackingNewFrame，跟踪的主要算法位于

SE3 SE3Tracker::trackFrame(
		TrackingReference* reference,
		Frame* frame,
		const SE3& frameToReference_initialEstimate)

reference为最近的一个KeyFrame，frame是当前帧，frameToReference_initialEstimate是初始值。整个跟踪过程从最高层的图像金字塔（也就是图像尺寸最小的那一层）开始计算，一直往下迭代到第一层。
论文中采用了最小化归一化方差的光度误差：
$$E_p({\xi }_{ji})=\sum _{\text{p}\in {\Omega }_{D_i}}\Vert \frac{r_p^2(\text{p},{\xi }_{ji})}{{\sigma }_{r_p(\text{p},{\xi }_{ji})}^2}{\Vert }_{\delta }\text{\hspace{1em}(1)}$$
$${r}_p(\text{p},{\xi }_{ji})=I_i(\text{p})-I_j(\omega (\text{p},D_i(\text{p}),{\xi }_{ji}))\text{\hspace{1em}(2)}$$
$${\sigma }_{r_p(\text{p},{\xi }_{ji})}^2=2{\sigma }_I^2+(\frac{\partial r_p(\text{p},{\xi }_{ji})}{\partial D_i(\text{p})}{)}^2{V}_i(\text{p})\text{\hspace{1em}(3)}$$
对于每层图像金字塔，计算参考帧到当前帧的相对位姿，主要由以下步骤构成。

2.1 参考帧创建点云

reference->makePointCloud(lvl);

该函数主要用于计算参考帧中有效点在参考帧坐标系下的3D位置，参考帧有效点的像素值和方差，保存对应有效点的位置索引，有效点个数。这些参数是下一步计算残差需要的。

2.2 计算残差和梯度

float SE3Tracker::calcResidualAndBuffers(
		const Eigen::Vector3f* refPoint,
		const Eigen::Vector2f* refColVar,
		int* idxBuf,
		int refNum,
		Frame* frame,
		const Sophus::SE3f& referenceToFrame,
		int level,
		bool plotResidual)

计算残差时首先将参考帧坐标系下的3D点转换到当前帧坐标系，再通过差值计算点在当前帧的像素值和梯度。然后跟当前帧对应的像素作差得到残差，也就是公式(2)对应的残差。

每个残差都会有一个权重，按以下代码计算：

float weight = fabsf(residual) < 5.0f ? 1 : 5.0f / fabsf(residual);

计算梯度时把插值出来的梯度乘以当前层焦距。

该函数最终返回没有乘以权重的平均残差。

2.3 计算归一化残差

float SE3Tracker::calcWeightsAndResidual(
		const Sophus::SE3f& referenceToFrame)

该函数的功能是计算归一化方差的光度误差系数，也就是计算公式(3)，并且乘以了Huber-weight，最终把这个系数存在数组buf_weight_p中。 接下来我们看具体实现。可能是考虑参考帧到当前帧的位姿变换比较小，所以作者只考虑了位移t而忽略旋转R。这样使得式子(3)中的偏导的形式简单了很多。

忽略旋转后，路标点在参考帧坐标系下和当前帧坐标系下的坐标满足以下公式：
$$\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right)=\left(\begin{array}{c}{\text{p}}_x/d\\ {\text{p}}_y/d\\ 1/d\end{array}\right)+\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)\text{\hspace{1em}(4)}$$
整理一下得到归一化坐标：
$$\omega (\text{p},D_i(\text{p}),\xi )=\left(\begin{array}{c}\frac{{\text{p}}_x/d+t_x}{1/d+t_z}\\ \frac{{\text{p}}_y/d+t_y}{1/d+t_z}\\ 1\end{array}\right)\text{\hspace{1em}(5)}$$
然后就可以把(3)中的偏导表示出来：
(6) ∂ r p ( p , ξ j i ) ∂ D i ( p ) = ∂ ( I i ( p ) − I j ( ω ( p , D i ( p ) , ξ j i ) ) ) ∂ D i ( p ) = − ∂ I j ( a ) ∂ a ∣ a = p ⋅ ∂ ω ( d ) ∂ d ∣ d = D i ( p ) = − ( d x f x d y f y ) ⋅ ( t x ( 1 / d + t z ) − t z ( p x / d + t x ) ( 1 / d + t z ) 2 d t y ( 1 / d + t z ) − t z ( p y / d + t y ) ( 1 / d + t z ) 2 d ) = − ( d x f x t x z ′ − t z x ′ z ′ 2 d + d y f y t y z ′ − t z y ′ z ′ 2 d ) \frac{\partial r_p(\textbf p,\xi_{ji})}{\partial D_i(\textbf p)}= \frac{\par