下载编译安装见之前博客：编程开发常用的第三方库安装

安装octomap_ros（cmake+make+install）

git clone https://github.com/OctoMap/octomap_ros

安装genmsg（cmake+make+install） ( octomap_msgs的依赖项)

git clone https://github.com/ros/genmsg 

安装octomap_msgs（cmake+make+install）

git clone https://github.com/OctoMap/octomap_msgs

相关头文件：

#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap/math/Pose6D.h> // 可以用octomath::Pose6D来存储位姿

常用接口：

// 创建不带颜色的八叉树对象，参数为分辨率，
octomap::OcTree tree(0.05); // 5cm x 5cm x 5cm
// 创建带颜色的八叉树对象
octomap::ColorOcTree tree(0.05);

// 清空octomap
tree.clear()

// 将点插入到octomap中
tree.updateNode(octomap::point3d(x, y, z), true); // true为occupy，　false为free
// 射线的形式，只有末端才存在被占据的点，中途的点则是没被占据的
octomap::Pointcloud cloud_octo;
for (auto p:temp->points)
　　　　cloud_octo.push_back(p.x, p.y, p.z);
tree.insertPointCloud(cloud_octo, octomap::point3d(translation_beam_to_world[0], translation_beam_to_world[1],translation_beam_to_world[2]));

// 设置颜色
tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
tree.setNodeColor(x, y, z, R, G, B);

// 更新octomap
tree.updateInnerOccupancy();

// 存储octomap
// octomap存储的文件后缀名是.bt（二进制文件）和.ot（普通文件）,带颜色的八叉树要存成.ot文件
tree.writeBinary(output_file);
tree.write(output_file);

先把图像转成pcl的点云，变换后再放到octotree中。这种做法的原因是比较便于处理颜色，因为我希望做出带有颜色的地图。如果你不关心颜色，完全可以不用pcl，直接用octomap自带的octomap::pointcloud来完成这件事。

insertPointCloud会比单纯的插入点更好一些。octomap里的pointcloud是一种射线的形式，只有末端才存在被占据的点，中途的点则是没被占据的。这会使一些重叠地方处理的更好。

CMakeList.txt:

find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})

add_executable(main main.cpp)
target_link_libraries(main ${OCTOMAP_LIBRARIES})

转换为ROSmsg：

octomap::ColorOcTree* octomap = new octomap::OcTree(0.05);

//声明advertise，octomap rviz plug in 默认接受topic为octomap_full的message
pub_octomap = n.advertise<octomap_msgs::Octomap>("octomap_full", 1, true);
 
//声明message
Octomap map_msg;
//设置header
map_msg.header.frame_id = "map";
map_msg.header.stamp = ros::Time::now();
//fullMapToMsg负责转换成message
if (octomap_msgs::fullMapToMsg(*octomap, map_msg))
    pub_octomap.publish(map_msg);
else
    ROS_ERROR("Error serializing OctoMap");

在RVIZ中实时显示：

 

octovis可视化.ot和.bt文件

octovis xxx.ot

 

地图障碍占据情况查询：

 

C++例子：

 

参考：

SLAM拾萃(1)：octomap

ROS octomap常用函数介绍和实时显示octomap

Octomap使用总结

https://github.com/OctoMap/octomap/blob/devel/octomap/src/simple_example.cpp