视觉SLAM笔记(64) 八叉树地图

视觉SLAM笔记(64) 八叉树地图


1. 点云地图缺陷

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

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

2. 八叉树

把三维空间建模为许多个小方块(或体素),是一种常见的做法
如果把一个小方块的每个面平均切成两片,那么这个小方块就会变成同样大小的八个小方块
这个步骤可以不断的重复,直到最后的方块大小达到建模的最高精度
在这个过程中,把“将一个小方块分成同样大小的八个”这件事,看成“从一个节点展开成八个子节点”
那么,整个从最大空间细分到最小空间的过程,就是一棵八叉树(Octo-tree)
在这里插入图片描述
左侧显示了一个大立方体不断地均匀分成八块,直到变成最小的方块为止
于是,整个大方块可以看成是根节点,而最小的块可以看作是“叶子节点”
于是,在八叉树中,当由下一层节点往上走一层时,地图的体积就能扩大八倍

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

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

前面说八叉树的节点存储了它是否被占据的信息
从点云层面来讲,自然可以用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 之间的概率,那么它们之间的变换由 logitlogit 变换描述:
在这里插入图片描述
其反变换为:
在这里插入图片描述
可以看到,当 y 从 −1 变到 +1 时, x 相应地从 0 变到了 1,而当 y 取 0 时, x 取到 0.5
因此,不妨存储 y 来表达节点是否被占据
当不断观测到“占据”时,让 y增加一个值;否则就让 y 减小一个值。

当查询概率时,再用逆 logitlogit 变换,将 y 转换至概率即可
用数学形式来说,设某节点为 n,观测数据为 z
那么从开始到 t 时刻某节点的概率对数值为 LL(nn|zz1: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) 相似度计算


谢谢!

发布了233 篇原创文章 · 获赞 322 · 访问量 296万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览