1. 点云地图缺陷

在点云地图中，虽然有了三维结构，亦进行了体素滤波以调整分辨率
但是点云有几个明显的缺陷：

1. 点云地图通常规模很大，所以一个 pcd 文件也会很大
   一张 640×480 的图像，会产生 30 万个空间点，需要大量的存储空间
   即使经过一些滤波之后， pcd 文件也是很大的
   而且讨厌之处在于，它的“大”并不是必需的，点云地图提供了很多不必要的细节
   对于地毯上的褶皱、阴暗处的影子，并不特别关心这些东西，把它们放在地图里是浪费空间
   由于这些空间的占用，除非降低分辨率，否则在有限的内存中，无法建模较大的环境
   然而降低分辨率会导致地图质量下降，可以通过某些方式对地图进行压缩地存储，舍弃一些重复的信息
2. 点云地图无法处理运动物体
   因为做法里只有“添加点”，而没有“当点消失时把它移除”的做法
   而在实际环境中，运动物体的普遍存在，使得点云地图变得不够实用

2. 八叉树

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

左侧显示了一个大立方体不断地均匀分成八块，直到变成最小的方块为止
于是，整个大方块可以看成是根节点，而最小的块可以看作是“叶子节点”
于是，在八叉树中，当由下一层节点往上走一层时，地图的体积就能扩大八倍

不妨做一点简单的计算：如果叶子节点的方块大小为 1 cm³
那么当我们限制八叉树为 10 层时，总共能建模的体积大约为 8¹⁰ = 1073 m³，这足够建模一间屋子
由于体积与深度成指数关系，所以当用更大的深度时，建模的体积会增长的非常快

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

前面说八叉树的节点存储了它是否被占据的信息
从点云层面来讲，自然可以用0 表示空白， 1 表示被占据
这种 0-1 的表示可以用一个比特来存储，节省空间，不过显得有些过于简单了
由于噪声的影响，可能会看到某个点一会为 0，一会儿为 1；或者大部分时刻为 0，小部分时刻为 1；或者除了“是、否”两种情况之外，还有一个“未知”的状态

能否更精细地描述这件事呢？
会选择用概率形式表达某节点是否被占据的事情
比方说，用一个浮点数 x $\in$ [0, 1] 来表达
这个 x 一开始取 0.5，如果不断观测到它被占据，那么让这个值不断增加
反之，如果不断观测到它是空白，那就让它不断减小即可
通过这种方式，动态地建模了地图中的障碍物信息

不过，现在的方式有一点小问题：
如果让 x 不断增加或减小，它可能跑到 [0, 1] 区间之外，带来处理上的不便
所以不是直接用概率来描述某节点被占据，而是用概率对数值（Log-odds）来描述
设 y $\in$ R为概率对数值， x 为 0 到 1 之间的概率，那么它们之间的变换由 $logit$ 变换描述：

其反变换为：

可以看到，当 y 从 −1 变到 +1 时， x 相应地从 0 变到了 1，而当 y 取 0 时， x 取到 0.5
因此，不妨存储 y 来表达节点是否被占据
当不断观测到“占据”时，让 y增加一个值；否则就让 y 减小一个值。

当查询概率时，再用逆 $logit$ 变换，将 y 转换至概率即可
用数学形式来说，设某节点为 n，观测数据为 z
那么从开始到 t 时刻某节点的概率对数值为 $L$( $n$| $z$_1:t)，那么 t + 1 时刻为：

如果写成概率形式而不是概率对数形式，就会有一点复杂：

有了对数概率，就可以根据 RGB-D 数据，更新整个八叉树地图了
假设在RGB-D 图像中观测到某个像素带有深度 d，这说明了一件事：
在深度值对应的空间点上观察到了一个占据数据
并且，从相机光心出发，到这个点的线段上，应该是没有物体的（否则会被遮挡）
利用这个信息，可以很好地对八叉树地图进行更新，并且能处理运动的结构

3. 八叉树地图

首先，安装 octomap 库：

$ git clone https://github.com/OctoMap/octomap

Octomap 库主要包含 octomap 地图与 octovis（一个可视化程序），二者都是 cmake 工程

$ cd octomap
$ mkdir build
$ cd build
$ cmake ..
$ make

或者独立安装octovis

$ sudo apt-get install octovis

主要依赖项是 doxygen：

$ sudo apt-get install doxygen

通过前面的五张图像生成八叉树地图，然后将它画出来
代码中，省略掉图像读取的部分，因为这和之前的内容相同

#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <octomap/octomap.h>    // for octomap 
#include <Eigen/Geometry> 
#include <boost/format.hpp>  // for formating strings

int main(int argc, char** argv)
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿

    ifstream fin("./data/pose.txt");
    if (!fin)
    {
        cerr << "cannot find pose file" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++)
    {
        boost::format fmt("./data/%s/%d.%s"); //图像文件格式
        colorImgs.push_back(cv::imread((fmt%"color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt%"depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像

        double data[7] = { 0 };
        for (int i = 0; i < 7; i++)
        {
            fin >> data[i];
        }
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        Eigen::Isometry3d T(q);
        T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(T);
    }

    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    cout << "正在将图像转换为 Octomap ..." << endl;

    // octomap tree 
    octomap::OcTree tree(0.05); // 参数为分辨率

    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;  // 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");
    return 0;
}

使用了 octomap::OcTree 来构建整张地图
实际上 octomap 提供了许多种八叉树：有带地图的，有带占据信息的
也可以自己定义每个节点需要携带哪些变量
简单起见，使用了不带颜色信息的，最基本的八叉树地图

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

现在，调用它打开地图文件

$ octovis octomap.bt 

就能看到地图的实际样子了，显示了构建的地图结果

由于没有在地图中加入颜色信息，所以一开始打开地图时将是灰色的
按 1 键可以根据高度信息进行染色

在右侧有八叉树地深度限制条，这里可以调节地图的分辨率
由于构造时使用的默认深度是 16 层，所以这里显示 16 层的话即最高分辨率，也就是每个小块的边长为 0.05米
当将深度减少一层时，八叉树的叶子节点往上提了一层，每个小块的边长就增加两倍，变成 0.1 米

可以看到，能够很容易地调节地图分辨率以适应不同的场合

Octomap 还有一些可以探索的地方，例如：
可以方便地查询任意点的占据概率，以此设计在地图中进行导航的方法
亦可比较点云地图与八叉树地图的文件大小
上一节生成的点云地图的磁盘文件大约为 7.2M
而 octomap 只有 56K，连点云地图的百分之一都不到，可以有效地建模较大的场景

参考：

《视觉SLAM十四讲》

相关推荐：

视觉SLAM笔记（63） RGB-D 稠密建图
视觉SLAM笔记（62） 单目稠密重建
视觉SLAM笔记（61） 单目稠密建图
视觉SLAM笔记（60） 建图
视觉SLAM笔记（59） 相似度计算

谢谢！