SLAM拾萃(1)：octomap

最新推荐文章于 2024-02-26 18:05:42 发布

老李lyj

最新推荐文章于 2024-02-26 18:05:42 发布

阅读量5.1k

点赞数 1

分类专栏： slam 文章标签： SLAM

slam 专栏收录该内容

18 篇文章 1 订阅

订阅专栏

本篇是《SLAM拾萃》第一篇，介绍一个建图工具：octomap。和往常一样，我会介绍它的原理、安装与使用方式，并提供例程供读者学习。必要时也会请小萝卜过来吐槽。（小萝卜真是太好用了，它可以代替读者提很多问题。）

什么是octomap？

　　RGBD SLAM的目的有两个：估计机器人的轨迹，并建立正确的地图。地图有很多种表达方式，比如特征点地图、网格地图、拓扑地图等等。在《一起做》系列中，我们使用的地图形式主要是点云地图。在程序中，我们根据优化后的位姿，拼接点云，最后构成地图。这种做法很简单，但有一些明显的缺陷：

地图形式不紧凑。
　　点云地图通常规模很大，所以一个pcd文件也会很大。一张640 × 480的图像，会产生30万个空间点，需要大量的存储空间。即使经过一些滤波之后，pcd文件也是很大的。而且讨厌之处在于，它的“大”并不是必需的。点云地图提供了很多不必要的细节。对于地毯上的褶皱、阴暗处的影子，我们并不特别关心这些东西。把它们放在地图里是浪费空间。
处理重叠的方式不够好。
　　在构建点云时，我们直接按照估计位姿拼在了一起。在位姿存在误差时，会导致地图出现明显的重叠。例如一个电脑屏变成了两个，原本方的边界变成了多边形。对重叠地区的处理方式应该更好一些。
难以用于导航
　　说起地图的用处，第一就是导航啦！有了地图，就可以指挥机器人从A点到B点运动，岂不是很方便的事？但是，给你一张点云地图，是否有些傻眼了呢？我至少得知道哪些地方可通过，哪些地方不可通过，才能完成导航呀！光有点是不够的！

　　octomap就是为此而设计的！亲，你没有看错，它可以优雅地压缩、更新地图，并且分辨率可调！它以八叉树（octotree，后面会讲）的形式存储地图，相比点云，能够省下大把的空间。octomap建立的地图大概是这样子的：（从左到右是不同的分辨率）

　　由于八叉树的原因，它的地图像是很多个小方块组成的（很像minecraft）。当分辨率较高时，方块很小；分辨率较低时，方块很大。每个方块表示该格被占据的概率。因此你可以查询某个方块或点“是否可以通过”，从而实现不同层次的导航。简而言之，环境较大时采用较低分辨率，而较精细的导航可采用较高分辨率。

　　小萝卜：师兄你这是介绍吗？真像广告啊……

octomap原理

　　本段会讲一些数学知识。如果你想“跑跑程序看效果”，可以跳过本段。

八叉树的表达

　　八叉树，也就是传说中有八个子节点的树！是不是很厉害呢？至于为什么要分成八个子节点，想象一下一个正方形的方块的三个面各切一刀，不就变成八块了嘛！如果你想象不出来，请看下图：

　　实际的数据结构呢，就是一个树根不断地往下扩，每次分成八个枝，直到叶子为止。叶子节点代表了分辨率最高的情况。例如分辨率设成0.01m，那么每个叶子就是一个1cm见方的小方块了呢！

　　每个小方块都有一个数描述它是否被占据。在最简单的情况下，可以用0－1两个数表示（太简单了所以没什么用）。通常还是用0～1之间的浮点数表示它被占据的概率。0.5表示未确定，越大则表示被占据的可能性越高，反之亦然。由于它是八叉树，那么一个节点的八个孩子都有一定的概率被占据或不被占据啦！（下图是一棵八叉树）

　　用树结构的好处时：当某个节点的子结点都“占据”或“不占据”或“未确定”时，就可以把它给剪掉！换句话说，如果没必要进一步描述更精细的结构（孩子节点）时，我们只要一个粗方块（父节点）的信息就够了。这可以省去很多的存储空间。因为我们不用存一个“全八叉树”呀！

　　2.　八叉树的更新

小萝卜：哦！这个我就懂了！每新来一个就直接加到原来的上面，是吧？

　　师兄：对，此外还要加一个最大最小值的限制。最后转换回原来的概率即可。

　　八叉树中的父亲节点占据概率，可以根据孩子节点的数值进行计算。比较简单的是取平均值或最大值。如果把八叉树按照占据概率进行渲染，不确定的方块渲染成透明的，确定占据的渲染成不透明的，就能看到我们平时见到的那种东西啦！

　　octomap本身的数学原理还是简单的。不过它的可视化做的比较好。下面我们来讲讲如何下载、安装八叉树程序，并给出几个小的例程。

安装octomap

　　octomap的网页见：https://octomap.github.io

　　它的github源码在：https://github.com/OctoMap/octomap

　　它还有ROS下的安装方式：http://wiki.ros.org/octomap

　　在开发过程中，可能需要不断地查看它的API文档。你可以自己用doxygen生成一个，或者查看在线文档：http://octomap.github.io/octomap/doc/

　　为了保持简洁，我们不要求读者安装ROS，仅介绍单独的octomap。我的编译环境是ubuntu 14.04。ubuntu系列的应该都不会有太大问题。

　　1.　　编译octomap
　　　新建一个目录，拷贝octomap代码。如果没有git请安装git：sudo apt-get install git

```
git clone https://github.com/OctoMap/octomap
```
git会把代码拷贝到当前目录/octomap下。进入该目录，参照README.md进行安装。编译方式和普通的cmake程序一样，如果你学过《一起做》就应该很熟悉了：
```
1 mkdir build
2 cd build
3 cmake ..
4 make
```
事实上，octomap的代码主要含两个模块：本身的octomap和可视化工具octovis。octovis依赖于qt4和qglviewer，所以如果你没有装这两个依赖，请安装它们：sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev

如果编译没有给出任何警告，恭喜你编译成功！
使用octovis查看示例地图
在bin/文件夹中，存放着编译出来可执行文件。为了直观起见，我们直接看一个示例地图：
```
bin/octovis octomap/share/data/geb079.bt
```
octovis会打开这个地图并显示。它的UI是长这样的。你可以玩玩菜单里各种东西（虽然也不多，我就不一一介绍UI怎么玩了），能看出这是一层楼的扫描图。octovis是一个比较实用的工具，你生成的各种octomap地图都可以用它来看。（所以你可以把octovis放到/usr/local/bin/下，省得以后还要找。）

例程1：转换pcd到octomap

　　GUI玩够了吧？仅仅会用UI是不够滴，现在让我们开始编代码使用octomap这个库吧！

　　我为你准备了三个小例程。在前两个中，我会教你如何将一个pcd格式的点云地图转换为octomap地图。后一个中，我会讲讲如何根据g2o优化的轨迹，以类似slam的方式，把几个RGBD图像拼接出一个octomap。这对你研究SLAM会有一些帮助。所有的代码与数据都可以在我的github上找到。有关编译的信息，我写在这个代码的Readme中了，请在编译前看一眼如何编译这些代码。

　　源代码地址：https://github.com/gaoxiang12/octomap_tutor

　　源代码如下：src/pcd2octomap.cpp 这份代码将命令行参数1作为输入文件，参数2作为输出文件，把输入的pcd格式点云转换成octomap格式的点云。通过这个例子，你可以学会如何创建一个简单的OcTree对象并往里面添加新的点。　　

#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//octomap 
#include <octomap/octomap.h>
using namespace std;

int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2octomap <input_file> <output_file>"<<endl;
        return -1;
    }

    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    //声明octomap变量
    cout<<"copy data into octomap..."<<endl;
    // 创建八叉树对象，参数为分辨率，这里设成了0.05
    octomap::OcTree tree( 0.05 );

    for (auto p:cloud.points)
    {
        // 将点云里的点插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }

    // 更新octomap
    tree.updateInnerOccupancy();
    // 存储octomap
    tree.writeBinary( output_file );
    cout<<"done."<<endl;

    return 0;
}

这个代码是相当直观的。在编译之后，它会产生一个可执行文件，叫做pcd2octomap，放在代码根目录的bin/文件夹下。你可以在代码根目录下这样调：

1 bin/pcd2octomap data/sample.pcd data/sample.bt

　　它会把data文件夹下的sample.pcd（一个示例pcd点云），转换成一个data/sample.bt的octomap文件。你可以比较下pcd点云与octomap的区别。下图是分别调用这些显示命令的结果。　　

1 pcl_viewer data/sample.pcd
2 octovis data/sample.ot

　　这个octomap里只存储了点的空间信息，而没有颜色信息。我按照高度给它染色了，否则它应该就是灰色的。通过octomap，我们能查看每个小方块是否可以通行，从而实现导航的工作。

　　以下是对代码的一些注解：

　　注1：有关如何读取pcd文件，你可以参见pcl官网的tutorial。不过这件事情十分简单，所以我相信你也能直接看懂。

　　注2：31行采用了C++11标准的for循环，它会让代码看起来稍微简洁一些。如果你的编译器比较老而不支持c++11，你可以自己将它改成传统的for循环的样式。

　　注3：octomap存储的文件后缀名是.bt（二进制文件）和.ot（普通文件），前者相对更小一些。不过octomap文件普遍都很小，所以也不差这么些容量。如果你存成了其他后缀名，octovis可能认不出来。

例程2：加入色彩信息

　　第一个示例中，我们将pcd点云转换为octomap。但是pcd点云是有颜色信息的，能否在octomap中也保存颜色信息呢？答案是可以的。octomap提供了ColorOcTree类，能够帮你存储颜色信息。下面我们就来做一个保存颜色信息的示例。代码见：src/pcd2colorOctomap.cpp

#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//octomap 
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>

using namespace std;

int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2colorOctomap <input_file> <output_file>"<<endl;
        return -1;
    }

    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    //声明octomap变量
    cout<<"copy data into octomap..."<<endl;
    // 创建带颜色的八叉树对象，参数为分辨率，这里设成了0.05
    octomap::ColorOcTree tree( 0.05 );

    for (auto p:cloud.points)
    {
        // 将点云里的点插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }

    // 设置颜色
    for (auto p:cloud.points)
    {
        tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
    }

    // 更新octomap
    tree.updateInnerOccupancy();
    // 存储octomap, 注意要存成.ot文件而非.bt文件
    tree.write( output_file );
    cout<<"done."<<endl;

    return 0;
}

　大部分代码和刚才是一样的，除了把OcTree改成ColorOcTree，以及调用integrateNodeColor来混合颜色之外。这段代码会编译出pcd2colorOctomap这个程序，完成带颜色的转换。不过，后缀名改成了.ot文件。　　

1 bin/pcd2colorOctomap data/sample.pcd data/sample.ot

　　颜色信息能够更好地帮助我们辨认结果是否正确，给予一个直观的印象。是不是好看了一些呢？

例程3：更好的拼接与转换

　　前两个例程中，我们都是对单个pcd文件进行了处理。实际做slam时，我们需要拼接很多帧的octomap。为了做这样一个示例，我从自己的实验数据中取出了一小段。这一小段总共含有五张图像（因为github并不适合传大量数据），它们存放在data/rgb_index和data/dep_index下。我的slam程序估计了这五个关键帧的位置，放在data/trajectory.txt中。它的格式是：帧编号 x y z qx qy qz qw （位置＋姿态四元数）。事实上它是从一个g2o文件中拷出来的。你可以用g2o_viewer data/result_after.g2o来看整个轨迹。

54 -0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042
144 -0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662
230 -0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536
313 -1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178
346 -1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741

　　现在我们要做的事，就是根据trajectory.txt里记录的信息，把几个RGBD图拼成一个octomap。这也是所谓的用octomap来建图。我写了一个示例，不知道你能否读懂呢？src/joinMap.cpp

#include <iostream>
#include <vector>

// octomap 
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap/math/Pose6D.h>

// opencv 用于图像数据读取与处理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// 使用Eigen的Geometry模块处理3d运动
#include <Eigen/Core>
#include <Eigen/Geometry> 

// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>

// boost.format 字符串处理
#include <boost/format.hpp>

using namespace std;

// 全局变量：相机矩阵
// 更好的写法是存到参数文件中，但为方便起见我就直接这样做了
float camera_scale  = 1000;
float camera_cx     = 325.5;
float camera_cy     = 253.5;
float camera_fx     = 518.0;
float camera_fy     = 519.0;

int main( int argc, char** argv )
{
    // 读关键帧编号
    ifstream fin( "./data/keyframe.txt" );
    vector<int> keyframes;
    vector< Eigen::Isometry3d > poses;
    // 把文件 ./data/keyframe.txt 里的数据读取到vector中
    while( fin.peek() != EOF )
    {
        int index_keyframe;
        fin>>index_keyframe;
        if (fin.fail()) break;
        keyframes.push_back( index_keyframe );
    }
    fin.close();

    cout<<"load total "<<keyframes.size()<<" keyframes. "<<endl;

    // 读关键帧姿态
    // 我的代码中使用了Eigen来存储姿态，类似的，也可以用octomath::Pose6D来做这件事
    fin.open( "./data/trajectory.txt" );
    while( fin.peek() != EOF )
    {
        int index_keyframe;
        float data[7]; // 三个位置加一个姿态四元数x,y,z, w,ux,uy,uz
        fin>>index_keyframe;
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
            cout<<data[i]<<" ";
        }
        cout<<endl;
        if (fin.fail()) break;
        // 注意这里的顺序。g2o文件四元数按 qx, qy, qz, qw来存
        // 但Eigen初始化按照qw, qx, qy, qz来做
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d t(q);
        t(0,3) = data[0]; t(1,3) = data[1]; t(2,3) = data[2];
        poses.push_back( t );
    }
    fin.close();

    // 拼合全局地图
    octomap::ColorOcTree tree( 0.05 ); //全局map

    // 注意我们的做法是先把图像转换至pcl的点云，进行姿态变换，最后存储成octomap
    // 因为octomap的颜色信息不是特别方便处理，所以采用了这种迂回的方式
    // 所以，如果不考虑颜色，那不必转成pcl点云，而可以直接使用octomap::Pointcloud结构
    
    for ( size_t i=0; i<keyframes.size(); i++ )
    {
        pcl::PointCloud<pcl::PointXYZRGBA> cloud; 
        cout<<"converting "<<i<<"th keyframe ..." <<endl;
        int k = keyframes[i];
        Eigen::Isometry3d& pose = poses[i];

        // 生成第k帧的点云，拼接至全局octomap上
        boost::format fmt ("./data/rgb_index/%d.ppm" );
        cv::Mat rgb = cv::imread( (fmt % k).str().c_str() );
        fmt = boost::format("./data/dep_index/%d.pgm" );
        cv::Mat depth = cv::imread( (fmt % k).str().c_str(), -1 );

        // 从rgb, depth生成点云，运算方法见《一起做》第二讲
        // 第一次遍历用于生成空间点云
        for ( int m=0; m<depth.rows; m++ )
            for ( int n=0; n<depth.cols; n++ )
            {
                ushort d = depth.ptr<ushort> (m) [n];
                if (d == 0)
                    continue;
                float z = float(d) / camera_scale;
                float x = (n - camera_cx) * z / camera_fx;
                float y = (m - camera_cy) * z / camera_fy;
                pcl::PointXYZRGBA p;
                p.x = x; p.y = y; p.z = z;

                uchar* rgbdata = &rgb.ptr<uchar>(m)[n*3];
                uchar b = rgbdata[0];
                uchar g = rgbdata[1];
                uchar r = rgbdata[2];

                p.r = r; p.g = g; p.b = b;
                cloud.points.push_back( p ); 
            }
        // 将cloud旋转之后插入全局地图
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp( new pcl::PointCloud<pcl::PointXYZRGBA>() );
        pcl::transformPointCloud( cloud, *temp, pose.matrix() );

        octomap::Pointcloud cloud_octo;
        for (auto p:temp->points)
            cloud_octo.push_back( p.x, p.y, p.z );
        
        tree.insertPointCloud( cloud_octo, 
                octomap::point3d( pose(0,3), pose(1,3), pose(2,3) ) );

        for (auto p:temp->points)
            tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
    }
    
    tree.updateInnerOccupancy();
    tree.write( "./data/map.ot" );

    cout<<"done."<<endl;
    
    return 0;

}

大部分需要解释的地方，我都在程序里写了注解。我用了一种稍微有些迂回的方式：先把图像转成pcl的点云，变换后再放到octotree中。这种做法的原因是比较便于处理颜色，因为我希望做出带有颜色的地图。如果你不关心颜色，完全可以不用pcl，直接用octomap自带的octomap::pointcloud来完成这件事。

　　insertPointCloud会比单纯的插入点更好一些。octomap里的pointcloud是一种射线的形式，只有末端才存在被占据的点，中途的点则是没被占据的。这会使一些重叠地方处理的更好。

　　最后，五帧数据拼接出来的点云大概长这样：　　

　　可能并不是特别完整，毕竟我们只用了五张图。这些数据来自于nyud数据集的dining_room序列，一个比较完整的图应该是这样的：

　　至少是比纯粹点云好些了吧？好了，关于例程就介绍到这里。如果你准备使用octomap，这仅仅是个入门。你需要去查看它的文档，了解它的类结构，以及一些重要类的使用、实现方式。

　　《SLAM拾萃》第一讲，octomap，就为大家介绍到这里啦。最近我发现自己写东西，讲东西都越来越长，所以请原谅我越来越啰嗦的写作和说话风格。希望它能帮助你！我们下讲再见！

老李lyj

关注

1
点赞
踩
29

收藏

觉得还不错? 一键收藏
0
评论
SLAM拾萃(1)：octomap

本篇是《SLAM拾萃》第一篇，介绍一个建图工具：octomap。和往常一样，我会介绍它的原理、安装与使用方式，并提供例程供读者学习。必要时也会请小萝卜过来吐槽。（小萝卜真是太好用了，它可以代替读者提很多问题。）什么是octomap？　　RGBD SLAM的目的有两个：估计机器人的轨迹，并建立正确的地图。地图有很多种表达方式，比如特征点地图、网格地图、拓扑地图等等。在《一起做》
复制链接

扫一扫