LSD-SLAM笔记之SE3Tracking

开源SLAM笔记

LSD-SLAM的Tracking是算法框架中三大部分之一。其主要实现函数为SlamSystem::trackFrame

void SlamSystem::trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp)

这个函数的代码主要分为如下几个步骤实现：

1. 构造新的图像帧：把当前图像构建为新的图像帧
2. 更新参考帧：如果当前参考帧不是最近的关键帧，则更新参考帧
3. 初始化位姿：把上一帧与参考帧的位姿当做初始位姿
4. SE3求解：调用SE3Tracker计算当前帧和参考帧间的位姿变换
5. 判断是否跟踪失败：根据跟踪的像素点个数多少以及跟踪质量来判断
6. 关键帧筛选：通过计算得分确定是否构造新的关键帧

接下来主要介绍SE3求解和关键帧筛选

1. SE3求解

LSD-SLAM在位姿估计中采用了直接法，也就是通过最小化光度误差来求解两帧图像间的位姿变化。并且采用了LM算法迭代求解。LSD-SLAM在求解两帧图像间的SE3变换主要在SE3Tracker类中进行实现。该类中有四个函数比较重要，分别为

• SE3Tracker::trackFrame
• SE3Tracker::calcResidualAndBuffers
• SE3Tracker::calcWeightsAndResidual
• SE3Tracker::calculateWarpUpdate

函数SE3Tracker::trackFrame需要传入三个形参，从参考帧跟踪到当前帧，首先是两帧图像帧的实例，另外就是给定的初始位姿。对于初始位姿，LSD-SLAM中使用了上一帧和参考帧间的位姿作为当前位姿估计的初值(见上面的第３个步骤)。

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

SE3Tracker::trackFrame函数的主体是一个for循环，从图像金字塔的高层level-4开始遍历直到底层level-1。在循环内实现加权高斯牛顿优化算法（Weighted Gauss-Newton Optimization），其实就是增加了鲁棒函数的高斯牛顿。首先看一下SE3Tracker::trackFrame函数的代码结构，可以归纳为如下：

图像金字塔迭代level-4到level-1
Step1: 对参考帧当前层构造点云(reference->makePointCloud)
Step2: 计算变换到当前帧的残差和梯度(calcResidualAndBuffers)
Step3: 计算法方差归一化残差(calcWeightsAndResidual)
Step4: 计算雅克比向量以及A和b(calculateWarpUpdate)
Step5: 计算得到收敛的delta，并且更新SE3(inc = A.ldlt().solve(b))
重复Step2-Step5直到收敛或者达到最大迭代次数
计算下一层金字塔

接下来首先对SE3求解的算法原理做个详细的介绍。

1.1 直接法SE3图像对齐算法

1.1.1 优化模型——归一化方差的光度误差

在LSD-SLAM论文的3.3节介绍了这个对齐算法。首先给出优化模型（这里的标号与论文中的统一），论文采用了最小化归一化方差的光度误差（variance-normalized photometric error）：

$$\begin{align} E_p(\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}\Biggr\|_\delta \tag{12}\\ r_p(\mathbf{p},{\mathbf\xi_{ji}}) :&= I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji})) \tag{13}\\ \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}) \tag{14} \end{align}$$
这里的 $\|\ast\|_\delta$ 为Huber-norm，表示为：
$$\|r^2\|_\delta:=\begin{cases} \frac{r^2}{2\delta} & \text{if}\quad |r|\le \delta \\[2ex] |r|-\frac{\delta}{2} & \text{otherwise} \end{cases}\tag{15}$$
式子中的$\xi_{ji}$就是要求的两帧间的SE3变换（从$i\rightarrow{j}$，也就是参考帧到当前帧的变换），这里是用李代数的形式表示；这里的$\mathbf{p}$是在参考帧$I_i$观测到的有深度信息（$\mathbf{p}\in\Omega_{D_i}$）的归一化图像点；函数 $D_i(\mathbf p)$ 是点$\mathbf p$在参考帧下的逆深度，$V_i(\mathbf p)$ 是对应逆深度的方差；函数$\omega({\mathbf p}, D_i({\mathbf p}), \xi_{ji})$ 是3D投影变换（3D projective warp），把参考帧下${\mathbf p}$对应的3D点投影到当前帧的图像平面；这里的$\sigma_I^2$是图像的高斯噪声。

我们考虑，这里的归一化方差是什么意思？论文中在估计两帧间位姿变换的时候，把所有有逆深度假设的像素都用上了。但是每个逆深度的确定性不同，也就是有些逆深度比较准确，有些不准确。而准确与否则体现在逆深度的方差上了。因此在公式$(12)$中在残差上除以了方差做了归一化。在论文中考虑了两个方面的方差，一个是由逆深度估计不准确引入的，另外一个是由图像高斯噪声引起的。也即是说$(14)$式前面的是连个图像的图像高斯噪声，后面的是逆深度造成的误差。这里逆深度误差不确定性是根据下式计算得到的：

$$\mathbf{\Sigma}_f = \mathbf{J}_f \mathbf{\Sigma}_{\mathbf x} \mathbf{J}_f^T \tag{11}$$
$(14)$式的具体推导在代码部分讲详细说明。

论文中给出了3D投影变换$\omega$的表达式，也就是论文中的公式$(3)$：

$$\begin{split} \omega({\mathbf p}, D_i({\mathbf p}), \xi)) := \begin{pmatrix}x'/z'\\y'/z'\\\color{red}{1/z'}\end{pmatrix} \quad\text{with}\quad \begin{pmatrix}x'\\y'\\z'\\1\end{pmatrix} := \text{exp}_{\mathcal se(3)}(\xi)\begin{pmatrix}\mathbf{p}_x/d\\\mathbf{p}_y/d\\1/d\\1\end{pmatrix} \end{split}\tag{3}$$
其实这个公式只是把3D点变换到单位平面上，这里有一个问题：一个是标红的$1/z'$应该是$1$，正确的写法应该是：
$$\omega({\mathbf p}, D_i({\mathbf p}), \xi)) = \begin{pmatrix}x'/z'\\y'/z'\\1\end{pmatrix} \tag{3*}$$
这里实际上是两个变换，先是一个SE3变换，再是一个从3D空间到单位平面的变换，可以记作：
$$\omega({\mathbf p}, D_i({\mathbf p}), \xi)) := (\omega_n \oplus \omega_s)({\mathbf p}, D_i({\mathbf p}), \xi))\tag{4}$$
另外，由于这个函数得到的是在归一化平面上的点，而论文中定义的$I({\mathbf{p}})$中的${\mathbf{p}}$是归一化图像坐标，这个和通常的习惯不符合，因为通常图像灰度函数的自变量应该是图像坐标$\mathbf{u}$，写作$I({\mathbf{u}})$，因此这里在实际求的时候需要把得到的${\mathbf{p}}$做一次投影（乘以相机内参$K$）才得到图像坐标，有：
$$\mathbf{u}=K\mathbf{p}=\begin{pmatrix}fx&0&cx\\0&fy&cy\\0&0&1\end{pmatrix}\mathbf{p}$$

1.1.2 非线性优化——加权GN算法

论文中讨论的是对如下形式优化的高斯牛顿算法，也就是基本的光度误差，优化目标是使得下式最小：

$$E(\xi)=\sum_i\underbrace{(I_{ref}(\mathbf{p}_i)-I(\omega({\mathbf{p}}, D_{ref}({\mathbf{p}}), \xi)))^2}_{=:r_i^2(\mathbf{\xi})}\tag{5}$$
在论文中作者李代数使用左乘的形式，根据GN算法，在初始位置$\mathbf{\xi}^{(0)}$处进行展开，每一次的增量记为$\delta\mathbf{\xi}^{(n)}$，首先把$(5)$式写成优化的形式有：
$$\delta\mathbf{\xi}^{*}=\text{arg}\min_{\delta\mathbf{\xi}}E(\delta\mathbf{\xi}\circ\mathbf{\xi})=\text{arg}\min_{\delta\mathbf{\xi}}\sum_i{r_i^2(\delta\mathbf{\xi}}\circ\mathbf{\xi})\tag{5*}$$
接下来对光度误差做一阶泰勒展开：
$$r_i(\delta\xi\circ\mathbf{\xi})=r_i(\mathbf{\xi})+\mathbf{J}_i\delta\xi$$
并且带入$(5*)$式后对$\delta\xi$求导数并且使求导后的结果为$0$，最后可以解出增量(如下为离散的形式)：
$$\delta\mathbf{\xi}^{(n)}＝-(\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\mathbf{r}(\mathbf{\xi}^{(n)}) \quad\text{with}\quad \mathbf{J}=\frac{\partial{\mathbf{r}(\epsilon\circ\mathbf{\xi}^{(n)})}}{\partial\epsilon}\bigg|_{\epsilon=0}\tag{6}$$
这里的$\mathbf{J}$是对残差向量$\mathbf{r}=(r_1,\dots,r_k)^T$求导数的结果，需要注意的是这里的$r_i$就是式子$(5)$中的$r_i(\mathbf\xi)$，$k$为参与优化的点的个数。这里的新的估计值由如下形式更新：
$$\mathbf{\xi}^{(n+1)}=\delta\mathbf{\xi}^{(n)}\circ\mathbf{\xi}^{(n)}\tag{7}$$
为了减少外点（outliers）对算法的影响，论文中使用了迭代变权重最小二乘（interatively re-weighted least-squares）的形式，也就是在每次计算残差的时候乘以一个权重矩阵$\mathbf{W}=\mathbf{W}(\mathbf\xi^{(n)})$,从而代价函数变为：
$$E(\xi)=\sum_i\omega_i(\mathbf\xi)r_i^2(\mathbf\xi)\tag{8}$$
更新量也变为：
$$\delta\mathbf{\xi}^{(n)}=-(\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\mathbf{W}\mathbf{r}(\mathbf{\xi}^{(n)})\tag{9}$$
其实在实现的时候，这里给的权重就是Huber-weight。
$$w(r)_\delta:=\begin{cases} 1 & \text{if}\quad |r|\le \delta \\[2ex] \frac{\delta}{|r|} & \text{if}\quad |r|> \delta \end{cases}$$
这里需要说明的是，在解$\delta\mathbf{\xi}$的时候，通常不采用对Hession矩阵（$\mathbf{J}^T\mathbf{J}$）求逆的方式来解，而是使用LDLT分解来解，也就是在对优化模型求导之后把公式整理为$Ax=b$的形式，然后调用Eigen库的ldlt函数求解：
$$\underbrace{(\mathbf{J}^T\mathbf{J})}_A\delta\mathbf{\xi}^{(n)}=\underbrace{-\mathbf{J}^T\mathbf{W}\mathbf{r}(\mathbf{\xi}^{(n)})}_b\tag{6*}$$
接下来我们结合代码来梳理一下。

1.2 代码剖析

1.2.1 计算残差等数据

在GN算法中，通常需要在每次更新了估计值之后重新计算一下在新估计值下的雅克比，而计算雅克比就需要图像梯度等数据，由于论文采用的是图像对齐算法中的正向加法（Lucas-Kanades Algorithm)，所以在每一次迭代的过程中都需要重新计算雅可比，也需要计算从模版图像（参考帧）上点变换到当前图像（当前帧）上对应位置的灰度以及梯度数据。计算雅可比前的这些预备工作就是在函数SE3Tracker::calcResidualAndBuffers和SE3Tracker::calcWeightsAndResidual中进行处理的。

首先我们看一下函数calcResidualAndBuffers：

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)
{
    calcResidualAndBuffers_debugStart();
    if(plotResidual)
        debugImageResiduals.setTo(0);
    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.rotationMatrix();
    Eigen::Vector3f transVec = referenceToFrame.translation();
    const Eigen::Vector3f* refPoint_max = refPoint + refNum;
    const Eigen::Vector4f* frame_gradients = frame->gradients(level);
    int idx=0;
    float sumResUnweighted = 0;
    bool* isGoodOutBuffer = idxBuf != 0 ? frame->refPixelWasGood() : 0;
    int goodCount = 0;
    int badCount = 0;
    float sumSignedRes = 0;
    float sxx=0,syy=0,sx=0,sy=0,sw=0;
    float usageCount = 0;
    for(;refPoint<refPoint_max; refPoint++, refColVar++, idxBuf++)
    {
        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))
        {
            if(isGoodOutBuffer != 0)
                isGoodOutBuffer[*idxBuf] = false;
            continue;
        }
        Eigen::Vector3f resInterp = getInterpolatedElement43(frame_gradients, u_new, v_new, w);
        float c1 = affineEstimation_a * (*refColVar)[0] + affineEstimation_b;
        float c2 = resInterp[2];
        float residual = c1 - c2;
        float weight = fabsf(residual) < 5.0f ? 1 : 5.0f / fabsf(residual);
        sxx += c1*c1*weight;
        syy += c2*c2*weight;
        sx += c1*weight;
        sy += c2*weight;
        sw += weight;
        bool isGood = residual*residual / (MAX_DIFF_CONSTANT + MAX_DIFF_GRAD_MULT*(resInterp[0]*resInterp[0] + resInterp[1]*resInterp[1])) < 1;
        if(isGoodOutBuffer != 0)
            isGoodOutBuffer[*idxBuf] = isGood;
        *(buf_warped_x+idx) = Wxp(0);
        *(buf_warped_y+idx) = Wxp(1);
        *(buf_warped_z+idx) = Wxp(2);
        *(buf_warped_dx+idx) = fx_l * resInterp[0];
        *(buf_warped_dy+idx) = fy_l * resInterp[1];
        *(buf_warped_residual+idx) = residual;
        *(buf_d+idx) = 1.0f / (*refPoint)[2];
        *(buf_idepthVar+idx) = (*refColVar)[1];
        idx++;
        if(isGood)
        {
            sumResUnweighted += residual*residual;
            sumSignedRes += residual;
            goodCount++;
        }
        else
            badCount++;
        float depthChange = (*refPoint)[2] / Wxp[2];    // if depth becomes larger: pixel becomes "smaller", hence count it less.
        usageCount += depthChange < 1 ? depthChange : 1;
        // DEBUG STUFF
        if(plotTrackingIterationInfo || plotResidual)
        {
            // for debug plot only: find x,y again.
            // horribly inefficient, but who cares at this point...
            Eigen::Vector3f point = KLvl * (*refPoint);
            int x = point[0] / point[2] + 0.5f;
            int y = point[1] / point[2] + 0.5f;
            if(plotTrackingIterationInfo)
            {
                setPixelInCvMat(&debugImageOldImageSource,getGrayCvPixel((float)resInterp[2]),u_new+0.5,v_new+0.5,(width/w));
                setPixelInCvMat(&debugImageOldImageWarped,getGrayCvPixel((float)resInterp[2]),x,y,(width/w));
            }
            if(isGood)
                setPixelInCvMat(&debugImageResiduals,getGrayCvPixel(residual+128),x,y,(width/w));
            else
                setPixelInCvMat(&debugImageResiduals,cv::Vec3b(0,0,255),x,y,(width/w));
        }
    }
    buf_warped_size = idx;
    pointUsage = usageCount / (float)refNum;
    lastGoodCount = goodCount;
    lastBadCount = badCount;
    lastMeanRes = sumSignedRes / goodCount;
    affineEstimation_a_lastIt = sqrtf((syy - sy*sy/sw) / (sxx - sx*sx/sw));
    affineEstimation_b_lastIt = (sy - affineEstimation_a_lastIt*sx)/sw;
    calcResidualAndBuffers_debugFinish(w);
    return sumResUnweighted / goodCount;
}

该函数主要做了如下几件事：

• 如果参考帧坐标系下的3D点$\mathbf{p}_i$能投影到当前帧，则把变换到当前帧坐标系下得到的3D点$\mathbf{p}_i'$，记录在变量buf_warped_x、buf_warped_y、buf_warped_z
• 计算$\mathbf{p}_i'$在当前帧的投影点位置的梯度buf_warped_dx、buf_warped_dx，以及残差buf_warped_residual
• 记录参考帧下点$\mathbf{p}_i$的逆深度buf_d以及（逆深度的）方差buf_idepthVar

这里有一点，就是现在求得的残差只是纯粹的光度误差，是把参考帧中3D点对应的灰度与投影到当前帧位置处得到的灰度（通过双线性插值）做差得到的，此时并没有乘以权重（代码L62-L66）：

        Eigen::Vector3f resInterp = getInterpolatedElement43(frame_gradients, u_new, v_new, w);
        float c1 = affineEstimation_a * (*refColVar)[0] + affineEstimation_b;
        float c2 = resInterp[2];
        float residual = c1 - c2;

而公式$(12)$中的归一化方差以及Huber-weight将在函数calcWeightsAndResidual中计算。

疑问1：代码中对参考帧的灰度做了一个仿射变换的处理，这里的原理是什么？在SVO代码中的确有考虑到两帧见位姿相差过大，因此通过在空间上的仿射变换之后再求灰度的操作。但是在这里的代码中没有看出具体原理。

需要注意的是，在给梯度变量赋值的时候（上面代码L84-L85），这里乘了焦距：

        *(buf_warped_dx+idx) = fx_l * resInterp[0];
        *(buf_warped_dy+idx) = fy_l * resInterp[1];

这样做的原因在函数calcWeightsAndResidual分析的时候会解释。

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

float SE3Tracker::calcWeightsAndResidual(
        const Sophus::SE3f& referenceToFrame)
{
    float tx = referenceToFrame.translation()[0];
    float ty = referenceToFrame.translation()[1];
    float tz = referenceToFrame.translation()[2];
    float sumRes = 0;
    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 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
        // calc dw/dd (first 2 components):
        float g0 = (tx * pz - tz * px) / (pz*pz*d);
        float g1 = (ty * pz - tz * py) / (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 weighted_rp = fabs(rp*sqrtf(w_p));
        float wh = fabs(weighted_rp < (settings.huber_d/2) ? 1 : (settings.huber_d/2) / weighted_rp);
        sumRes += wh * w_p * rp*rp;
        *(buf_weight_p+i) = wh * w_p;
    }
    return sumRes / buf_warped_size;
}

在代码中22-23行是计算式$(14)$中的偏导数，考虑只有位移，对于SE3变换，也就是$(3)$右边的式子则有：

$$\begin{pmatrix}x'\\y'\\z'\end{pmatrix} = \begin{pmatrix}\mathbf{p}_x/d\\\mathbf{p}_y/d\\1/d\end{pmatrix} + \begin{pmatrix}\mathbf{t}_x\\\mathbf{t}_y\\\mathbf{t}_z\end{pmatrix}\tag{3.1a}$$
我们把式子整理一下，写出式子$(3*)$的形式：
$$\omega({\mathbf p}, D_i({\mathbf p}), \xi)) = \begin{pmatrix}\frac{\mathbf{p}_x/d+\mathbf{t}_x}{1/d+\mathbf{t}_z}\\\frac{\mathbf{p}_y/d+\mathbf{t}_y}{1/d+\mathbf{t}_z}\\1\end{pmatrix}$$
现在我们就可以把式子$(14)$中的偏导求解出来
$$\begin{split} \frac{\partial{r_p(\mathbf{p},\mathbf\xi_{ji})}}{\partial{D_i(\mathbf{p})}} &= \frac{\partial({I_i({\mathbf p}) - I_j(\omega({\mathbf p}, D_i({\mathbf p}), \xi_{ji})))}}{\partial{D_i(\mathbf{p})}} \\ &= - \frac{\partial{I_j(\mathbf{a})}}{\partial{\mathbf{a}}}\bigg|_{\mathbf{a}=\mathbf{p}} \cdot\frac{\partial{w(d)}}{\partial{d}}\bigg|_{d=D_i(\mathbf{p})} \\ &= -\begin{pmatrix}dxfx&dyfy\end{pmatrix}\cdot\begin{pmatrix}\frac{\mathbf{t}_x(1/d+\mathbf{t}_z)-\mathbf{t}_z(\mathbf{p}_x/d+\mathbf{t}_x)}{(1/d+\mathbf{t}_z)^2d} \\ \frac{\mathbf{t}_y(1/d+\mathbf{t}_z)-\mathbf{t}_z(\mathbf{p}_y/d+\mathbf{t}_y)}{(1/d+\mathbf{t}_z)^2d}\end{pmatrix}\\ &= -(dxfx\frac{\mathbf{t}_xz'-\mathbf{t}_zx'}{z'^2d} + dyfy\frac{\mathbf{t}_yz'-\mathbf{t}_zy'}{z'^2d}) \end{split} \tag{*}$$
其中第一个偏导是在参考帧的3D点$\mathbf{p}$的位置求的，dx和dy分别是点$\mathbf{p}$在图像$I_j$投影位置处两个方向的梯度；而第二个偏导是在点$\mathbf{p}$在参考帧中的逆深度$D_i(\mathbf{p})$处求的，所有最终我们得到式子$(*)$中的结果。

接下来我们回到代码,如下两行就是第二个偏导的结果（本来是一个三维的列向量，最后一维是$0$，所以直接忽略了）

        // calc dw/dd (first 2 components):
        float g0 = (tx * pz - tz * px) / (pz*pz*d);
        float g1 = (ty * pz - tz * py) / (pz*pz*d);

接下来是求的是整个$(*)$式：

        // calc w_p
        float drpdd = gx * g0 + gy * g1;    // ommitting the minus

注意：正如源码所注释，这里省略了负号，另外，gx和gy是梯度没错，但是少了两个焦距fx和fy。这里需要说明的是，梯度变量buf_warped_dx和buf_warped_dy在函数SE3Tracker::calcResidualAndBuffers中已经乘以焦距了。同时我们看$(*)$式第一个图像函数对单位化平面上的点求偏导的结果，就是梯度和焦距乘在了一起，这就不难理解为什么在代码中作者这么处理的原因了：

        *(buf_warped_dx+idx) = fx_l * resInterp[0];
        *(buf_warped_dy+idx) = fy_l * resInterp[1];

1.2.2 计算雅可比

最后一个函数SE3Tracker::calculateWarpUpdate是计算雅可比以及最小二乘方程的系数

Vector6 SE3Tracker::calculateWarpUpdate(
        NormalEquationsLeastSquares &ls)
{
//  weightEstimator.reset();
//  weightEstimator.estimateDistribution(buf_warped_residual, buf_warped_size);
//  weightEstimator.calcWeights(buf_warped_residual, buf_warped_weights, buf_warped_size);
//
    ls.initialize(width*height);
    for(int i=0;i<buf_warped_size;i++)
    {
        float px = *(buf_warped_x+i);
        float py = *(buf_warped_y+i);
        float pz = *(buf_warped_z+i);
        float r =  *(buf_warped_residual+i);
        float gx = *(buf_warped_dx+i);
        float gy = *(buf_warped_dy+i);
        // step 3 + step 5 comp 6d error vector
        float z = 1.0f / pz;
        float z_sqr = 1.0f / (pz*pz);
        Vector6 v;
        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;
        // step 6: integrate into A and b:
        ls.update(v, r, *(buf_weight_p+i));
    }
    Vector6 result;
    // solve ls
    ls.finish();
    ls.solve(result);
    return result;
}

首先看下这里的雅可比，回顾一下$(12)$式，这个求雅可比？这么复杂？分子分母都和要求的SE3有关。

$$E_p(\mathbf\xi_{ij}) =\sum_{\mathbf{p}\in\Omega_{D_i}} \begin{Vmatrix}\frac{r_p^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2}\end{Vmatrix}_\delta \tag{12}$$
实际上代码只是把分母当作一个系数来处理，所以我们直接看$(5*)$式
$$\delta\mathbf{\xi}^{*}=\text{arg}\min_{\delta\mathbf{\xi}}E_p(\delta\mathbf{\xi}\circ\mathbf{\xi})=\text{arg}\min_{\delta\mathbf{\xi}}\sum_i{r_i^2(\delta\mathbf{\xi}}\circ\mathbf{\xi})\tag{5*}$$
并且$(6)$式直接求了我们需要的雅可比，对于参考帧中的一个3D点$\mathbf{p}_i$位置处的残差求雅可比，这里需要使用链式求导法则
$$\begin{split} \mathbf{J}_i &= \frac{\partial{r_i(\epsilon\circ\mathbf{\xi}^{(n)})}}{\partial\epsilon}\bigg|_{\epsilon=0}\\ &= -\frac{\partial{I(\omega({\mathbf{p}_i},D_{ref}({\mathbf{p}_i}),\epsilon\circ\xi))}}{\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 \begin{pmatrix}I|-[\mathbf{p}_i'{\cdot}z_i']_\times\end{pmatrix}\\ &= -\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}1&0&0&0&z'&-y'\\0&1&0&-z'&0&x'\\0&0&1&y'&-x'&0\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\end{pmatrix}^T \end{split}\tag{**}$$
上面第一个求的偏导和$(*)$式是一样的，是图像灰度对归一化平面上的点的偏导；第二个是归一化平面上的点对空间三维点的偏导（对应式子$(3*)$）；第三个偏导是SE3变换对扰动$\mathfrak\epsilon$的偏导（对应$(3)$右式）。式子中的$[*]_\times$为叉乘矩阵。
这样求偏导就十分清晰，最终我们得到了所求的雅可比向量，对照代码

        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;

这里和函数calcWeightsAndResidual一样，gx和gy是已经乘了焦距的梯度，因此代码和我们的推导结果一致。

注意：这里的雅可比和实际我们推导出来的差了一个负号！最终解方程的时候需要注意！

函数的最后就是更新最小二乘系数矩阵$A$和向量$b$，我们查看调用的最小二乘update函数：

inline void NormalEquationsLeastSquares::update(const Vector6& J, const float& res, const float& weight)
{
//  printf("up: %f, %f, %f, %f, %f, %f; res: %f; w: %f\n",
//          J[0],J[1],J[2],J[3],J[4],J[5],res, weight);
  A_opt.rankUpdate(J, weight);
  //MathSse<Sse::Enabled, float>::addOuterProduct(A, J, factor);
  //A += J * J.transpose() * factor;
  //MathSse<Sse::Enabled, float>::add(b, J, -res * factor); // not much difference :(
  b -= J * (res * weight);
  error += res * res * weight;
  num_constraints += 1;
}

这里做的就是计算式子$(6*)$的$A$和$b$。

在函数的最后，调用finish()函数：

void NormalEquationsLeastSquares::finish()
{
  A_opt.toEigen(A);
  A /= (float) num_constraints;
  b /= (float) num_constraints;
  error /= (float) num_constraints;
}

这里对矩阵的每个元素都除以了num_constraints，这也就是点的个数。

疑问2：为什么要除以这个系数，有什么区别吗？

1.2.3 计算增量

得到最小二乘形式的方程$A\delta\mathbf{\xi}=\mathbf{b}$后，就是解出我们的$\delta\mathbf{\xi}$！这部分代码在SE3Tracker::trackFrame的GN迭代中：

                // solve LS system with current lambda
                Vector6 b = -ls.b;
                Matrix6x6 A = ls.A;
                for(int i=0;i<6;i++) A(i,i) *= 1+LM_lambda;
                Vector6 inc = A.ldlt().solve(b);

这里很明白了，调用了Eigen的LDLT来求解，并且这里把向量$b$取了负号，这样就和之前推导的结果一致了！

1.2.4 迭代

SE3Tracker::trackFrame函数的主体是一个for循环，从图像金字塔的高层level-4开始遍历直到底层level-1。每一层都进行LM优化迭代，则是另外一个for循环。
上面讲的哪些步骤都是GN的方法，LM的方法区别就在于对于解最小二乘$A\mathbf{x}=\mathbf{b}$的时候，增加了一个系数$\lambda$，也就是$(A+{\lambda}I)\mathbf{x}=\mathbf{b}$。在代码中，作者只在迭代结果使得误差发散的时候给$\lambda$赋予一个非零的值，同时不断增加$\lambda$的值进行求解，直到解得的结果是收敛的，才进行下一次迭代。

2. 关键帧筛选

在论文中的3.4节说明了如何选取关键帧。论文的意思是根据运动距离来确定，如果当前相机运动距离参考帧过远则把当前帧创建为关键帧。并且给出了距离函数：

$$\text{dist}(\mathbf{\xi_{ji}}):=\mathbf{\xi_{ji}}^T\mathbf{W}\mathbf{\xi_{ji}}\tag{16}$$
其中$\mathbf{W}$是权重矩阵。并且距离阈值根据当前帧场景平均逆深度来确定。

其实我们在看代码时发现，关键帧筛选策略和论文中说的还是有点不一样的。代码中有两处构建新关键帧的地方。一个是正常系统运行的时候，一个是在重定位的时候。

首先看重定位部分，在函数SlamSystem::trackFrame中对关键帧进行筛选，代码如下:

if (!my_createNewKeyframe && currentKeyFrame->numMappedOnThisTotal > MIN_NUM_MAPPED)
    {
        Sophus::Vector3d dist = newRefToFrame_poseUpdate.translation() * currentKeyFrame->meanIdepth;
        float minVal = fmin(0.2f + keyFrameGraph->keyframesAll.size() * 0.8f / INITIALIZATION_PHASE_COUNT,1.0f);
        if(keyFrameGraph->keyframesAll.size() < INITIALIZATION_PHASE_COUNT) minVal *= 0.7;
        lastTrackingClosenessScore = trackableKeyFrameSearch->getRefFrameScore(dist.dot(dist), tracker->pointUsage);
        if (lastTrackingClosenessScore > minVal)
        {
            createNewKeyFrame = true;
            if(enablePrintDebugInfo && printKeyframeSelectionInfo)
                printf("SELECT %d on %d! dist %.3f + usage %.3f = %.3f > 1\n",trackingNewFrame->id(),trackingNewFrame->getTrackingParent()->id(), dist.dot(dist), tracker->pointUsage, trackableKeyFrameSearch->getRefFrameScore(dist.dot(dist), tracker->pointUsage));
        }
        else
        {
            if(enablePrintDebugInfo && printKeyframeSelectionInfo)
                printf("SKIPPD %d on %d! dist %.3f + usage %.3f = %.3f > 1\n",trackingNewFrame->id(),trackingNewFrame->getTrackingParent()->id(), dist.dot(dist), tracker->pointUsage, trackableKeyFrameSearch->getRefFrameScore(dist.dot(dist), tracker->pointUsage));
        }
    }

第一个if判断的条件是当前建图线程是否已经更新好上一帧关键帧以及跟踪最近的关键帧（参考帧）的图像帧个数不小于一定值（MIN_NUM_MAPPED，为$5$）。如果满足上述的条件则计算得分，得分大于一定阈值则确定构建新的关键帧。

首先我们看一下得分是怎么计算的。这里的得分和当前帧和参考帧之间的位移大小有关。计算得分的函数TrackableKeyFrameSearch::getRefFrameScore如下：

inline float getRefFrameScore(float distanceSquared, float usage)
{
    return distanceSquared*KFDistWeight*KFDistWeight
            + (1-usage)*(1-usage) * KFUsageWeight * KFUsageWeight;
}

该函数第一个参数是运动距离；第二个参数是当前帧跟踪参考帧所用的3D点占参考帧所有3D点的比例。tracker->pointUsage这个变量在函数SE3Tracker::calcResidualAndBuffers中进行计算，可以理解为当前帧和参考帧重叠区域的比例。看得分的计算方法，可以看出，当位移越大以及当前帧和参考帧重叠度越低，则得分越大（KFUsageWeight默认为４）。

这里有一点需要注意，在计算运动距离的时候，这里采用来位移向量先乘以一个平均场景逆深度：

Sophus::Vector3d dist = newRefToFrame_poseUpdate.translation() * currentKeyFrame->meanIdepth;

显然，大场景的逆深度会小，小场景的逆深度会大，这相当于一个权重，对于相同的位移，认为小场景的运动程度比较大。

然后是阈值变量：

float minVal = fmin(0.2f + keyFrameGraph->keyframesAll.size() * 0.8f / INITIALIZATION_PHASE_COUNT,1.0f);
if(keyFrameGraph->keyframesAll.size() < INITIALIZATION_PHASE_COUNT) minVal *= 0.7;

这里的两个操作都是使得初始化阶段当关键帧比较少的时候（INITIALIZATION_PHASE_COUNT为5）放宽了阈值，之后就是$１$。

正常情况下，则是在函数SlamSystem::changeKeyframe中改变当前参考关键帧。在changeKeyframe函数中通过调用函数TrackableKeyFrameSearch::findRePositionCandidate来找一帧参考帧，如果没有符合的就创建一个新的关键帧。同样也用函数TrackableKeyFrameSearch::getRefFrameScore来计算得分。

3. 总结

在这部分算法和代码中，对于实际设计SLAM系统有几个可以借鉴的地方：

1. 使用归一化方差的残差模型，通过方差给残差设置权重。在SVO中使用的是收敛的空间点，而这里基本使用了所有的点。
2. 图像灰度函数自变量定义为归一化图像坐标，求偏导时焦距和梯度就可以直接作为一个整体来处理。
3. 在高斯-牛顿的基础上可以改变为列文伯格-马夸尔特方法，系数只有在迭代结果发散的情况下设置为非零，从而变为梯度下降法，使得结果收敛。
4. 在关键帧筛选的时候，可以引入当前帧场景平均（逆）深度作为一个标准，可以比较好处理大场景和小场景的情况。

以及存在一个疑问：

1. 在求残差的时候使用的仿射变换是什么原理？光流中有构建简单的线性（仿射）亮度变化模型的做法，但是和这里的还是有点不一样。
2. 在求最小二乘矩阵的时候，为什么要在最后求得的$\mathbf A$和$\mathbf b$都除以约束（点）的个数？

---

参考文献

1. LSD-SLAM: Large-Scale Direct Monocular SLAM