系统概述

​ 我们采取主流的稠密SLAM系统那样tracking 和 mapping 的结构，主要使用CUDA来实现tracking部分，OpenGL Shading语言来实现地图预测和管理。我们的方法可以实时的使用标准的RGB-D相机来估计一个环境的稠密3D地图，下面为我们方法的关键部分。

1. 受到参考文献9的启发， 我们基于融合面元的模型来估计一个环境。

2. 当在最近经常观察的区域（active area）进行跟踪和数据融合时，将超过${\delta }_t$时间没有被观察到的区域转成inactive area，且这一部分不用来跟踪和数据融合。

3. 每一帧都尝试将当前估计的相机帧内的活动模型部分与位于同一帧内的非活动模型部分进行配准，如果配准成功，那么当前帧就与旧的不活跃的模型形成回环，然后整个模型将会非刚性变换，这旧的不活跃的模型将重新激活。

4. 对于全局回环检测，添加场景的预测视图到一个随机数据库中，每一帧都通过这个数据库来找匹配的预测视图。如果检测到匹配，将视图配准到一起，并检查这一次配准是否与模型的集合形状全局一致，如果是，在地图中用非刚性变换来反映这个配准，并且将它全局对齐。

用有颜色的表面来表示活跃的模型，灰色区域表示非活跃的模型。

1. 初始时，所有的数据都是活跃模型，并且向左探索；

2. 随着时间推移，最近观察不到的区域被设置为非活跃状态；

3. 相机重新观察到了之前非活跃的区域，形成闭环并且配准表面，之前非活跃的区域重新激活；

4. 相机继续向右探索，形成了更多的闭环；

5. 继续探索新的区域；

6. 相机重新访问到非活跃的区域，但出现了漂移；

7. 说明图6的漂移不对齐，这里的红色箭头是活跃区域的点指向非活跃区域的点，这两点是等价的；

8. 一次全局闭环触发，对齐活跃与非活跃模型；

9. 继续向右探索，形成更多的局部回环，跟更多非活跃区域重新激活；

10. 最终全部地图着色？？

融合预测跟踪

​ 我们的场景表示是一个无序列表的surfels ${\mathcal{M}}^s$ ，（与文献9的表示方式类似）

$$\{\begin{array}{l}p\in {\mathbb{R}}^3,\mathrm{点}\mathrm{的}\mathrm{位}\mathrm{置}\mathrm{信}\mathrm{息}\\ n\in {\mathbb{R}}^3,\mathrm{法}\mathrm{向}\mathrm{量}\\ c\in {\mathbb{N}}^3,\mathrm{颜}\mathrm{色}\mathrm{信}\mathrm{息}(R,G,B)\\ w\in \mathbb{R},\mathrm{权}\mathrm{重}\\ r\in \mathbb{R},\mathrm{面}\mathrm{片}\mathrm{的}\mathrm{半}\mathrm{径}\\ t,\mathrm{点}\mathrm{的}\mathrm{获}\mathrm{取}\mathrm{时}\mathrm{间}\mathrm{信}\mathrm{息}\end{array}$$

然而，当使用地图进行姿态估计时，我们的方法有两个不同之处

​ 1. 不是仅通过用于几何帧到模型跟踪的碎片渲染来预测深度图，我们还预测模型面的全彩色碎片渲染以执行光度帧到模型跟踪；

​ 2. 我们定义了一个时间窗口阈值 ${\delta }_t$，它将 $\mathcal{M}$ 划分为活跃和不活跃的面元。 只有标记为活动模型面元的面元才用于相机姿态估计和深度图融合。 当 $\mathcal{M}$ 中的一个面元自上次更新（即具有与其关联的原始深度测量以进行融合）以来的时间大于 ${\delta }_t$ 时，该面元被声明为非活动状态。

​ 我们描述了我们的联合光度和几何姿态估计的方法，这些方法来自一个的面元预测。

​ 我们定义图片域是$\mathrm{\Omega }\subset {\mathbb{N}}^2$, 深度图，像素，颜色...对每一张深度图计算法线图。$p(u,\mathcal{D})=K^{-1}vd(u)$, $v$是$u$的齐次形式，$d(u)$是$u$的深度。对于时间t的每个输入帧，我们通过当前实时输入的深度图和彩色图，与前一个位姿的活跃模型的预测深度图和彩色图，配准，来估计相机的位姿，这里的位姿用李群T来表示。

几何位姿估计

​ 在当前实时深度图${\mathcal{D}}_t^l$ 与 来自最后一帧 ${\mathcal{D}}_{t-1}^a$ 的预测的活动深度图模型之间，我们旨在找一个$\xi$ 使得最小化投影误差。

$$E_{icp}=\sum _k((v^k-\exp (\hat{\xi })T{v}_t^k)\cdot n^k{)}^2$$

其中，$v_t^k$是在${\mathcal{D}}_{\mathcal{t}}^{\mathcal{l}}$中第k个顶点的相机坐标，$v^k$和$n^k$是上一时刻对应顶点的相机坐标和法线。$T$是从前一个相机位姿到当前位姿变换的估计，（$\exp (\hat{\xi })$是一个增量）。

亮度位姿估计

​ 在当前实时的颜色图${\mathcal{C}}_{\mathcal{t}}^{\mathcal{l}}$ 和 上一时刻 ${\hat{\mathcal{C}}}_{\mathcal{t}\mathcal{-}\mathcal{1}}^{\mathcal{a}}$ 的预测活跃颜色模型之间，我们旨在找一个$\xi$ 使得亮度误差最小。

$$E_{rgb}=\sum _{u\in \mathrm{\Omega }}(I(u,{\mathcal{C}}_{\mathcal{t}}^{\mathcal{l}})-I(\pi (K\exp (\hat{\xi })Tp(u,{\mathcal{D}}_{\mathcal{t}}^{\mathcal{l}})，{\hat{\mathcal{C}}}_{\mathcal{t}\mathcal{-}\mathcal{1}}^{\mathcal{a}}{)}^2$$

​ 这里的 $T$ 同上，注意这里的 p 存在维数的变换。

联合优化

$$E_{track}=Eicp+w_{rgb}{E}_{rgb}$$

​ 我们使用高斯牛顿法非线性最小二乘法 与 三层金字塔 来优化该目标函数。为了解决每次迭代，我们计算最小二乘解

$$\underset{\xi }{argmin}{\Vert J\xi +r\Vert }^2$$

产生改进的相机变换估计

$$\begin{gathered} T^{{}^{\prime }}=\exp (\hat{\xi })T \\ \hat{\xi }=\left[\begin{array}{cccc}& [w{]}_{\times }& & x\\ 0& 0& 0& 0\end{array}\right] \end{gathered}$$

​ 可以填充组合测量雅克比矩阵 J 和残差 r 的块（同时根据 $w_{rgb}$ 进行加权）并使用高度并行的树缩减 CUDA 求解以生成 6×6 法线方程组，然后由 Cholesky 在 CPU 上分解得到 $\xi$ ，这个过程的结果是最新的相机位姿估计$p_t=T{P}_{t-1}$ ，它使得实时相机 ${\mathcal{D}}_t^l$ 和 ${\mathcal{C}}_t^l$ 当前活跃模型强对齐。

变形图

​ 为了确保地图中的局部和全局表面一致性，我们在面元 $\mathcal{M}$ 集合中反映了成功的表面环闭合，这是通过闭环提供的表面约束对所有面元（活跃和非活跃）进行非刚性变换。我们采用嵌入变形技术的空间变形方法。

​ 变形图由分布在要变形的模型中的一组节点和边组成。每个节点${\mathcal{G}}^{\mathcal{n}}$有，时间戳${\mathcal{G}}_{{\mathcal{t}}_{\mathcal{0}}}^{\mathcal{n}}$，位置${\mathcal{G}}_{\mathcal{g}}^{\mathcal{n}}$和一组邻居节点$\mathcal{N}\left({\mathcal{G}}^{\mathcal{n}}\right)$，放射变换${\mathcal{G}}_{\mathcal{R}}^{\mathcal{n}}$和${\mathcal{G}}_{\mathcal{t}}^{\mathcal{n}}$，每个节点的邻居构成了图的（有向）边，一般邻居个数为4。

​ 面元的变形后的位置由下式给出

$${\hat{\mathcal{M}}}_{\mathbf{p}}^s=\varphi \left({\mathcal{M}}^s\right)=\sum _{n\in \mathcal{I}({\mathcal{M}}^s,\mathcal{G})}{w}^n\left({\mathcal{M}}^s\right)[{\mathcal{G}}_{\mathbf{R}}^n({\mathcal{M}}_{\mathbf{p}}^s-{\mathcal{G}}_{\mathbf{g}}^n)+{\mathcal{G}}_{\mathbf{g}}^n+{\mathcal{G}}_{\mathbf{t}}^n]$$

​ 面元变形后的法线由下式给出

$${\hat{\mathcal{M}}}_{\mathbf{n}}^s=\sum _{n\in \mathcal{I}({\mathcal{M}}^s,\mathcal{G})}{w}^n\left({\mathcal{M}}^s\right){\mathcal{G}}_{\mathbf{R}}^{n-1^{\mathrm{\top }}}{\mathcal{M}}_{\mathbf{n}}^s$$

​ 其中 ${\omega }^n\left({\mathcal{M}}^s\right)$ 是一个标量，表示节点${\mathcal{G}}^{\mathcal{n}}$对面元${\mathcal{M}}^{\mathcal{s}}$的影响，当 n = k 时，总和为 1。

$$w^n\left({\mathcal{M}}^s\right)={(1-{\Vert {\mathcal{M}}_{\mathbf{p}}^s-{\mathcal{G}}_{\mathbf{g}}^n\Vert }_2/d_{max})}^2$$

​ 这里$d_{max}$ 是到${\mathcal{M}}^{\mathcal{s}}$的 k + 1 - 最近节点的欧几里得距离。

​ 在下文中，我们描述了我们从一组面元 $\mathcal{M}$ 中采样变形图 $\mathcal{G}$的方法以及我们确定图连通性的方法。

构造

​ 每次初始化新的 Deformation Graph，每次初始化新的 Deformation Graph 要比保持更新同一个 graph 计算量小而且还要简单可行。初始时，从面元中系统抽样（均匀分布）节点，节点的位置和时间戳，都为面元的位置和时间戳。(后面的不懂)

应用

​ 为了应用优化后的变形图（在下一节中详细说明）来更新地图，必须确定影响每个面元${\mathcal{M}}^{\mathcal{s}}$的节点集。算法 1 中列出了实现 I(M s , G) 并将变形图 G 应用于给定面元的算法。当每个面元变形时，会在整个变形节点集合中搜索时间上最近的节点。这个 L 1 -范数最小化的解决方案实际上是对集合 G 的二分搜索，因为它已经是有序的。 从这里开始，收集时间上附近的其他节点，并选择最近的 k 个节点（在欧几里德距离意义上）作为 I(M s , G)。最后，每个节点的权重按照公式 10 计算，并应用公式 8 和 9 的变换。 更新后的面元 M̂ s 的所有其他属性均从 M s 复制。

​ 算法1描述为：

  	1.  $\alpha$ number of nodes to explore;
  	2.  Find closest node in time;
  	3.  Gather set of temporally nearby nodes $-\alpha/2$  to  $\alpha/2$ ;
  	4.  Take closest k as influencing nodes;
  	5.  Compute weights;
  	6.  Apply transformations.

优化

​ 给定一组表面对应 Q，可以优化变形图的参数以反映表面模型 M 中的表面配准。以下定义四个损失函数

​ 第一个最大化变形刚度：

$$E_{rot}=\sum _l{\Vert {\mathcal{G}}_{\mathbf{R}}^l{}^{\mathrm{\top }}{\mathcal{G}}_{\mathbf{R}}^l-\mathbf{I}\Vert }_F^2$$

​ 使用 Frobenius 范数。 第二个是正则化项，可确保整个图的平滑变形：

$$E_{\text{reg }}=\sum _l\sum _{n\in \mathcal{N}\left({\mathcal{G}}^l\right)}{\Vert {\mathcal{G}}_{\mathbf{R}}^l({\mathcal{G}}_{\mathbf{g}}^n-{\mathcal{G}}_{\mathbf{g}}^l)+{\mathcal{G}}_{\mathbf{g}}^l+{\mathcal{G}}_{\mathbf{t}}^l-({\mathcal{G}}_{\mathbf{g}}^n+{\mathcal{G}}_{\mathbf{t}}^n)\Vert }_2^2$$

​ 第三个是最小化位置约束 Q 上的误差的约束项：

$$E_{con}=\sum _p{\Vert \varphi \left({\mathcal{Q}}_{\mathrm{s}}^p\right)-{\mathcal{Q}}_{\mathrm{d}}^p\Vert }_2^2$$

​ 这些表面可以多次通过非刚性变形对齐，从而将新的数据融合到地图中已经访问过的区域。最后的误差函数保证非活跃区域固定，将活跃区域变形到非活跃区域的坐标系中：

$$E_{pin}=\sum _p{\Vert \varphi \left({\mathcal{Q}}_{\mathrm{d}}^p\right)-{\mathcal{Q}}_{\mathrm{d}}^p\Vert }_2^2$$

​ 最终的损失函数为：

$$E_{def}=w_{rot}{E}_{rot}+w_{reg}{E}_{reg}+w_{con}{E}_{con}+w_{con}{E}_{pin}$$

​ 我们使用高斯牛顿法迭代算误差函数最小的R和t，这个问题的雅克比矩阵是稀疏的，因此我们使用Cholesky分解来使有效地解决CPU系统上的问题。这里变形图使用GPU来计算，应用到整个表面贴图。

局部回环

​ 为了确保整个地图的局部表面一致性，我们的系统在重新访问这些区域时会形成很多小的回环，我们融合到模型的活动区域，同时逐渐将一段时间内未出现的面元标记为不活动。非活跃区域不参与跟踪和融合过程，直到在活跃区域和非活跃区域形成回环，匹配到的非活跃区域中的点转变称活跃。

​ 将地图中的一组面元分为活跃区域和非活跃区域，如果全局回环检测没有检测到，那么就尝试计算这两个区域的匹配度。

​ 验证这次匹配是否有质量，最后添加一个条件，要让$E_{track}$足够小，同时还要检查海森矩阵的特征值要小于一个阈值。

​ 如果实现了高质量的对齐，我们会生成一组表面约束Q，将他输入到上面优化过程中，进行对齐。我们均匀地采样一组像素坐标 U ⊂ Ω 来计算集合 Q。对于每个像素 u ∈ U 我们填充一个约束：

$${\mathcal{Q}}^p=(\left({\mathbf{H}\mathbf{P}}_t\right)\mathbf{p}(\mathbf{u},{\mathcal{D}}_t^a);{\mathbf{P}}_t\mathbf{p}(\mathbf{u},{\mathcal{D}}_t^a);{\mathcal{T}}_t^i(\mathbf{u});t)$$

​ 变形发生后，新的最新相机位姿解析为 P̂ t = HP t 。此时，作为对齐一部分的一组面元被重新激活，以允许实时摄像机跟踪并与现有的活动面元融合。活动模型深度的最新预测必须渲染以反映非活动面的深度测试的变形， 对于每个面元 M s ：

$${\mathcal{M}}_t^s=\{\begin{array}{ll}t& \text{ if }\mathit{\pi }\left(\mathbf{K}{\hat{\mathbf{P}}}_t^{-1}{\mathcal{M}}_{\mathbf{p}}^s\right)\in \mathrm{\Omega }\\ & \text{ and }{\left(\mathbf{K}{\hat{\mathbf{P}}}_t^{-1}{\mathcal{M}}_{\mathbf{p}}^s\right)}_z\lesssim {\tilde{\mathcal{D}}}_t^a\left(\mathit{\pi }\left(\mathbf{K}{\hat{\mathbf{P}}}_t^{-1}{\mathcal{M}}_{\mathbf{p}}^s\right)\right)\\ {\mathcal{M}}_t^s& \text{ else. }\end{array}$$

​ 本节中描述的过程使模型的活动区域与模型的非活动区域紧密对齐，以实现紧密的局部表面环闭合。活动模型与非活动模型漂移太远，导致局部对齐无法收敛。我们采用基于外观的全局闭环方法来引导表面变形它将活动模型与底层非活动模型重新对齐，以获得紧密的全局闭环和表面全局一致性。

全局回环

​ 我们利用Fern编码方法进行基于外观的位置识别，Ferns 将 RGB-D 图像编码为一串代码，由一组固定像素位置中每个 RGB-D 通道的二进制测试值组成。和上面一样，如果匹配成功，添加一组表面约束Q作为变形的输入，其中u是一个随机采样的Fern编码的像素位置：

$${\mathcal{Q}}^p=(\left(\mathbf{H}{\mathcal{E}}_{\mathbf{P}}^i\right)\mathbf{p}(\mathbf{u},{\mathcal{D}}_t^a);{\mathbf{P}}_t\mathbf{p}(\mathbf{u},{\mathcal{D}}_t^a);{\mathcal{E}}_t^i;t)$$

​ 这里的回环也需要进行检验$E_{con}$，这里的变形不修改活跃和非活跃面元组。正确的全局闭环使地图的活动和非活动区域足够接近对齐，以在下一帧触发局部闭环，并且这允许地图从可能不正确的全局闭环中恢复。如果相机跟踪失败，我们还可以选择依赖Fern编码数据库进行全局重新定位。

全局回环代码部分

// 随机初始化 Ferns 的位置，像素通道和阈值大小
void Ferns::generateFerns()
{
    for(int i = 0; i < num; i++)
    {
        Fern f;

        //随机初始化 Fern 的位置
        f.pos(0) = widthDist(random);
        f.pos(1) = heightDist(random);

        // 随机初始化每个通道的阈值
        f.rgbd(0) = rgbDist(random);
        f.rgbd(1) = rgbDist(random);
        f.rgbd(2) = rgbDist(random);
        f.rgbd(3) = dDist(random);

        conservatory.push_back(f);
    }
}

//新输入的帧和以前帧进行比较，如果相似度小于一定阈值，则将当前帧插入作为回环检测的关键帧
bool Ferns::addFrame(GPUTexture * imageTexture, GPUTexture * vertexTexture, GPUTexture * normalTexture, const Eigen::Matrix4f & pose, int srcTime, const float threshold)
{
    Img<Eigen::Matrix<unsigned char, 3, 1>> img(height, width);
    Img<Eigen::Vector4f> verts(height, width);
    Img<Eigen::Vector4f> norms(height, width);

    resize.image(imageTexture, img);
    resize.vertex(vertexTexture, verts);
    resize.vertex(normalTexture, norms);

    Frame * frame = new Frame(num,
                              frames.size(),
                              pose,
                              srcTime,
                              width * height,
                              (unsigned char *)img.data,
                              (Eigen::Vector4f *)verts.data,
                              (Eigen::Vector4f *)norms.data);

    int * coOccurrences = new int[frames.size()];

    memset(coOccurrences, 0, sizeof(int) * frames.size());

    for(int i = 0; i < num; i++)
    {
        unsigned char code = badCode;

        if(verts.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) > 0)
        {
            const Eigen::Matrix<unsigned char, 3, 1> & pix = img.at<Eigen::Matrix<unsigned char, 3, 1>>(conservatory.at(i).pos(1), conservatory.at(i).pos(0));

            //随机选取像素点处的编码
            code = (pix(0) > conservatory.at(i).rgbd(0)) << 3 |
                   (pix(1) > conservatory.at(i).rgbd(1)) << 2 |
                   (pix(2) > conservatory.at(i).rgbd(2)) << 1 |
                   (int(verts.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) * 1000.0f) > conservatory.at(i).rgbd(3));

            frame->goodCodes++;

            // 计算和以前存储帧之间的相似度
            for(size_t j = 0; j < conservatory.at(i).ids[code].size(); j++)
            {
                coOccurrences[conservatory.at(i).ids[code].at(j)]++;
            }
        }

        frame->codes[i] = code;
    }

    float minimum = std::numeric_limits<float>::max();

    if(frame->goodCodes > 0)
    {
        for(size_t i = 0; i < frames.size(); i++)
        {
            float maxCo = std::min(frame->goodCodes, frames.at(i)->goodCodes);

            float dissim = (float)(maxCo - coOccurrences[i]) / (float)maxCo;

            if(dissim < minimum)
            {
                minimum = dissim;
            }
        }
    }

    delete [] coOccurrences;

    if((minimum > threshold || frames.size() == 0) && frame->goodCodes > 0)
    {
        for(int i = 0; i < num; i++)
        {
            if(frame->codes[i] != badCode)
            {
                //conservatory 存储关系：第一层 fern 的编号，第二层 code 的编号，第三层图像帧的编号
                conservatory.at(i).ids[frame->codes[i]].push_back(frame->id);
            }
        }

        frames.push_back(frame);

        return true;
    }
    else
    {
        delete frame;

        return false;
    }
}

// 对于输入的图像进行全局的回环检测，通过判断和以前存储的帧编码相似度，判断当前帧是否作为关键帧
Eigen::Matrix4f Ferns::findFrame(std::vector<SurfaceConstraint> & constraints,
                                 const Eigen::Matrix4f & currPose,
                                 GPUTexture * vertexTexture,
                                 GPUTexture * normalTexture,
                                 GPUTexture * imageTexture,
                                 const int time,
                                 const bool lost)
{
    lastClosest = -1;

    Img<Eigen::Matrix<unsigned char, 3, 1>> imgSmall(height, width);
    Img<Eigen::Vector4f> vertSmall(height, width);
    Img<Eigen::Vector4f> normSmall(height, width);

    //对输入图像降采样 8X8 倍
    resize.image(imageTexture, imgSmall);
    resize.vertex(vertexTexture, vertSmall);
    resize.vertex(normalTexture, normSmall);

    Frame * frame = new Frame(num, 0, Eigen::Matrix4f::Identity(), 0, width * height);

    int * coOccurrences = new int[frames.size()];

    memset(coOccurrences, 0, sizeof(int) * frames.size());

    for(int i = 0; i < num; i++)
    {
        unsigned char code = badCode;

        if(vertSmall.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) > 0)
        {
            const Eigen::Matrix<unsigned char, 3, 1> & pix = imgSmall.at<Eigen::Matrix<unsigned char, 3, 1>>(conservatory.at(i).pos(1), conservatory.at(i).pos(0));

            //指定像素点处做编码
            code = (pix(0) > conservatory.at(i).rgbd(0)) << 3 |
                   (pix(1) > conservatory.at(i).rgbd(1)) << 2 |
                   (pix(2) > conservatory.at(i).rgbd(2)) << 1 |
                   (int(vertSmall.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) * 1000.0f) > conservatory.at(i).rgbd(3));

            frame->goodCodes++;

            // conservatory.at(i) 表示第 i 个 fern 总共随机采样了 num 个
            // conservatory.at(i).ids[code] 表示第 i 个 fern 编码值为 code 的 vector
            // conservatory.at(i).ids[code].at(j) 值表示第 at(j) 帧图像
            for(size_t j = 0; j < conservatory.at(i).ids[code].size(); j++)
            {
                // coOccurrences[conservatory.at(i).ids[code].at(j)] 表示和第 conservatory.at(i).ids[code].at(j) 帧图像的相似度
                coOccurrences[conservatory.at(i).ids[code].at(j)]++;
            }
        }

        frame->codes[i] = code;
    }

    float minimum = std::numeric_limits<float>::max();
    int minId = -1;

    //在所有帧中找相似度最小的帧
    for(size_t i = 0; i < frames.size(); i++)
    {
        float maxCo = std::min(frame->goodCodes, frames.at(i)->goodCodes);

        float dissim = (float)(maxCo - coOccurrences[i]) / (float)maxCo;

        if(dissim < minimum && time - frames.at(i)->srcTime > 300)
        {
            minimum = dissim;
            minId = i;
        }
    }

//根据图像的编码计算两帧之间的相似度
float Ferns::blockHDAware(const Frame * f1, const Frame * f2)
{
    int count = 0;
    float val = 0;

    for(int i = 0; i < num; i++)
    {
        if(f1->codes[i] != badCode && f2->codes[i] != badCode)
        {
            count++;

            if(f1->codes[i] == f2->codes[i])
            {
                val += 1.0f;
            }
        }
    }

    return val / (float)count;
}