点云地图变为八叉树地图

参考高博文章

一、将pcd格式变为bt格式，使用高度渲染颜色

#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 != 2)
    {
        cout<<"error"<<endl;
        return -1;
    }

    string input_file = argv[1];
    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("../octomap.bt");
    cout<<"done."<<endl;

    return 0;
}

//运行方法
octovis octomap.bt

 不知道为什么我这里生成的地图只有蓝色（有大佬知道原因可以在评论区留言，谢谢！！！）

二、将pcd格式变为ot格式，使用点云的颜色

octomap提供了ColorOcTree类

#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 != 2) {
        cout << ".." << endl;
        return -1;
    }

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

    cout << "point cloud loaded, point 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();
 
    //使用点云的颜色信息  用ot格式
    tree.write("../octomap.ot");
    
    cout << "done." << endl;

    return 0;
}

上图是对fr1_desk数据集进行八叉树地图的结果。

  三、根据trajectory.txt里记录的信息，把几个RGBD图拼成一个octomap

TODO