这个程序可以把OctoMap压缩后的八叉树地图解压成PCL可以处理的3D点云地图。

先使用octovis查看一下输入的八叉树点云地图geb079.bt：

up写了《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
  vector<OcTreeNode*> 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 (vector<OcTreeNode*>::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);

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