构建语义地图时，用的是 octomap_server和 semantic_slam: octomap_generator，不过还是整理下之前的学习笔记。

如果你不需要修改源码，可以直接安装编译好的 octomap 库，记得把 ROS 版本「kinetic」替换成你用的：

上面这一行命令等价于安装以下的 octomap 组件：

注意：上面没有安装 ros-kinetic-octomap-server，原因是我要使用这个包来建图，并且需要修改它，所以在下一步我直接通过编译源码来安装它！

因为我要启用八叉树体素栅格的 RGB 颜色支持，需要修改源码，所以必须使用源码编译安装，过程如下：

编译过程基本没有报错，如果你遇到问题，直接复制错误信息浏览器搜索解决，然后启动测试的 launch：

没问题的话应该可以用 rostopic list 看到一个 octomap_full 的话题： 有这个话题说明这个建图包可以正常工作啦：）

ROS 中提供了一个 Rviz 可视化 Octomap 的插件，如果没有安装使用下面的命令即可：

安装后启动 Rviz，直接添加一个八叉树占用网格的类型，第一个是带颜色的类型，第二个不带颜色： 建图节点启动后，选择话题名称为 octomap_full，即可显示出八叉树体素栅格，这是我的实验结果，我用的是第一个带颜色的类型：

我把点云和体素栅格一起显示了，所以会重叠。这里要注意的是，如果你的点云显示不出来，要检查下「Global Options」的「Fixed Frame」有没有设置正确，我是设置的是 Robosense 雷达的 frame_id：「rslidar」。

默认编译的 octomap 不能显示颜色，要开启颜色的支持，需要 2 个步骤，第一步编辑 OctomapServer.h 文件：

打开下面 COLOR_OCTOMAP_SERVER 宏的注释即可：

然后重新编译一遍源码：

第二步是在使用时，在 launch 文件中禁用 height_map，启用 colored_map，这个配置是我阅读源码查找的，因为官网文档很久没有更新了，一些参数配置方法需要通过阅读源码才能知道：

比如以下是我实验用的 launch 文件：

我设置了八叉树帧的 frame 为 rslidar，并将融合的点云话题 /fusion_cloud 作为节点的输入，我没有提供 TF，因为目前只是做了一个单帧的体素栅格构建，效果如下：

为了能够让 octomap_server 建图包实现增量式的地图构建，需要以下 2 个步骤：

这 3 个参数是建图必备：

以下是所有可以配置的参数：

frame_id (string, default: /map)

resolution (float, default: 0.05)

height_map (bool, default: true)

color/[r/g/b/a] (float)

sensor_model/max_range (float, default: -1 (unlimited))

sensor_model/[hit|miss] (float, default: 0.7 / 0.4)

sensor_model/[min|max] (float, default: 0.12 / 0.97)

latch (bool, default: True for a static map, false if no initial map is given)

base_frame_id(string, default: base_footprint)

filter_ground(bool, default: false)

ground_filter/distance (float, default: 0.04)

ground_filter/angle (float, default: 0.15)

ground_filter/plane_distance (float, default: 0.07)

pointcloud_[min|max]_z (float, default: -/+ infinity)

occupancy_[min|max]_z (float, default: -/+ infinity)

filter_speckles(bool)

有了全局坐标系和每一帧的点云，但是建图包怎么知道把每一帧点云插入到哪个位置呢？

因为随着机器人的不断移动，会不断产生新的点云帧，每个点云帧在全局坐标系中插入的时候都有一个确定的位置，否则构建的地图就不对了，因此需要给建图包提供一个当前帧点云到全局坐标系的位姿，这样建图包才能根据这个位姿把当前获得的点云插入到正确的位置上。

在 ROS 中可以很方便的使用 TF 来表示这个变换，我们只需要在启动建图包的时候，在系统的 TF 树中提供「cloud frame -> world frame」的变换就可以了：

注意：

cloud frame：就是输入点云话题中 head.frame_id，比如 Robosense 雷达的 frame_id = rslidar world frame：这是全局坐标系的 frame_id，在启动 launch 中需要手动给定，比如我给的是 map 如果你给 world frame id 指定的是输入点云的 frame_id，比如 fusion_cloud.head.frame_id == wolrd_frame_id == rslidar，则只会显示当前帧的八叉树，而不会增量构建地图，这点要注意了，可以自己测试看看。

那么为了增量式建图，还需要在系统的 TF 树中提供「rslidar -> world」的变换，这个变换可以通过其他的 SLAM 等获得，比如我测试时候的一个 TF 树如下： 我找了下源代码 OctomapServer.cpp 中寻找 TF 的部分：

如果你也遇到这个问题，可以试试发个静态 TF 做做测试，关于静态 TF 详细技术可以参考之前的文章：ROS 机器人技术 - 静态 TF 坐标帧

为了显示 RGB 颜色，我分析了下源码，第一步修改头文件，打开注释切换地图类型：OctomapServer.h

CMakeList.txt 文件中有 COLOR_OCTOMAP_SERVER 的编译选项：

OctomapServer.cpp 中有 colored_map 的参数：

地图默认是按照高度设置颜色，如果要设置为带颜色的地图，就要禁用 HeightMap，并启用 ColoredMap：

第二步、需要在 octomap_server 的 launch 文件中禁用 height_map，并启用 colored_map ：

2 个核心的八叉树生成函数 insertCloudCallback 和 insertScan 中有对颜色的操作：

启动 octomap_server 节点后，可以使用它提供的地图保存服务，保存压缩的二进制存储格式地图：

保存一个完整的概率八叉树地图：

安装八叉树可视化程序 octovis 来查看地图：

安装后重启终端，使用以下命令显示一个八叉树地图：

同时笔者整理了以下这个建图包的关键流程，只有 2 个关键的函数也不是很复杂。

第一步是订阅点云的回调： 第二步是插入单帧点云构建八叉树，这里就不详细介绍过程了，因为涉及到八叉树库 octomap 的更新原理： 以下是我建图过程的 launch：

这里给一个链接：https://github.com/Quitino/IndoorMapping，该作者实现了基于ORB-SLAM生成三维密集点云，并使用OctoMap构建室内导航地图。也意味着我们可以在此基础上加入轨迹规划算法实现基于OBR-SLAM的室内规划导航。具体代码可以参照：https://blog.csdn.net/lovely_yoshino/article/details/105272602中的3D-RRT路径规划方法实现。

AngeloG 博文中写了三维 A的相关matlab仿真。为了便于在ROS中对A算法进行开发，这里也提供一下之前我学习的A*算法