《第13讲 建图》

本文是《视觉SLAM十四讲》第13讲的个人读书笔记，为防止后期记忆遗忘写的。

---

我们之前的讨论， 基本集中于“稀疏路标地图”的部分，还没有探讨稠密地图。稀疏地图只建模感兴趣的部分，也就是前面说了很久的特征点（路标点）。而稠密地图是指，建模所有看到过的部分。对于同一个桌子，稀疏地图可能只建模了桌子 的四个角，而稠密地图则会建模整个桌面。虽然从定位(稀疏地图可以）角度看，只有四个角的地图也可以用于对相机进行定位，但由于我们无法从四个角推断这几个点之间的空间结构，所以无法仅用四个角来完成导航、避障等需要稠密地图才能完成的工作。

接下来，我们要做的是进行视觉SLAM稠密地图构建。

---

13.2 单目稠密重建

13.2.1 立体视觉

单个图像中的像素，只能提供物体与相机成像平面的角度以及物体采集到的亮度，而无法提供物体的距离（Range）。而在稠密重建，我们需要知道每一个像素点（或大部分像素点）的距离。所以在单目和多目距离测算中，多目优于单目。而单目双目的好处，是在目前 RGB-D 还无法很好应用的室外、大场景场合中，仍能通过立体视觉估计深度信息。

假定有某一段视频序列，我们通过某种魔法得到了每一帧对应的轨迹。现在我们以第一张图像为参考帧，计算参考帧中每一个 像素的深度（或者说距离）。回忆前面的三角化，发现问题并不难解决。

在稠密深度图估计中，不同之处在于，我们无法把每个像素都当作特征点，计算描述子（当然，我认为在稀疏地图上多留下准确匹配的特征点，变成准稠密也是可行的）。因此，稠密深度估计问题中，匹配就成为很重要的一环：如何确定第一张图的某 像素，出现在其他图里的位置呢？这需要用到极线搜索和块匹配技术。然后，当我们 知道了某个像素在各个图中的位置，就能像特征点那样，利用三角测量确定它的深度。不 过不同的是，在这里我们要使用很多次三角测量让深度估计收敛，而不仅是一次。我们希望深度估计，能够随着测量的增加，从一个非常不确定的量，逐渐收敛到一个稳定值。这就是深度滤波器技术。

---

13.2.2 极线搜索与块匹配

两个相机间的运动时，这条极线也是能够确定的。那么问题就是：极线上的哪一个点，是我们刚才看到的 p1 点呢？

一种直观的想法是：既然单个像素的亮度没有区分性，那是否可以比较像素块呢？我们在 p1 周围取一个大小为 w × w 的小块，然后在极线上也取很多同样大小的小块进行比 较，就可以一定程度上提高区分性。这就是所谓的块匹配。注意到在这个过程中，只有我们的假设在不同图像间整个小块的灰度值不变，这种比较才有意义。所以算法的假设，从像素的灰度不变性，变成了图像块的灰度不变性——在一定程度上变得更强了。

如何计算小块与小块间的差异呢？有很多种方法

现在，我们在极线上，计算了 A 与每一个 Bi 的相似性度量。为了方便叙述，假设我 们用了 NCC，那么，我们将得到一个沿着极线的 NCC 分布。在这种情况下，我们会倾向于使用概率分布来描述深度值，而非用某个单一个的数值来描述深度。于是，我们的问题就转到了，在不断对不同图像进行极线搜索时，我们估计的深度分布将发生怎样的 变化——这就是所谓的深度滤波器。

虽然非线性优化效果较好，但是在 SLAM 这种实时性要求较强的场合，考虑到前端已经占据了不少的计算量，建图方面则通常采用计算量较少的滤波器方式了。这里的滤波器，也就是深度滤波器。

---

13.2.3 高斯分布的深度滤波器

设某个像素点的深度 d 服从：（先验）。

每当新的数据到来，我们都会观测到它的深度。同样的，假设这次观测亦是一个高斯分布：（似然）

我们的问题是，如何使用观测的信息，更新原先 d 的分布。这正是一个信息融合问题（贝叶斯问题）。设融合后的 d 的分布为   那么根据高斯分布的乘积，有：

那么现在，如何确定我们观测到深度的分布，也就是如何计算 µobs, σobs 的问题。

为了解决上面的求值问题，我们现在则仅考虑几何不确定性。

考虑极线 l2 上存在着 一个像素大小的误差，使得 β 角变成了 β ′，而 p 也变成了 p ′，并记上面那个角为 γ。我 们要问的是，这一个像素的误差，会导致 p ′ 与 p 产生多大的差距呢？显然有：

对 p2 扰动一个像素，将使得 β 产生一个变化量 δβ，由于相机焦距为 f，于是：

 

单目稠密地图构建代码，详见书本和十四讲作者高博github。

在实地运行代码过程中，其实程序存在很多问题，其实程序本身的思路是有点局限或者不是很灵活的特点。书本上程序演示的是估算第一帧图像每个像素对应的深度值，即进行单目稠密重建。也就是上百多张的图片，就只为了构建第一张照片的地图。除了第1张图之外，其他的图片只为了不断进行极线搜素修正深度。显然这是不合理的，代价太高。（教学过程是很好地学习例子，实际应用中延时巨大）。

在进行块匹配（和 NCC 的计算）时，我们必须假设小块不变，然后将该小块与其他小块进行对比。这时，有明显梯度的小块将具有良好的区分度，不易引起误匹配。对于梯度不明显的像素，由于在块匹配 时没有区分性，所以我们将难以有效地估计其深度。反之，像素梯度比较明显的地方，我们 得到的深度信息也相对准确，例如桌面上的杂志、电话等具有明显纹理的物体。因此，演示程序反映了立体视觉中一个非常常见的问题：对物体纹理的依赖性。该问题在双目视觉中也极其常见，体现了立体视觉的重建质量，十分依赖于环境纹理。

13.5 RGB-D 稠密建图

RGB-D 相机优点：在 RGB-D 相机中可以完全通过传感器中硬 件测量得到，无需消耗大量的计算资源来估计它们。并且，RGB-D 的结构光或飞时原理， 保证了深度数据对纹理的无关性。即使面对纯色的物体，只要它能够反射光，我们就能测量到它的深度。这亦是 RGB-D 传感器的一大优势。

在地图构建中，根据应用的不同，可采用不同的地图形式。最直观最简单的方法，就是根据估算的相机位姿，将 RGB-D 数 据转化为点云（Point Cloud），然后进行拼接，最后得到一个由离散的点组成的点云地图 （Point Cloud Map）。

第五讲的例子主要是为了让读者理解相 机的内外参，而在实际建图当中，我们还会对点云加一些滤波处理，获得更好的视觉效果。 在本程序中，我们主要使用两种滤波器：外点去除滤波器以及降采样滤波器。

滤波器关键代码如下：

 // voxel filter 
    pcl::VoxelGrid<PointT> voxel_filter; 
    voxel_filter.setLeafSize( 0.01, 0.01, 0.01 );       // resolution 
    PointCloud::Ptr tmp ( new PointCloud );
    voxel_filter.setInputCloud( pointCloud );
    voxel_filter.filter( *tmp );
    tmp->swap( *pointCloud );
    
    cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl;
    
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );

运行的效果

其实，数据中的图片只有5张，所以提取出的点云规模不大。但是当图片超过50张或者更多，那么点云规模大。如果没有进行滤波处理，那么点云文件会达到数几百兆，打开都是一件难事。然而滤波之后，只需十几兆便可解决（亲身经历）。

相对于第5讲，思路没有太大变化，主要不同之处在于：

1. 在生成每帧点云时，去掉深度值太大或无效的点。这主要是考虑到 Kinect 的有效量程，超过量程之后的深度值会有较大误差。
2. 利用统计滤波器方法去除孤立点。该滤波器统计每个点与它最近 N 个点的距离值的 分布，去除距离均值过大的点。这样，我们保留了那些“粘在一起”的点，去掉了孤 立的噪声点。
3. 最后，利用体素滤波器（Voxel Filter）进行降采样。由于多个视角存在视野重叠，在 重叠区域会存在大量的位置十分相近的点。这会无益地占用许多内存空间。体素滤波 保证在某个一定大小的立方体（或称体素）内仅有一个点，相当于对三维空间进行了 降采样，从而节省了很多存储空间。

可以看到在滤波前存在着由噪声产生的许多孤 立的点。经过统计外点去除之后，我们消去了这些噪声，使得整个地图变得更干净。另一 方面，我们在体素滤波器中，把分辨率调至 0.01，表示每立方厘米有一个点。这是一个比较高的分辨率，所以在截图中我们感觉不出地图的差异，然而程序输出中可以看到点数明 显减少了许多（从 90 万个点减到了 44 万个点，去除了一半左右）。

点云地图为我们提供了比较基本的可视化地图，让我们能够大致了解环境的样子。点 云的一大优势是可以直接由 RGB-D 图像高效地生成，不需要额外的处理。但是，点云地图是“基础”的或“初级的”，是指它更接近于传感器读取的 原始数据。它具有一些基本的功能，但通常用于调试和基本的显示，不便直接用于应用程序。所以。很多具体用处的地图，可以在生成的点云上进行改造。  

---

13.5.2 八叉树地图

点云的缺陷：

1. 点云地图通常规模很大，所以一个 pcd 文件也会很大。一张 640×480 的图像，会产 生 30 万个空间点，需要大量的存储空间。即使经过一些滤波之后，pcd 文件也是很 大的。点云地图提供了很多不必要的 细节。对于地毯上的褶皱、阴暗处的影子，我们并不特别关心这些东西。把它们放在 地图里是浪费空间。
2. 点云地图无法处理运动物体。因为我们的做法里只有“添加点”，而没有“当点消失 时把它移除”的做法。而在实际环境中，运动物体的普遍存在，使得点云地图变得不 够实用。

八叉树地图：可以很大程度上解决点云的上述问题，有较好的压缩性能的地图形式。是一种灵活的、压缩的、又能随时更新的地图形式。

八叉树原理

把三维空间建模为许多个小方块（或体素），是一种常见的做法。如果我 们把一个小方块的每个面平均切成两片，那么这个小方块就会变成同样大小的八个小方块。 这个步骤可以不断的重复，直到最后的方块大小达到建模的最高精度。在这个过程中，把 “将一个小方块分成同样大小的八个”这件事，看成“从一个节点展开成八个子节点”，那 么，整个从最大空间细分到最小空间的过程，就是一棵八叉树（Octo-tree）。

 

于是，整个大方块可以看成是根节点，而最小的块可以看作是“叶子节 点”。于是，在八叉树中，当我们由下一层节点往上走一层时，地图的体积就能扩大八倍。

点云占空间的原因

在点云的体素滤波器中，我们不是也限制了一个体素中只有一个点 吗？为何我们说点云占体积，而八叉树比较节省空间呢？这是因为，在八叉树中，我们在节 点中存储它是否被占据的信息。然而，不一样之处，在于当某个方块的所有子节点都被占 据或都不被占据时，就没必要展开这个节点。例如，一开始地图为空白时，我们就只需一个 根节点，而不需要完整的树。当在地图中添加信息时，由于实际的物体经常连在一起，空 白的地方也会常常连在一起，所以大多数八叉树节点都无需展开到叶子层面。所以说，八 叉树比点云节省了大量的存储空间。

八叉树存点的形式：(数理过程，书上描述很详尽，这里不再断章取义了。直接断章，呵呵）

 代码：

这里演示的是不带颜色的八叉树地图的构建。实际上 octomap 提供了许多种八叉树：有带地图的，有带占据信息的，你也可以自己定义每个节点需要携带哪些变量。我们使用了不带颜色信息的，最基本的八叉树地图。

比较之前的点云地图，其实代码变化不大，只是将点的存储形式换了不同的存储类对象（结构）中。关键的代码如下：

    octomap::OcTree tree( 0.005 ); // 参数为分辨率
    
    for ( int i=0; i<5; i++ )
    {
        cout<<"转换图像中: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        
        octomap::Pointcloud cloud;  // the point cloud in octomap 
        
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
                Eigen::Vector3d point; 
                point[2] = double(d)/depthScale; 
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;
                // 将世界坐标系的点放入点云
                cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] ); 
            } 
            
        // 将点云存入八叉树地图，给定原点，这样可以计算投射线
        tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) );     
    }
    
    // 更新中间节点的占据信息并写入磁盘
    tree.updateInnerOccupancy();
    cout<<"saving octomap ... "<<endl;
    tree.writeBinary( "octomap.bt" );

它比 PCL 的点云稍微简单一些，只携带点的空间 位置信息。我们根据 RGB-D 图像和相机位姿信息，先将点的坐标转至世界坐标，然后放入 octomap 的点云，最后交给八叉树地图。之后，octomap 会根据之前介绍的投影信息，更 新内部的占据概率，最后保存成压缩后的八叉树地图。

由于我们构造时使用的 默认深度是 16 层，所以这里显示 16 层的话即最高分辨率，也就是每个小块的边长为 0.05 米。当我们将深度减少一层时，八叉树的叶子节点往上提了一层，每个小块的边长就增加 两倍，变成 0.1 米。可以看到，我们能够很容易地调节地图分辨率以适应不同的场合。