DSO 位姿估计及代码解析

最新推荐文章于 2024-09-10 16:34:11 发布

waittingforyou12

最新推荐文章于 2024-09-10 16:34:11 发布

阅读量1.9k

点赞数 4

分类专栏：视觉slam

本文链接：https://blog.csdn.net/waittingforyou12/article/details/104449733

版权

视觉slam 专栏收录该内容

6 篇文章 1 订阅

订阅专栏

在进行具体介绍之前，我们需要对DSO系统的Tracking部分有个大致的了解。深入代码的读者会知道作者是将当前帧与最近的关键帧进行direct image alignment从而求解当前帧到最近关键帧的位姿变化，即tracking过程。既然是与最近关键帧进行追踪，自然的是需要获取最近关键帧上一系列特征点的深度，即最近关键帧的半稠密深度图。那么应该怎么获取到这个半稠密的深度图呢？我们知道，DSO在后端通过边缘化的方式去维护一个包含5-7个关键帧的动态窗口。每个关键帧都对应一系列的成熟地图点，所以作者是将后端所有关键帧中的成熟地图点通过相互位姿关系转化到最近关键帧中，因此可以很精确的计算出这些地图点在最近关键帧中深度和其对应在图像上的投影位置。有了这个最近关键帧的半稠密深度图和相应在图像上的投影位置，我们就可以运用direct image alignment的方式进行位姿估计了。

direct image alignment

如图1.1所示，最近关键帧中的黑色点代表滑动窗口中所有成熟点在最近关键帧中的投影，也因此计算出了每个投影点对应深度，我们以一个点为例进行说明。假设空间中某一地图点在最近关键帧的坐标为, 其在当前帧坐标系下的坐标为 $P^{'}$ 。结合光度仿射变换函数（对光度标定有疑问的可以参考系列文章DSO光度标定），作者定义光度残差能量方程：

$E_{Pj}:=\sum_{p\in\mathbb{N}_{P}}w_{p}\left \| (I_{j}[p^{'}]-b_{j})- \frac{t_je^{a_j}}{t_ie^{a_i}}(I_i[p]-b_i) \right \|_{\gamma }$

其中 $\mathbb{N}_{p}$ 代表最近关键帧坐标系下一系列的地图点， $I_{i}$ 代表最近关键帧的图像， $I_{j}$ 代表当前帧的图像， $t_{i}$ 和 $t_{j}$ 分别代表最近关键帧和当前帧的曝光时间， $\left \| \cdot \right \|_{\gamma }$ 代表Huber Norm, 代表坐标在最近关键帧上的投影位置，它对应的逆深度为 $d_{p}$ ， $p^{'}$ 代表坐标 $P^{'}$ 在当前帧的投影。假设最近关键帧到当前帧的位姿为（R,t） (R,t) ,因此可以建立如下投影关系：

$p^{'}=\Pi _{c}(R\Pi^{-1}(p,d_{p})+t)$

综上，direct image alignment 的核心就是优化关键帧到当前帧的位姿为（R,t） (R,t) ，让光度残差能量达到最小值，不过由于加入了光度相关的建模函数，在跟踪阶段也实时的对光度参数进行优化，因此在direct image alignment 阶段，优化变量的自由度为8，即6自由度位姿加上2个自由度的光度参数。需要特别指出的是，由于光度参数和位姿真值的尺度不在同一个范围内，因此作者在代码实现中对优化变量进行了不同尺度的缩放，目的是让每个优化变量都大致在范围内，这样在优化过程中不容易产生震荡，让优化速度变慢，这一点在深度学习中应用较广（既可以看做深度学习中的数据normalization）。

接着自然就是获取光度残差对优化变量的雅克比，推导过程完全可以参考高博知乎的DSO详解，在这里就不再赘述了。有了残差和雅克比，那么就可以采用Gauss-Nentow或者Levenberg Marquart 方法进行优化求解了。上述便是DSO中位姿跟踪的大致描述，不过论文介绍的知识仅仅体现了作者思想的一部分，更多的技巧与优化方式需要我们在阅读代码中去加深理解，接下来笔者将根据自己的一些理解对Tracking部分代码进行一定的解析，如果不对，还请大家指出共同讨论学习。

Tracking 代码解析

DSO的Tracking代码只要是在src/FullSystem/CoarseTracker.h 和 src/FullSystem/CoarseTracker.cpp中。我们先来分析一下它的类声明。

class CoarseTracker
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    // 构造函数
    CoarseTracker(int w, int h);
    ~CoarseTracker();

    // 进行当前帧与最近关键帧跟踪的主函数接口
    // 其中newFrameHessian代表最近的关键帧
    // lastToNew_out 是最近关键帧到当前帧的估计初值， aff_a2l_out是当前帧光度参数的估计初值
    // coarsestLvl 是表示direct image alignment在图像金字塔第几层进行
    bool trackNewestCoarse(
        FrameHessian* newFrameHessian,
        SE3 &lastToNew_out, AffLight &aff_g2l_out,
        int coarsestLvl, Vec5 minResForAbort,
        IOWrap::Output3DWrapper* wrap = 0);
    // 设置tracking 需要的最近关键帧
    void setCoarseTrackingRef(std::vector<FrameHessian*> frameHessians);
    // 准备内参数据，因为在DSO中作者也实时的在每一个关键帧的时候优化相机内参，
    // 因此，每次内参优化后，需要重新进行设置
    void makeK(
        CalibHessian* HCalib);
    
    bool debugPrint, debugPlot;

    Mat33f K[PYR_LEVELS];
    Mat33f Ki[PYR_LEVELS];
    float fx[PYR_LEVELS];
    float fy[PYR_LEVELS];
    float fxi[PYR_LEVELS];
    float fyi[PYR_LEVELS];
    float cx[PYR_LEVELS];
    float cy[PYR_LEVELS];
    float cxi[PYR_LEVELS];
    float cyi[PYR_LEVELS];
    int w[PYR_LEVELS];
    int h[PYR_LEVELS];

    void debugPlotIDepthMap(float* minID, float* maxID, 
                             std::vector<IOWrap::Output3DWrapper*> &wraps);
    void debugPlotIDepthMapFloat(std::vector<IOWrap::Output3DWrapper*> &wraps);

    // 将最近关键帧设置为lastRef
    FrameHessian* lastRef;
    // 将最近关键帧的光度参数设置为lastRef_aff_g2l
    AffLight lastRef_aff_g2l;
    // 将当前帧设置为newFrame
    FrameHessian* newFrame;
    int refFrameID;

    // act as pure ouptut
    Vec5 lastResiduals;
    Vec3 lastFlowIndicators;
    double firstCoarseRMSE;
private:

    // 这个函数是将所有优化窗口中的关键帧的地图点去全部投影到最近的关键帧，在
    // 最近关键帧中形成半稠密深度图
    void makeCoarseDepthL0(std::vector<FrameHessian*> frameHessians);
    float* idepth[PYR_LEVELS];
    float* weightSums[PYR_LEVELS];
    float* weightSums_bak[PYR_LEVELS];


    Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
    // 计算残差的函数
    Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
    // 计算雅克比，构建Hessian矩阵和b矩阵。用SSE 4float并行加速
    void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
    void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);

    // pc buffers
    // 最近关键帧形成的深度对应的uv
    float* pc_u[PYR_LEVELS];
    float* pc_v[PYR_LEVELS];
    // 最近关键帧形成的深度图
    float* pc_idepth[PYR_LEVELS];
    // 对应的灰度
    float* pc_color[PYR_LEVELS];
    int pc_n[PYR_LEVELS];

    // warped buffers
    float* buf_warped_idepth;
    float* buf_warped_u;
    float* buf_warped_v;
    float* buf_warped_dx;
    float* buf_warped_dy;
    float* buf_warped_residual;
    float* buf_warped_weight;
    float* buf_warped_refColor;
    int buf_warped_n;

    // 指针管理
    std::vector<float*> ptrToDelete;

    // 这个类用来构建优化中的线性方程 （Hessian H and b）
    Accumulator9 acc;
};

接着深入的解析函数makeCoraseDepthL0函数，我们知道，这个函数就是在将后端优化窗口中所有关键帧上的地图点投影到最近的关键帧，形成半稠密深度图，我们来看一下具体是怎么进行。这个函数的输入就是std::vector<FrameHessian*> 即是后端优化窗口中所有的关键帧。

void CoarseTracker::makeCoarseDepthL0(std::vector<FrameHessian*> frameHessians)
{
    // make coarse tracking templates for latstRef.
    memset(idepth[0], 0, sizeof(float)*w[0]*h[0]);
    memset(weightSums[0], 0, sizeof(float)*w[0]*h[0]);
    
    // 遍历后端窗口关键帧
    for (FrameHessian* fh : frameHessians)
    {
        // 遍历关键帧上的成熟地图点
        for (PointHessian* ph : fh->pointHessians)
        {
            // 地图点会在所有的关键帧形成投影，在最近关键帧的投用会记录在lastResidual中
            // 首先会判断这个投影是否是合理，即灰度块残差在一个非常小的范围内
            if (ph->lastResiduals[0].first != 0 && ph->lastResiduals[0].second == ResState::IN)
            {
                // 如果残差合理就计算这个地图点在最近关键帧的逆深度和图像像素位置，分别
                // 为new_idepth， 和 （u，v）。注意每次在后端优化地图点的逆深度时，都会
                // 根据逆深度对应的hessian值自适应的给与相应的权重，表示这个深度的可信度
                // 后续的深度图滤波可以认为是一种置信度滤波的方式
                PointFrameResidual* r = ph->lastResiduals[0].first;
                assert(r->efResidual->isActive() && r->target == lastRef);
                int u = r->centerProjectedTo[0] + 0.5f;
                int v = r->centerProjectedTo[1] + 0.5f;
                float new_idepth = r->centerProjectedTo[2];
                float weight = sqrtf(1e-3 / (ph->efPoint->HdiF + 1e-12));
                
                idepth[0][u + w[0]*v] += new_idepth * weight;
                weightSums[0][u + w[0]*v] += weight;
            }
        }
    }


    for (int lvl = 1; lvl < pyrLevelsUsed; lvl++)
    {
        int lvlm1 = lvl - 1;
        int wl = w[lvl], hl = h[lvl], wlm1 = w[lvlm1];

        float* idepth_l = idepth[lvl];
        float* weightSums_l = weightSums[lvl];

        float* idepth_lm = idepth[lvlm1];
        float* weightSums_lm = weightSums[lvlm1];

        for (int y = 0; y < hl; y++)
            for (int x = 0; x < wl; x++)
            {
                int bidx = 2 * x   + 2 * y * wlm1;
                idepth_l[x + y * wl] =        idepth_lm[bidx] +
                                              idepth_lm[bidx + 1] +
                                              idepth_lm[bidx + wlm1] +
                                              idepth_lm[bidx + wlm1 + 1];

                weightSums_l[x + y * wl] =    weightSums_lm[bidx] +
                                              weightSums_lm[bidx + 1] +
                                              weightSums_lm[bidx + wlm1] +
                                              weightSums_lm[bidx + wlm1 + 1];
            }
    }


    // dilate idepth by 1.
    for (int lvl = 0; lvl < 2; lvl++)
    {
        int numIts = 1;


        for (int it = 0; it < numIts; it++)
        {
            int wh = w[lvl] * h[lvl] - w[lvl];
            int wl = w[lvl];
            float* weightSumsl = weightSums[lvl];
            float* weightSumsl_bak = weightSums_bak[lvl];
            memcpy(weightSumsl_bak, weightSumsl, w[lvl]*h[lvl]*sizeof(float));
            float* idepthl = idepth[lvl];   // dotnt need to make a temp copy of depth, since I only
            // read values with weightSumsl>0, and write ones with weightSumsl<=0.
            for (int i = w[lvl]; i < wh; i++)
            {
                if (weightSumsl_bak[i] <= 0)
                {
                    float sum = 0, num = 0, numn = 0;
                    if (weightSumsl_bak[i + 1 + wl] > 0) { sum += idepthl[i + 1 + wl]; num += weightSumsl_bak[i + 1 + wl]; numn++;}
                    if (weightSumsl_bak[i - 1 - wl] > 0) { sum += idepthl[i - 1 - wl]; num += weightSumsl_bak[i - 1 - wl]; numn++;}
                    if (weightSumsl_bak[i + wl - 1] > 0) { sum += idepthl[i + wl - 1]; num += weightSumsl_bak[i + wl - 1]; numn++;}
                    if (weightSumsl_bak[i - wl + 1] > 0) { sum += idepthl[i - wl + 1]; num += weightSumsl_bak[i - wl + 1]; numn++;}
                    if (numn > 0) {idepthl[i] = sum / numn; weightSumsl[i] = num / numn;}
                }
            }
        }
    }


    // dilate idepth by 1 (2 on lower levels).
    for (int lvl = 2; lvl < pyrLevelsUsed; lvl++)
    {
        int wh = w[lvl] * h[lvl] - w[lvl];
        int wl = w[lvl];
        float* weightSumsl = weightSums[lvl];
        float* weightSumsl_bak = weightSums_bak[lvl];
        memcpy(weightSumsl_bak, weightSumsl, w[lvl]*h[lvl]*sizeof(float));
        float* idepthl = idepth[lvl];   // dotnt need to make a temp copy of depth, since I only
        // read values with weightSumsl>0, and write ones with weightSumsl<=0.
        for (int i = w[lvl]; i < wh; i++)
        {
            if (weightSumsl_bak[i] <= 0)
            {
                float sum = 0, num = 0, numn = 0;
                if (weightSumsl_bak[i + 1] > 0) { sum += idepthl[i + 1]; num += weightSumsl_bak[i + 1]; numn++;}
                if (weightSumsl_bak[i - 1] > 0) { sum += idepthl[i - 1]; num += weightSumsl_bak[i - 1]; numn++;}
                if (weightSumsl_bak[i + wl] > 0) { sum += idepthl[i + wl]; num += weightSumsl_bak[i + wl]; numn++;}
                if (weightSumsl_bak[i - wl] > 0) { sum += idepthl[i - wl]; num += weightSumsl_bak[i - wl]; numn++;}
                if (numn > 0) {idepthl[i] = sum / numn; weightSumsl[i] = num / numn;}
            }
        }
    }


    // normalize idepths and weights.
    for (int lvl = 0; lvl < pyrLevelsUsed; lvl++)
    {
        float* weightSumsl = weightSums[lvl];
        float* idepthl = idepth[lvl];
        Eigen::Vector3f* dIRefl = lastRef->dIp[lvl];

        int wl = w[lvl], hl = h[lvl];

        int lpc_n = 0;
        float* lpc_u = pc_u[lvl];
        float* lpc_v = pc_v[lvl];
        float* lpc_idepth = pc_idepth[lvl];
        float* lpc_color = pc_color[lvl];


        for (int y = 2; y < hl - 2; y++)
            for (int x = 2; x < wl - 2; x++)
            {
                int i = x + y * wl;

                if (weightSumsl[i] > 0)
                {
                    idepthl[i] /= weightSumsl[i];
                    lpc_u[lpc_n] = x;
                    lpc_v[lpc_n] = y;
                    lpc_idepth[lpc_n] = idepthl[i];
                    lpc_color[lpc_n] = dIRefl[i][0];



                    if (!std::isfinite(lpc_color[lpc_n]) || !(idepthl[i] > 0))
                    {
                        idepthl[i] = -1;
                        continue;   // just skip if something is wrong.
                    }
                    lpc_n++;
                }
                else
                    idepthl[i] = -1;

                weightSumsl[i] = 1;
            }

        pc_n[lvl] = lpc_n;
    }

}

因为最近有点忙，代码解析后续补上