pcd点云地图转换为八叉树ot地图

最新推荐文章于 2024-07-19 12:11:17 发布

古路

最新推荐文章于 2024-07-19 12:11:17 发布

阅读量4.7k

点赞数 2

分类专栏： slam 文章标签： pcd点云地图转换为八叉树ot地图

本文链接：https://blog.csdn.net/fb_941219/article/details/89002257

版权

slam 专栏收录该内容

70 篇文章 22 订阅

订阅专栏

pcd点云地图转换为八叉树ot地图

引言

参考高博博客

1.code

#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];
    string output_file = argv[2];
    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.04
    octomap::ColorOcTree tree( 0.04 );

    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;
}