SLAM 将PCL点云转换成八叉树地图

这个程序可以把PCL点云压缩成不带颜色的八叉树地图。

先看一下输入数据，是一个咖啡厅的一小部分：

程序运行后会生成不带颜色的八叉树地图，使用octovis查看：

把输出结果按高度染色：

输入：任意PCL点云 <pointcloud.pcd>

输出：以二进制保存的八叉树地图 <pointcloud.bt>

运行：编译后在cmd里带参数才能运行 <pointcloud.pcd> <pointcloud.bt>

示例：program,exe sample.pcd sample.bt

视频演示：https://www.bilibili.com/video/BV18sLi6dEHW/

代码：

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

参考：SLAM拾萃(1)：octomap - 半闲居士 - 博客园