LSD-SLAM笔记之一致性约束

开源SLAM笔记

建图一致性约束也就是做闭环检测和全局优化。由于论文中采用的是直接法，虽然代码中有fabmap检测闭环的部分，但是其默认检测闭环的方式是帧与帧之间做双向跟踪。

这部分的代码入口在函数SlamSystem::constraintSearchThreadLoop，代码主要分了两种情况：

1. 新关键帧队列为空，则在所有关键帧中随机选取测试闭环
2. 新关键帧队列不为空，则取最早的新关键帧测试闭环

在测试闭环时调用的函数为SlamSystem::findConstraintsForNewKeyFrames。该函数主要是根据视差、关键帧连接关系，找出并且在删选处候选帧，然后对每个候选帧和测试的关键帧之间进行双向sim3跟踪，如果求解出的两个李代数满足马氏距离在一定范围内，则认为是闭环成功，并且在位姿图中添加边的约束。而图优化的部分在另外一个线程SlamSystem::optimizationThreadLoop，基本就是使用g2o，这部分就略过了。

关于如何选择候选帧，主要步骤如下：

• 视角和距离判别
• SE3跟踪检测
• Sim3跟踪检测

其实第一个步骤是根据每个关键帧求得的位姿取判断的，其实这是一个因果倒置的方式。在代码中把候选帧分为近处的候选帧和远处的候选帧，对于第二个步骤，只是检测处与当前闭环检测关键帧相近的候选帧。第三步则是核心，把删选处的每一个候选帧与检测的关键帧做双向的Sim3跟踪，如果都成功，并且两个$\mathfrak{sim}(3)$的马氏距离足够小，则认为是检测处闭环，并且在位姿图中构建$\mathfrak{sim}(3)$的边。

接下来主要看一下Sim(3)求解以及双向跟踪检测（reciprocal tracking check）。

1. Sim3求解

1.1 原理分析

1.1.1 Sim3图像对齐

首先和$\mathfrak{se}(3)$变换一样，对于两帧图像上对应归一化图像点$\mathbf{p}'$和$\mathbf{p}$通过$\mathfrak{sim}(3)$变换有：

$$\begin{equation} \underbrace{\mathbf{p}' = \begin{pmatrix}x'/z'\\y'/z'\\1\end{pmatrix}}_{\omega_n} \quad\text{with}\quad \underbrace{\begin{pmatrix}x'\\y'\\z'\\1\end{pmatrix} := \text{exp}_{\mathfrak{sim}(3)}(\mathbf\xi)\begin{pmatrix}\mathbf{p}_x/d\\\mathbf{p}_y/d\\1/d\\1\end{pmatrix}}_{w_s} \label{eq:proj} \end{equation}$$
然后我们看代价函数，$\eqref{eq:proj}$代价函数和SE3相比多了一项尺度项，并且依旧使用归一化方差：
$$\begin{equation} E(\mathbf\xi_{ji}) =\sum_{\mathbf{p}\in\Omega_{D_i}}\Biggl\| \frac{r_p^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2} + \frac{r_d^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2} \Biggr\|_\delta \label{eq:sim3err} \end{equation}$$
这里的光度残差和方差的定义和SE3跟踪是一样的:
$$\begin{align} r_p(\mathbf{p},{\mathbf\xi_{ji}}) :&= I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji}))\\ \sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2 :&= 2\sigma_I^2 + (\frac{\partial{r_p(\mathbf{p}, \mathbf\xi_{ji})}}{\partial{D_i(\mathbf{p})}})^2V_i(\mathbf{p})\label{eq:varrp} \end{align}$$
以及深度残差和方差定义如下：
$$\begin{align} r_d(\mathbf{p},\mathbf\xi_{ji}) &= [\mathbf{p}']_3-D_j([\mathbf{p}']_{1,2}) \label{eq:rd}\\ \sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2 &= V_j([\mathbf{p}']_{1,2})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_j([\mathbf{p}']_{1,2})}\end{pmatrix} + V_i(\mathbf{p})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_i(\mathbf{p})}\end{pmatrix}\label{eq:varrd} \end{align}$$

这里的$\mathbf{p}':=\omega_s(\mathbf{p},D_i(\mathbf{p}),\mathbf\xi_{ji})$表示从图像帧$j$变换到图像帧$i$上的点。这里在求解$\mathfrak{sim}(3)$的时候也是同求$\mathfrak{se}(3)$一样，使用了迭代变权重高斯牛顿算法。

注意：论文中提及，把深度图的梯度近似为$0$，因此对$D_j(\mathbf{u})$求的偏导都可以忽略。

根据高斯牛顿算法，对$\eqref{eq:sim3err}$式子进行处理。由于Huber-Norm是在最外面的，其最终是等效为一个权重$\omega_h$。

$$\delta\mathbf{\xi}^{*} = \text{arg}\min_{\delta\mathbf{\xi}}\sum_{\mathbf{p}\in\Omega_{D_i}}\left( \frac{\omega_h}{\sigma_{r_p(\mathbf{p},\mathbf\xi^{(n)})}^2} \left( r_p(\mathbf{p},\mathbf\xi^{(n)})+\mathbf{J}_p(\xi^{(n)})\delta\mathbf{\xi}\right)^2 + \frac{\omega_h}{\sigma_{r_d(\mathbf{p},\mathbf\xi^{(n)})}^2}\left(r_d(\mathbf{p},\mathbf\xi^{(n)}+\mathbf{J}_d(\xi^{(n)})\delta\xi)\right)^2 \right)$$
对$\delta\xi$求偏导，并且令式子为$0$，可得
$$\begin{equation} \underbrace{\left(\frac{\omega_h}{\sigma_{r_p}^2}\mathbf{J}_p^T\mathbf{J}_p + \frac{\omega_h}{\sigma_{r_d}^2}\mathbf{J}_d^T\mathbf{J}_d\right)}_{\mathbf A}\delta\xi = \underbrace{-\left(\frac{\omega_h r_p}{\sigma_{r_p}^2}\mathbf{J}_p + \frac{\omega_h r_p}{\sigma_{r_d}^2}\mathbf{J}_d\right)}_{\mathbf b} \end{equation} \label{eq:ax=b}$$
这就是最终求的$\mathbf A\mathbf x=\mathbf b$的形式。

1.1.2 归一化方差

我们需要计算每个位置处的归一化方差。对于已知的向量$\mathbf x$，其每一个元素$x_i\sim N(u_i,\sigma_i^2)$，对于函数$f(\mathbf x)$的方差有：

$$\begin{equation} \mathbf{\Sigma}_f = \mathbf{J}_f \mathbf{\Sigma}_{\mathbf x} \mathbf{J}_f^T \label{eq:varprop} \end{equation}$$
根据上式我们可以求得式子$\eqref{eq:varrd}$。对于在图像$j$上的逆深度，直接可以在深度图$D_j$上获取，因此$D_j([\mathbf{p}']_{1,2})$的方差直接是深度图对应位置逆深度的方差。剩下我们只需要看$[\mathbf{p}']_3$处的方差。我们有
$$[\mathbf{p}']_3=1/z'=D(\omega_s({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji}))$$
代码在实现上和SE3跟踪一样，忽略旋转使用了近似：
$$\omega_s({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji}) = \mathbf{p}/d + \mathbf{t}$$
从而有
$$[\mathbf{p}']_3 = \frac{1}{\frac{1}{d}+\mathbf{t}_z} = \frac{d}{1+d\cdot\mathbf{t_z}}$$
对其求偏导可得
$$\begin{equation} \mathbf{J} = \frac{\partial[\mathbf{p}']_3}{d} = \frac{(1/d+\mathbf{t}_z)-\mathbf{t}_z}{(1/d+\mathbf{t}_z)^2\cdot d} := \frac{z'-\mathbf{t}_z}{z'^2\cdot d} \end{equation} \label{eq:varrdjac}$$
求得雅可比后，就可以求得对应的方差。

疑问1：这里和之前求$\eqref{eq:varrp}$一样，都是近似可以忽略旋转的情况下计算的。在求得雅可比之后，变量尽量都用$\mathbf p' = (x',y',z')^T$的数据来表示，而不是用参考帧上的数据来表示呢？就以$\eqref{eq:varrdjac}$为例，完全可以化作$\frac{1}{(1/d+\mathbf t_z)^2d^2}$的形式。这两者有什么优劣差别？

1.1.3 残差雅可比

在Sim3中比SE3多了一个逆深度的雅克比。首先我们看一下光度残差的雅克比，这里和SE3部分唯一不同的地方在连式法则展开后最后一项变换模型有所差异，一个是对$\mathfrak{se}(3)$求偏导，一个是对$\mathfrak{sim}(3)$求偏导。

$$\begin{equation} \begin{split} \mathbf{J}_p &= \frac{\partial{r_i(\epsilon\circ\mathbf{\xi}^{(n)})}}{\partial\epsilon}\bigg|_{\epsilon=0}\\ &= -\frac{\partial{I(\mathbf{b})}}{\partial{\mathbf{b}}}\bigg|_{\mathbf{b}=\mathbf{p'}_i} \cdot \frac{\partial{\omega_n(\mathbf{q})}}{\partial{\mathbf{q}}}\bigg|_{\mathbf{q}=\mathbf{p'}_i{\cdot}z_i'} \cdot \frac{\partial{\omega_s(\epsilon\circ\mathbf{\xi}^{(n)})}}{\partial{\epsilon}}\bigg|_{\epsilon=0}\\ &= -\begin{pmatrix}dxfx&dyfy\end{pmatrix}\cdot \begin{pmatrix}1/z'&0&-x'/z'^2 \\ 0&1/z'&-y'/z'^2 \end{pmatrix} \cdot \left(\begin{array}{c|c|c}I & -[\mathbf{p}_i'{\cdot}z_i']_\times & \mathbf{p}_i'{\cdot}z_i'\end{array}\right)\\ &= -\begin{pmatrix}1/z'{\cdot}dxfx\\1/z'{\cdot}dyfy\\-x'/z'^2{\cdot}dxfx-y'/z'^2{\cdot}dyfy\end{pmatrix}^{T} \cdot \left(\begin{array}{ccc|ccc|c}1&0&0&0&z'&-y'&x'\\0&1&0&-z'&0&x'&y'\\0&0&1&y'&-x'&0&z'\end{array}\right)\\ &=-\begin{pmatrix}1/z'{\cdot}dxfx \\ 1/z'{\cdot}dyfy \\ -x'/z'^2{\cdot}dxfx-y'/z'^2{\cdot}dyfy \\ -x'y'/z'^2{\cdot}dxfx-(z'^2+y'^2)/z'^2{\cdot}dyfy \\ (1+x'^2/z'^2)dxfx+x'y'/z'^2dyfy \\ x'/z'{\cdot}dyfy-y'/z'{\cdot}dxfx\\ 0\end{pmatrix}^T \end{split} \label{eq:respjac} \end{equation}$$
我们可以看到最后一项是0，也就是说忽略最后一个0，对$\mathfrak{se}(3)$求的雅克比和对$\mathfrak{sim}(3)$求的雅克比是一样的。

然后我们看对逆深度$r_d(\mathbf{p},\mathbf\xi_{ji})$求的雅可比矩阵，而这雅可比矩阵和$\eqref{eq:rd}$式子中的两个逆深度都有关。这里的雅可比是在Sim3李代数$\xi_{ji}$的扰动求的：

$$\mathbf J_d = \frac{r_d(\mathbf{p},\mathbf\epsilon\circ\xi)}{\partial\mathbf\epsilon} ＝ \frac{\partial[\mathbf{p}']_3}{\partial\mathbf\epsilon} - \frac{D_j([\mathbf{p}']_{1,2})}{\partial\mathbf\epsilon}$$
我们先看第二个雅可比
$$\frac{\partial D_j([\mathbf{p}']_{1,2})}{\partial\mathbf\epsilon} = \frac{\partial D_j(\mathbf{a})}{\partial\mathbf{a}}\bigg|_{\mathbf{a}=\mathbf{u}'} \cdot \frac{\partial\omega_n(\mathbf{b})}{\partial\mathbf{b}}\bigg|_{\mathbf{b}=\mathbf{p}'\cdot z'} \cdot \frac{\partial{{\omega_s(\mathbf\epsilon\circ\mathbf\xi)}}}{\partial\mathbf\epsilon}\bigg|_{\mathbf\epsilon=0}$$
由于深度图的梯度近似为$0$，因此该雅可比的第一项就是$0$，从而这个雅可比就可以略去。因此有：
$$\begin{equation} \begin{split} \mathbf{J}_{d} &= \frac{\partial[\mathbf{p}']_3}{\partial\mathbf\epsilon}=\frac{\partial{{d(\mathbf{b})}}}{\partial\mathbf{b}}\bigg|_{\mathbf{b}=\mathbf{p'}{\cdot}z'}\cdot\frac{\partial{{\omega_s(\mathbf\epsilon\circ\mathbf\xi)}}}{\partial\mathbf\epsilon}\bigg|_{\mathbf\epsilon=0}\\ &= \begin{pmatrix}0&0&1/{z'}^2\end{pmatrix}\cdot\left(\begin{array}{c|c|c}I&-(\mathbf{p'}{\cdot}z')^\land&\mathbf{p'}{\cdot}z'\end{array}\right)\\ &= \begin{pmatrix}0&0&1/{z'}^2\end{pmatrix}\cdot\left(\begin{array}{ccc|ccc|c}1&0&0& 0&z'&-y'& x'\\ 0&1&0& -z'&0&x'& y'\\ 0&0&1& y'&-x'&0 &z'\end{array}\right)\\ &=\begin{pmatrix}0&0&1/{z'}^2&y'/{z'}^2&-x'/{z'}^2&0&1/z'\end{pmatrix} \end{split} \label{eq:resdjac} \end{equation}$$
这里的雅可比的第一项是逆深度对3D点求偏导，第二项是$\mathbf{p}'＝\omega_s(\mathbf{p},D_i(\mathbf{p}),\mathbf\xi_{ji})$对在$\mathfrak{sim}(3)$上扰动的偏导。

1.1.4 ESM

由于图像是非凸的，因此对图像对齐问题，我们需要一个很好的初始化位置。但是对于查找闭环约束的时候，这个条件不容易满足，因此论文中提出了两种增加收敛半径的方法：使用高效二阶最小化方法（Efficient Second Order Minimization，ESM）；从粗到细（Coarse-to-Fine）方法，也就是图像金字塔的方式。第二种方法是比较常见的，对于第一种方法之前没有碰到过ESM方法，这里将对ESM方法做个简明的介绍。

我们设代价函数为$f(\mathbf x)$，做二阶泰勒展开如下：

$$\begin{equation} f(\mathbf\delta) \approx f(\mathbf 0) + \mathbf J(\mathbf 0)\mathbf\delta + \frac{1}{2}\mathbf\delta^T\mathbf{H}(\mathbf 0)\mathbf\delta \end{equation}$$
这里的$\mathbf H$是代价函数的Hession矩阵。再将雅可比做一阶展开：
$$\begin{equation} \mathbf J(\mathbf\delta) \approx \mathbf J(\mathbf 0) + \mathbf H(\mathbf 0) \mathbf\delta \end{equation}$$
把$(10)$带入$(9)$中消去$\mathbf H(\mathbf 0)$得：
$$\begin{equation} \begin{split} f(\mathbf\delta) &\approx f(\mathbf 0) + \mathbf J(\mathbf 0)\mathbf \delta + \frac{1}{2}(\mathbf J(\mathbf\delta) - \mathbf J(\mathbf 0))\mathbf\delta \\ &= f(\mathbf 0) + \frac{1}{2}(\mathbf J(\mathbf\delta) + \mathbf J(\mathbf 0))\mathbf\delta \end{split} \end{equation}$$
对于最小化残差作为代价函数的情况，有
$$\begin{equation} \mathbf J_{\text{esm}} =　\frac{1}{2}\left(\frac{\partial I(\mathbf u)}{\partial\mathbf u}\bigg|_{\mathbf u = \omega(\mathbf p,\mathbf X)} + \frac{\partial I^*(\mathbf u)}{\partial\mathbf u}\bigg|_{\mathbf u = \omega(\mathbf 0,\mathbf X)}\right) \frac{\partial\omega(\mathbf x)}{\partial\mathbf x}\bigg|_{\mathbf x = \mathbf 0} \label{eq:esmjac} \end{equation}$$
我们可以看出，ESM的雅可比是混合了两幅图像的梯度。

代码中其他步骤基本都和SE3跟踪相同。对于ESM的详细介绍见论文Real-time image-based tracking of planes using efficient second-order minimization。

1.2 代码剖析

Sim3有几个主要的函数，基本和SE3跟踪是一样的。接下里看一下这些代码。

1.2.1 Sim3Tracker::trackFrameSim3

这是Sim3跟踪的主体函数，跟踪图像帧reference到frame求相应的Sim3变换。

Sim3 Sim3Tracker::trackFrameSim3(
        TrackingReference* reference,
        Frame* frame,
        const Sim3& frameToReference_initialEstimate,
        int startLevel, int finalLevel)

由于主要的框架和SE3都是一样的，我们只看一下具体实现的那几个函数。

1.2.2 Sim3Tracker::calcSim3Buffers

我们看下代码，首先得到一些全局不变量，这里的rotMat对应初始Sim3中的$sR$，rotMatUnscaled为$R$，transVec是$\mathbf t$。

    // get static values
    int w = frame->width(level);
    int h = frame->height(level);
    Eigen::Matrix3f KLvl = frame->K(level);
    float fx_l = KLvl(0,0);
    float fy_l = KLvl(1,1);
    float cx_l = KLvl(0,2);
    float cy_l = KLvl(1,2);
    Eigen::Matrix3f rotMat = referenceToFrame.rxso3().matrix().cast<float>();
    Eigen::Matrix3f rotMatUnscaled = referenceToFrame.rotationMatrix().cast<float>();
    Eigen::Vector3f transVec = referenceToFrame.translation().cast<float>();

接下来计算了图像平面沿着光轴方向的旋转。我们可以把两帧的相对旋转分为主轴的旋转以及沿着主轴方向的旋转。

    // Calculate rotation around optical axis for rotating source frame gradients
    Eigen::Vector3f forwardVector(0, 0, -1);
    Eigen::Vector3f rotatedForwardVector = rotMatUnscaled * forwardVector;
    Eigen::Quaternionf shortestBackRotation;
    shortestBackRotation.setFromTwoVectors(rotatedForwardVector, forwardVector);
    Eigen::Matrix3f rollMat = shortestBackRotation.toRotationMatrix() * rotMatUnscaled;
    float xRoll0 = rollMat(0, 0);
    float xRoll1 = rollMat(0, 1);
    float yRoll0 = rollMat(1, 0);
    float yRoll1 = rollMat(1, 1);

这里的setFromTwoVectors函数，计算得到从rotatedForwardVector到forwardVector的旋转，然后乘以rotMatUnscaled后就是图像平面沿着主轴方向forwardVector的旋转。这里主要是在ESM求梯度时使用的。

在遍历前获取参考帧的3D点、灰度、梯度、图像帧的逆深度、逆深度方差和灰度、梯度的指针。

    const Eigen::Vector3f* refPoint_max = reference->posData[level] + reference->numData[level];
    const Eigen::Vector3f* refPoint = reference->posData[level];
    const Eigen::Vector2f* refColVar = reference->colorAndVarData[level];
    const Eigen::Vector2f* refGrad = reference->gradData[level];
    const float*            frame_idepth = frame->idepth(level);
    const float*            frame_idepthVar = frame->idepthVar(level);
    const Eigen::Vector4f*  frame_intensityAndGradients = frame->gradients(level);

然后遍历每个参考帧的3D点，把点通过初始的Sim3变换到图像帧的坐标系下，并且查看对应投影点是不是在图像平面范围内。

    for(;refPoint<refPoint_max; refPoint++, refGrad++, refColVar++)
    {
        Eigen::Vector3f Wxp = rotMat * (*refPoint) + transVec;
        float u_new = (Wxp[0]/Wxp[2])*fx_l + cx_l;
        float v_new = (Wxp[1]/Wxp[2])*fy_l + cy_l;
        // step 1a: coordinates have to be in image:
        // (inverse test to exclude NANs)
        if(!(u_new > 1 && v_new > 1 && u_new < w-2 && v_new < h-2))
            continue;

把数据保存到缓存变量中。如果使用了ESM，则把参考帧上的梯度方向旋转到当前帧的方向。并且这里的的梯度就是按照公式$\eqref{eq:esmjac}$中的形式计算。

        *(buf_warped_x+idx) = Wxp(0);
        *(buf_warped_y+idx) = Wxp(1);
        *(buf_warped_z+idx) = Wxp(2);
        Eigen::Vector3f resInterp = getInterpolatedElement43(frame_intensityAndGradients, u_new, v_new, w);
        // save values
#if USE_ESM_TRACKING == 1
        // get rotated gradient of point
        float rotatedGradX = xRoll0 * (*refGrad)[0] + xRoll1 * (*refGrad)[1];
        float rotatedGradY = yRoll0 * (*refGrad)[0] + yRoll1 * (*refGrad)[1];
        *(buf_warped_dx+idx) = fx_l * 0.5f * (resInterp[0] + rotatedGradX);
        *(buf_warped_dy+idx) = fy_l * 0.5f * (resInterp[1] + rotatedGradY);
#else
        *(buf_warped_dx+idx) = fx_l * resInterp[0];
        *(buf_warped_dy+idx) = fy_l * resInterp[1];
#endif

疑问2：在ESM求雅克比的时候，对参考帧像素位置求的雅克比得到的梯度为什么需要旋转到图像帧的方向上来？

这里和SE3一样，在计算残差的时候也是使用了迷之仿射变换，先不管这个问题。

        float c1 = affineEstimation_a * (*refColVar)[0] + affineEstimation_b;
        float c2 = resInterp[2];
        float residual_p = c1 - c2;
        float weight = fabsf(residual_p) < 2.0f ? 1 : 2.0f / fabsf(residual_p);
        sxx += c1*c1*weight;
        syy += c2*c2*weight;
        sx += c1*weight;
        sy += c2*weight;
        sw += weight;
        *(buf_warped_residual+idx) = residual_p;
        *(buf_idepthVar+idx) = (*refColVar)[1];

接下来的代码是Sim3特有的部分，计算逆深度的残差，如果当前图像帧对应位置的逆深度方差不为零（对应位置若为-1则表示没有深度），则在缓存变量中赋值。

        // new (only for Sim3):
        int idx_rounded = (int)(u_new+0.5f) + w*(int)(v_new+0.5f);
        float var_frameDepth = frame_idepthVar[idx_rounded];
        float ref_idepth = 1.0f / Wxp[2];
        *(buf_d+idx) = 1.0f / (*refPoint)[2];
        if(var_frameDepth > 0)
        {
            float residual_d = ref_idepth - frame_idepth[idx_rounded];
            *(buf_residual_d+idx) = residual_d;
            *(buf_warped_idepthVar+idx) = var_frameDepth;
        }
        else
        {
            *(buf_residual_d+idx) = -1;
            *(buf_warped_idepthVar+idx) = -1;
        }

然后这个函数主要的功能都完成了。与SE3Tracker中的calcResidualAndBuffers相对应，差别在于计算梯度的时候按照ESM求的，并且多了两个变量：逆深度残差buf_residual_d和当前图像帧对应逆深度的方差buf_warped_idepthVar。

1.2.3 Sim3Tracker::calcSim3WeightsAndResidual

这个函数主要计算对应的归一化方差以及最终的残差。这里和SE3Tracker中的calcWeightsAndResidual对应，都有旋转很小的假设，认为只有平移。

    float tx = referenceToFrame.translation()[0];
    float ty = referenceToFrame.translation()[1];
    float tz = referenceToFrame.translation()[2];

接下来遍历每个点，得到buff中对应的数据。

    for(int i=0;i<buf_warped_size;i++)
    {
        float px = *(buf_warped_x+i);   // x'
        float py = *(buf_warped_y+i);   // y'
        float pz = *(buf_warped_z+i);   // z'
        float d = *(buf_d+i);   // d
        float rp = *(buf_warped_residual+i); // r_p
        float rd = *(buf_residual_d+i);  // r_d
        float gx = *(buf_warped_dx+i);  // \delta_x I
        float gy = *(buf_warped_dy+i);  // \delta_y I
        float s = settings.var_weight * *(buf_idepthVar+i); // \sigma_d^2
        float sv = settings.var_weight * *(buf_warped_idepthVar+i); // \sigma_d^2'

接下来计算归一化方差，这里比SE3多了g2。通过公式$\eqref{eq:varrdjac}$计算得到了雅克比，则对应的方差w_d计算如下

$$\sigma_d^2 = \sigma_1^2 + \sigma_2^2 = \sigma_{D_j}^2 + \mathbf{J}^T\sigma_{D_i}^2 \mathbf{J} = \sigma_{D_j}^2 ＋ \frac{(z'-\mathbf{t}_z)^2}{(z'^2\cdot d)^2}\sigma_{D_i}^2$$

        // calc dw/dd (first 2 components):
        float g0 = (tx * pz - tz * px) / (pz*pz*d);
        float g1 = (ty * pz - tz * py) / (pz*pz*d);
        float g2 = (pz - tz) / (pz*pz*d);
        // calc w_p
        float drpdd = gx * g0 + gy * g1;    // ommitting the minus
        float w_p = 1.0f / (cameraPixelNoise2 + s * drpdd * drpdd);
        float w_d = 1.0f / (sv + g2*g2*s);

然后计算对应的Huber权重

        float weighted_rd = fabs(rd*sqrtf(w_d));
        float weighted_rp = fabs(rp*sqrtf(w_p));
        float weighted_abs_res = sv > 0 ? weighted_rd+weighted_rp : weighted_rp;
        float wh = fabs(weighted_abs_res < settings.huber_d ? 1 : settings.huber_d / weighted_abs_res);

最后把保存光度误差的权重和逆深度误差的权重。

        *(buf_weight_p+i) = wh * w_p;
        if(sv > 0)
            *(buf_weight_d+i) = wh * w_d;
        else
            *(buf_weight_d+i) = 0;

1.2.4 Sim3Tracker::calcSim3LGS

这个函数求高斯牛顿迭代的雅克比了。函数传进来的是一个7个参数的最小二乘的类。这个函数与的SE3Tracker的函数calculateWarpUpdate对应，比SE3多了一个参数，就是尺度。在函数开始的时候，初始化了两个最小二乘的类，一个是4个参数的，一个是6个参数的。6个参数的对应于光度残差的雅克比，4参数对应逆深度残差的雅克比，这里都不是7，因为都先不保存非零项。

void Sim3Tracker::calcSim3LGS(NormalEquationsLeastSquares7 &ls7)
{
    NormalEquationsLeastSquares4 ls4;
    NormalEquationsLeastSquares ls6;
    ls6.initialize(width*height);
    ls4.initialize(width*height);

然后看循环，遍历每个点

    for(int i=0;i<buf_warped_size;i++)
    {
        float px = *(buf_warped_x+i);   // x'
        float py = *(buf_warped_y+i);   // y'
        float pz = *(buf_warped_z+i);   // z'
        float wp = *(buf_weight_p+i);   // wr/wp
        float wd = *(buf_weight_d+i);   // wr/wd
        float rp = *(buf_warped_residual+i); // r_p
        float rd = *(buf_residual_d+i);  // r_d
        float gx = *(buf_warped_dx+i);  // \delta_x I
        float gy = *(buf_warped_dy+i);  // \delta_y I
        float z = 1.0f / pz;
        float z_sqr = 1.0f / (pz*pz);

先计算光度残差的雅克比，这里和公式$\eqref{eq:respjac}$前6项对应，且差一个负号。

        Vector6 v;
        Vector4 v4;
        v[0] = z*gx + 0;
        v[1] = 0 +         z*gy;
        v[2] = (-px * z_sqr) * gx +
              (-py * z_sqr) * gy;
        v[3] = (-px * py * z_sqr) * gx +
              (-(1.0 + py * py * z_sqr)) * gy;
        v[4] = (1.0 + px * px * z_sqr) * gx +
              (px * py * z_sqr) * gy;
        v[5] = (-py * z) * gx +
              (px * z) * gy;

然后是逆深度残差的雅克比，回顾一下公式$\eqref{eq:resdjac}$：

$$\mathbf J_d = \begin{pmatrix}0&0&1/{z'}^2&y'/{z'}^2&-x'/{z'}^2&0&1/z'\end{pmatrix}$$
下面这部分代码和公式中的第2、3、4、6位置对应（从0开始）。

        v4[0] = z_sqr;
        v4[1] = z_sqr * py;
        v4[2] = -z_sqr * px;
        v4[3] = z;

然后分别求对应的$\mathbf A$和$\mathbf b$。我们先看一下$\eqref{eq:ax=b}$:

$$\underbrace{\left(\frac{\omega_h}{\sigma_{r_p}^2}\mathbf{J}_p^T\mathbf{J}_p + \frac{\omega_h}{\sigma_{r_d}^2}\mathbf{J}_d^T\mathbf{J}_d\right)}_{\mathbf A}\delta\xi = \underbrace{-\left(\frac{\omega_h r_p}{\sigma_{r_p}^2}\mathbf{J}_p + \frac{\omega_h r_p}{\sigma_{r_d}^2}\mathbf{J}_d\right)}_{\mathbf b}$$
这里是先求l6，这里的雅克比差了一个负号:

        ls6.update(v, rp, wp);      // Jac = - v

对应调用了NormalEquationsLeastSquares4::update：

inline void NormalEquationsLeastSquares::update(const Vector6& J, const float& res, const float& weight)
{
  A_opt.rankUpdate(J, weight);
  b -= J * (res * weight);
  error += res * res * weight;
  num_constraints += 1;
}

很明显，这里求的时候$\mathbf b$的符号已经反过来了。
然后是求l4:

        ls4.update(v4, rd, wd); // Jac = v4

对应了NormalEquationsLeastSquares4::update：

inline void NormalEquationsLeastSquares4::update(const Vector4& J, const float& res, const float& weight)
{
    // TODO: SSE optimization
  A.noalias() += J * J.transpose() * weight;
  b.noalias() -= J * (res * weight);
  error += res * res * weight;
  num_constraints += 1;
}

函数的最后计算7参数的最小二乘，这里调用的finish是没有除以约束个数的（SE3中直接除以约束个数了），所以是NoDivide：

    ls4.finishNoDivide();
    ls6.finishNoDivide();
    ls7.initializeFrom(ls6, ls4);

我们查看NormalEquationsLeastSquares7::initializeFrom:

void NormalEquationsLeastSquares7::initializeFrom(const NormalEquationsLeastSquares& ls6, const NormalEquationsLeastSquares4& ls4)
{
    // set zero
    A.setZero();
    b.setZero();
    // add ls6
    A.topLeftCorner<6,6>() = ls6.A;
    b.head<6>() = ls6.b;
    // add ls4
    int remap[4] = {2,3,4,6};
    for(int i=0;i<4;i++)
    {
        b[remap[i]] += ls4.b[i];
        for(int j=0;j<4;j++)
            A(remap[i], remap[j]) += ls4.A(i,j);
    }
    num_constraints = ls6.num_constraints + ls4.num_constraints;
}

这里按照公式$\eqref{eq:ax=b}$求得最终的$\mathbf A$和$\mathbf b$，先拷贝光度残差的l6部分，然后把逆深度残差按照在矩阵块的位置对应拷贝。

最终回到Sim3Tracker::trackFrameSim3的位置就是最终求解增量的部分：

                // solve LS system with current lambda
                Vector7 b = - ls7.b / ls7.num_constraints;
                Matrix7x7 A = ls7.A / ls7.num_constraints;
                for(int i=0;i<7;i++) A(i,i) *= 1+LM_lambda;
                Vector7 inc = A.ldlt().solve(b);

这里和SE3一样，最终把$\mathbf A$和$\mathbf b$都除以了约束个数，很是不解。

2. 双向跟踪检测

论文中提及，为了避免误检测，采用双向跟踪检测的方式，对每一个候选帧都计算其与检测关键帧彼此跟踪的$\mathfrak{sim}(3)$，然后计算两者的马氏距离[1]（其实是马氏距离的平方）：

$$\begin{equation} e\left(\mathbf\xi_{j_k i}, \mathbf\xi_{i j_k}\right) := \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)^T \left(\mathbf\Sigma_{j_k i} + \text{Adj}_{j_k i}\mathbf\Sigma_{i j_k}\text{Adj}_{j_k i}^T\right)^{-1} \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)\label{eq:mdis} \end{equation}$$

其实双向跟踪在一开始选取临近的候选帧的时候做SE3跟踪也用到了，但是只是对其中的$\mathfrak{so}(3)$做了2-Norm距离的判断。

公式$\eqref{eq:mdis}$其实把变量都转换到计算$\mathbf\xi_{j_k i}$的坐标系下，也就是对应的关键帧$\cal K_{j_k}$的世界坐标下。为了把两个协方差都用上，这里需要把$\xi_{i j_k}$的协方差转换到对应的切空间（也就是$\xi_{j_k i}$所在的切空间），这里同样使用$\eqref{eq:varprop}$的方差传播公式。首先把在$\cal K_{i}$参考系的位姿变换到$\cal K_{j_k}$参考帧有

$$T(\xi) := \xi_{j_k i} \circ \xi$$
有
$$\mathbf J_T = \frac{d(\xi_{j_k i} \circ \xi)}{d\xi} = \text{Adj}_{j_k i}$$
因此有方差
$$\Sigma_{i j_k}' = \mathbf J_T \Sigma_{i j_k} \mathbf J_T^T$$
上面只是给出了大概的推导，但是在tangent space的方差传播没有看过具体的材料，也不是很懂。

3. 总结

到这篇总结结束，LSD-SLAM主要的算法和代码都已经总结完毕了。在本篇总结中，主要是Sim3跟踪，论文中使用双向跟踪的方式，来确保两帧图像是匹配的。这样的策略以前在光流跟踪中看到过，但是用在图像匹配上还是第一次，但是理论上这都是一致的，只是变换的模型有差异而已。

另外这里有几个疑问：

1. 不论是在SE3跟踪还是Sim3跟踪，在求归一化方差的时候，都使用了旋转微小的假设。在求雅可比的时候，坐标等变量的值是使用近似模型（小旋转假设前提下的）的坐标还是使用完整变换模型下的值呢？会有什么影响？
2. 在ESM求雅克比的时候，对参考帧像素位置求的雅克比得到的梯度为什么需要旋转到图像帧的方向上来？
3. 李代数tangent space上误差传播的计算不是很明确。

---

参考：

1. Semi-Dense Visual Odometry for a Monocular Camera
2. LSD-SLAM: Large-Scale Direct Monocular SLAM
3. Efficient Compositional Approaches for Real-Time Robust Direct Visual Odometry from RGB-D Data
4. Real-time image-based tracking of planes using efficient second-order minimization
5. Lie Groups for 2D and 3D Transformations