这个程序可以把OctoMap压缩后的八叉树地图解压成PCL可以处理的3D点云地图。先使用octovis查看一下输入的八叉树点云地图geb079.btup写了《SLAM 基于Qt C的八叉树地图可视化工具octovis-CSDN博客》介绍octovis这里就不多说了。现在看一下运行程序后生成的3D点云地图geb079.pcd输入geb079.bt 压缩后的八叉树地图大小为4KB输出geb079.pcd 解压后的3D点云地图大小为2.95MB运行编译后在cmd里运行需要2个参数 输入的八叉树地图 输出的3D点云地图示例octree2pointcloud.exe geb079.bt geb079.pcd视频演示https://www.bilibili.com/video/BV1YGVh6kEMN/代码#include string.h #include stdlib.h #include iostream #include fstream #include octomap/octomap.h #include octomap/octomap_timing.h using namespace std; using namespace octomap; void printUsage(char* self){ cerr USAGE: self InputFile.bt OutputFile.pcd\n; cerr This tool creates a point cloud of the occupied cells\n; exit(0); } int main(int argc, char** argv) { if (argc ! 3) printUsage(argv[0]); string inputFilename argv[1]; string outputFilename argv[2]; OcTree* tree new OcTree(0.1); if (!tree-readBinary(inputFilename)){ OCTOMAP_ERROR(Could not open file, exiting.\n); exit(1); } unsigned int maxDepth tree-getTreeDepth(); cout tree depth is maxDepth endl; // expand collapsed occupied nodes until all occupied leaves are at maximum depth vectorOcTreeNode* collapsed_occ_nodes; do { collapsed_occ_nodes.clear(); for (OcTree::iterator it tree-begin(); it ! tree-end(); it) { if(tree-isNodeOccupied(*it) it.getDepth() maxDepth) { collapsed_occ_nodes.push_back((*it)); } } for (vectorOcTreeNode*::iterator it collapsed_occ_nodes.begin(); it ! collapsed_occ_nodes.end(); it) { tree-expandNode(*it); } cout expanded collapsed_occ_nodes.size() nodes endl; } while(collapsed_occ_nodes.size() 0); vectorpoint3d pcl; for (OcTree::iterator it tree-begin(); it ! tree-end(); it) { if(tree-isNodeOccupied(*it)) { pcl.push_back(it.getCoordinate()); } } delete tree; ofstream f(outputFilename.c_str(), ofstream::out); f # .PCD v0.7 endl VERSION 0.7 endl FIELDS x y z endl SIZE 4 4 4 endl TYPE F F F endl COUNT 1 1 1 endl WIDTH pcl.size() endl HEIGHT 1 endl VIEWPOINT 0 0 0 0 0 0 1 endl POINTS pcl.size() endl DATA ascii endl; for (size_t i 0; i pcl.size(); i) f pcl[i].x() pcl[i].y() pcl[i].z() endl; f.close(); return 0; }
