ROS 八叉树地图构建详解：使用 octomap_server 实现增量式建图

本文详细介绍了在 ROS（机器人操作系统）环境下，利用 octomap_server 进行八叉树地图（OctoMap）的构建过程。内容涵盖了增量式地图构建的具体步骤、关键参数配置、TF 变换的要求、彩色八叉树的启用方法，以及地图的保存与可视化。通过系统化的学习笔记，帮助读者深入理解 octomap_server 的使用与优化。

一、增量式八叉树地图构建步骤

为了实现增量式的八叉树地图构建，使用 octomap_server 需要完成以下两个主要步骤：

1.1 配置 Launch 启动参数

配置启动参数是构建八叉树地图的基础，主要涉及以下三个关键参数：

1. 地图分辨率 (resolution)：用于初始化地图对象，决定地图的精细程度。分辨率越高，地图越精细，但计算和存储开销也越大。
2. 全局坐标系 (frame_id)：定义构建的全局地图所基于的坐标系，通常为 map 坐标系。这一参数确保所有点云数据被正确地叠加到全局地图中。
3. 输入点云话题 (/cloud_in)：作为建图的数据输入，octomap_server 将逐帧点云数据叠加到全局坐标系中，实现地图的构建。

示例 Launch 文件配置

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- 地图分辨率，单位为米 -->
    <param name="resolution" value="0.10" />

    <!-- 全局坐标系，通常为 "map" -->
    <param name="frame_id" type="string" value="map" />

    <!-- 订阅的点云话题重映射，将 "/cloud_in" 重映射为 "/fusion_cloud" -->
    <remap from="/cloud_in" to="/fusion_cloud" />
  </node>
</launch>

可配置参数详解

octomap_server 提供了丰富的参数选项，以适应不同的应用需求。以下是主要参数及其默认值和功能说明：

• frame_id (string, 默认: /map)

  • 定义地图发布的全局坐标系。建图时需要提供传感器数据到该坐标系的 TF 变换。

• resolution (float, 默认: 0.05)

  • 地图的分辨率，以米为单位。决定地图的细腻程度。

• height_map (bool, 默认: true)

  • 是否使用高度图进行可视化。高度图通过不同颜色编码高度信息。

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

  • 当 height_map 为 false 时，用于可视化占用单元的颜色，取值范围为 [0,1]。

• sensor_model/max_range (float, 默认: -1 (无限))

  • 插入点云数据的最大范围，单位为米。限制在有效范围内（如5米）可以防止虚假的错误点。

• sensor_model/[hit|miss] (float, 默认: 0.7 / 0.4)

  • 传感器模型的命中率和未命中率，影响地图更新的概率计算。

• sensor_model/[min|max] (float, 默认: 0.12 / 0.97)

  • 最小和最大概率限制，防止概率值过低或过高。

• latch (bool, 默认: True (静态地图), false (无初始地图))

  • 发布主题的锁定方式。频繁更新时建议设置为 false，以提高性能。

• base_frame_id (string, 默认: base_footprint)

  • 机器人底盘的坐标系，用于地面检测（如果启用）。

• filter_ground (bool, 默认: false)

  • 是否滤除地面点云。启用后，地面点不会被插入到地图中。

• ground_filter/distance (float, 默认: 0.04)

  • 地面分割的距离阈值，单位为米。小于该阈值的点被认为是地面。

• ground_filter/angle (float, 默认: 0.15)

  • 检测平面相对于水平面的角度阈值，单位为弧度。超过该角度的平面不被视为地面。

• ground_filter/plane_distance (float, 默认: 0.07)

  • 检测为平面的平面方程距离阈值，单位为米。

• pointcloud_[min|max]_z (float, 默认: -∞ / +∞)

  • 插入点云的最小和最大高度，超出此范围的点将被丢弃。

• occupancy_[min|max]_z (float, 默认: -∞ / +∞)

  • 地图中考虑的占用单元格的高度范围，超出此范围的体素在可视化和碰撞检测中被忽略，但不影响实际的八叉树表示。

• filter_speckles (bool)

  • 是否滤除斑点噪声，减少地图中的孤立噪声点。

1.2 配置 TF 变换

在 ROS 中，TF（Transform）用于表示不同坐标系之间的变换关系。为了将每一帧点云数据正确地插入到全局坐标系中，需要提供传感器坐标系到全局坐标系的 TF 变换。

1.2.1 定义坐标系

• 传感器坐标系 (cloud frame)：点云数据的坐标系，例如 rslidar。
• 全局坐标系 (world frame)：全局地图的坐标系，例如 map。

1.2.2 提供 TF 变换

octomap_server 通过监听 TF 树中的 cloud frame -> world frame 变换，将当前帧的点云数据转换到全局坐标系中。具体步骤如下：

1. 确保 TF 树中存在必要的变换：

   • 如果 TF 树中已有 cloud frame -> world frame 的变换，则 octomap_server 能够正确地将点云数据插入到地图中。

2. 手动发布静态 TF（如果缺失）：

   • 在某些情况下，TF 树中可能缺少必要的变换。例如，只有 world -> base_link 的变换，但缺少 base_link -> rslidar。此时，可以手动发布一个静态的 base_link -> rslidar 变换。

   rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar
   

   • 上述命令发布了一个零位移和零旋转的静态变换，将 base_link 坐标系与 rslidar 坐标系对齐。

1.2.3 注意事项

• 坐标系一致性：确保 world frame_id 与输入点云的 frame_id 不同。如果两者相同，octomap_server 只会显示当前帧的八叉树，而不会进行增量式建图。
• 动态变换：在使用 SLAM 等方法时，TF 变换会动态更新，确保每一帧点云数据的位姿是最新的。
• 静态变换的影响：如果手动发布的静态变换为零位姿，则所有点云数据将被插入到相同的位置，可能导致地图构建错误。确保静态变换的准确性。

示例 Launch 文件中发布静态 TF

<node pkg="tf2_ros" type="static_transform_publisher" name="dlonng_static_test_broadcaster" args="0 0 0 0 0 0 base_link rslidar" />

二、启用 ColorOctomap（彩色八叉树地图）

为了在地图中显示 RGB 颜色，需要对 octomap_server 进行源码修改和配置调整。以下是详细步骤：

2.1 修改源代码

2.1.1 修改头文件 OctomapServer.h

在 OctomapServer.h 中取消注释 COLOR_OCTOMAP_SERVER 以启用颜色支持。

// switch color here - easier maintenance, only maintain OctomapServer. 
// Two targets are defined in the cmake, octomap_server_color and octomap_server. One has this defined, and the other doesn't
// 打开这个注释
#define COLOR_OCTOMAP_SERVER

#ifdef COLOR_OCTOMAP_SERVER
  typedef pcl::PointXYZRGB PCLPoint;
  typedef pcl::PointCloud<pcl::PointXYZRGB> PCLPointCloud;
  typedef octomap::ColorOcTree OcTreeT;
#else
  typedef pcl::PointXYZ PCLPoint;
  typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
  typedef octomap::OcTree OcTreeT;
#endif

• 定义新的类型：启用 COLOR_OCTOMAP_SERVER 后，点云数据类型从 pcl::PointXYZ 变为 pcl::PointXYZRGB，八叉树类型从 octomap::OcTree 变为 octomap::ColorOcTree。

2.1.2 修改 CMakeLists.txt

在 CMakeLists.txt 文件中添加编译选项，定义 COLOR_OCTOMAP_SERVER 宏。

target_compile_definitions(${PROJECT_NAME}_color PUBLIC COLOR_OCTOMAP_SERVER)

• 目标编译定义：为 octomap_server_color 目标添加 COLOR_OCTOMAP_SERVER 定义，启用彩色八叉树功能。

2.1.3 修改源码 OctomapServer.cpp

在 OctomapServer.cpp 中配置颜色参数，禁用高度图并启用彩色地图。

m_useHeightMap = false;
m_useColoredMap = true;

m_nh_private.param("height_map", m_useHeightMap, m_useHeightMap);
m_nh_private.param("colored_map", m_useColoredMap, m_useColoredMap);

// 确保只启用其中一个
if (m_useHeightMap && m_useColoredMap) {
    ROS_WARN_STREAM("You enabled both height map and RGB color registration. This is contradictory. Defaulting to height map.");
    m_useColoredMap = false;
}

• 配置参数：通过禁用 height_map 并启用 colored_map，确保地图以 RGB 颜色显示，而非仅根据高度进行颜色编码。
• 冲突检测：防止同时启用 height_map 和 colored_map，避免颜色显示冲突。

2.2 配置 Launch 文件

在 Launch 文件中禁用 height_map 并启用 colored_map：

<param name="height_map" value="false" />
<param name="colored_map" value="true" />

• 禁用高度图：确保颜色显示不受高度图影响。
• 启用彩色地图：使地图中的占用单元根据 RGB 颜色进行可视化。

2.3 修改核心生成函数

在 octomap_server 的核心八叉树生成函数 insertCloudCallback 和 insertScan 中，添加对颜色的处理。

#ifdef COLOR_OCTOMAP_SERVER
  unsigned char* colors = new unsigned char[3];
#endif

// 仅在节点被占用时读取和解释颜色
#ifdef COLOR_OCTOMAP_SERVER 
    m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b);
#endif

• 颜色处理：在插入点云数据时，计算每个节点的平均 RGB 颜色，并存储在八叉树中。
• 内存管理：为颜色数据分配内存，确保颜色信息被正确处理和存储。

三、保存与显示地图

在成功构建八叉树地图后，需要保存和可视化地图以供后续使用。

3.1 保存地图

octomap_server 提供了地图保存服务，可以将构建的地图保存为压缩的二进制格式。

保存压缩二进制地图

octomap_saver mapfile.bt

• 命令说明：将当前八叉树地图保存为 mapfile.bt 文件，采用压缩的二进制格式，适合存储和传输。

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

octomap_saver -f mapfile.ot

• 命令说明：将当前八叉树地图保存为 mapfile.ot 文件，包含完整的概率信息，适用于需要详细地图信息的应用场景。

3.2 可视化地图

为了查看保存的八叉树地图，可以使用 octovis 可视化工具。

安装 octovis

sudo apt-get install octovis

• 安装说明：通过包管理器安装 octovis 工具，用于可视化八叉树地图。

使用 octovis 查看地图

octovis mapfile.ot

• 命令说明：启动 octovis 并加载 mapfile.ot 地图文件，进行可视化查看。

四、源码阅读笔记

在理解 octomap_server 的工作机制时，源码阅读是必不可少的。以下是对关键流程的整理与分析：

4.1 订阅点云的回调函数

octomap_server 通过订阅点云话题，接收传感器数据并触发回调函数。

回调函数流程

1. 接收点云数据：订阅 /cloud_in 话题，接收到 sensor_msgs/PointCloud2 消息。
2. 获取 TF 变换：利用 TF 监听器获取当前点云数据到全局坐标系的变换。
3. 调用插入函数：将转换后的点云数据传递给八叉树插入函数，实现地图更新。

关键代码片段

tf::StampedTransform sensorToWorldTf;
try {
    m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
} catch(tf::TransformException& ex){
    ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
    return;
}

• TF 获取：尝试获取传感器坐标系到全局坐标系的变换。如果失败，记录错误并跳过当前帧。

4.2 插入单帧点云构建八叉树

将单帧点云数据插入到八叉树中，实现地图的更新。具体过程涉及点云数据的遍历、过滤、转换及插入。

插入过程概述

1. 点云预处理：根据配置参数进行滤波，如地面滤波、高度限制等。
2. 点云转换：将点云数据从传感器坐标系转换到全局坐标系。
3. 八叉树更新：遍历点云中的每个点，更新八叉树中的对应体素概率。
4. 颜色处理（如启用 ColorOctomap）：计算并存储每个体素的平均 RGB 颜色。

关键代码片段

for (auto it = cloud->begin(); it != cloud->end(); ++it) {
    // 处理每个点
    // ...

#ifdef COLOR_OCTOMAP_SERVER
    m_octree->averageNodeColor(it->x, it->y, it->z, it->r, it->g, it->b);
#endif
}

• 点云遍历：遍历点云中的每个点，根据条件进行处理和插入。
• 颜色计算：在启用彩色八叉树时，计算每个节点的平均颜色。

4.3 关键函数概述

octomap_server 的核心功能集中在以下两个关键函数中：

1. insertCloudCallback：负责接收点云数据，获取 TF 变换，并调用插入函数更新八叉树。
2. insertScan：将单帧点云数据插入到八叉树中，完成地图的更新。

通过对这两个函数的理解，可以深入掌握 octomap_server 的建图流程。

五、建图示例

为了更好地理解和应用 octomap_server，以下提供了一个实际的建图示例，包括配置文件和建图结果展示。

5.1 Launch 文件示例

以下是一个实际的 Launch 文件配置示例，用于启动 octomap_server 节点并配置相关参数：

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- 地图分辨率，单位为米 -->
    <param name="resolution" value="0.15" />

    <!-- 全局坐标系，定义为 "camera_init" -->
    <param name="frame_id" type="string" value="camera_init" />

    <!-- 传感器模型的最大范围，单位为米 -->
    <param name="sensor_model/max_range" value="15.0" />

    <!-- 基座坐标系，用于地面滤波（项目中未启用） -->
    <!-- <param name="base_frame_id" type="string" value="base_link" /> -->

    <!-- 地面滤波配置（项目中未启用） -->
    <!--
    <param name="filter_ground" type="bool" value="true" />
    <param name="ground_filter/distance" type="double" value="1.0" />
    <param name="ground_filter/plane_distance" type="double" value="0.3" />
    -->

    <!-- 点云 Z 轴范围限制 -->
    <!--
    <param name="pointcloud_max_z" type="double" value="100.0" />
    <param name="pointcloud_min_z" type="double" value="-1.0" />
    -->

    <!-- 斑点滤波（项目中未启用） -->
    <!-- <param name="filter_speckles" type="bool" value="true" /> -->

    <!-- 禁用高度图并启用彩色地图 -->
    <param name="height_map" value="false" />
    <param name="colored_map" value="true" />

    <!-- 增加半径滤波器参数 -->
    <param name="outrem_radius" type="double" value="1.0" />
    <param name="outrem_neighbors" type="int" value="10" />

    <!-- 设置 latch 为 false，以提高建图性能 -->
    <param name="latch" value="false" /> 

    <!-- 订阅的点云话题重映射，将 "/cloud_in" 重映射为 "/fusion_cloud" -->
    <remap from="/cloud_in" to="/fusion_cloud" />
  </node>
</launch>

主要配置说明

• 分辨率设置：将地图分辨率设置为 15 厘米，适用于需要中等精度的应用场景。
• 坐标系设置：全局坐标系定义为 camera_init，确保与 TF 树中的变换一致。
• 传感器模型范围：限制传感器数据的最大插入范围为 15 米，避免远距离的噪声点影响地图质量。
• 地面滤波：项目中未启用地面滤波，注释掉相关参数。
• 彩色地图：禁用高度图，启用彩色地图，以 RGB 颜色显示占用单元。
• 半径滤波器：通过 outrem_radius 和 outrem_neighbors 参数配置半径滤波器，进一步优化点云数据。
• 性能优化：设置 latch 为 false，避免频繁发布主题，提高建图性能。
• 话题重映射：将订阅的点云话题从 /cloud_in 重映射为 /fusion_cloud，确保与实际数据源匹配。

5.2 建图结果展示

通过上述配置，成功构建了学院后方的一条小路地图，分辨率为 15 厘米。地图清晰地展示了环境中的障碍物和结构细节，证明了 octomap_server 的有效性。

地图截图

（此处应插入实际的建图结果截图，展示八叉树地图的可视化效果）

六、总结与小贴士

6.1 关键配置的重要性

• 输入点云话题：确保 octomap_server 正确订阅到有效的点云数据话题，是实现准确建图的前提。
• TF 变换：准确的 TF 变换关系确保每一帧点云数据被正确地转换到全局坐标系中，避免地图构建错误。

6.2 彩色八叉树地图的优势

• 视觉效果提升：彩色八叉树地图通过 RGB 颜色显示占用单元，提升了地图的可视化效果，便于识别不同物体和区域。
• 信息丰富：颜色信息可以用于进一步的语义理解和高级应用，如物体识别和分类。

6.3 处理缺少 TF 的情况

• 静态 TF 发布：在缺少必要的 TF 变换时，可以手动发布静态 TF 进行测试，确保建图过程不受影响。
• 动态 TF 更新：在使用 SLAM 等方法时，确保 TF 变换动态更新，反映机器人的实时位姿变化。

6.4 参数优化建议

• 分辨率选择：根据应用需求选择合适的地图分辨率，权衡精细度与计算开销。
• 滤波参数调整：根据环境特点调整地面滤波、斑点滤波等参数，优化点云数据质量。
• 范围限制：合理设置传感器模型的最大范围，避免远距离噪声点干扰地图构建。

6.5 源码阅读的重要性

• 深入理解：通过阅读源码，深入理解 octomap_server 的工作机制，有助于进行定制化开发和问题排查。
• 功能扩展：了解核心函数的实现，便于在现有基础上进行功能扩展和优化。

6.6 常见问题与解决方案

• TF 变换错误：如果 octomap_server 无法获取必要的 TF 变换，检查 TF 树中是否存在正确的 cloud frame -> world frame 变换。
• 地图更新缓慢：调整 latch 参数、优化滤波参数，确保地图更新性能与实时性。
• 颜色显示异常：确保在源码中正确启用 COLOR_OCTOMAP_SERVER，并正确配置 colored_map 参数。

七、参考资料与扩展阅读

• ROS 官方文档：了解更多 ROS 中 TF、点云处理和八叉树地图构建的详细信息。
• OctoMap 官方文档：深入学习 OctoMap 库的功能和应用场景。
• 相关开源项目：参考社区中的开源项目，学习不同应用场景下的八叉树地图构建方法。

通过本文的详细讲解，读者可以系统地掌握在 ROS 环境下使用 octomap_server 实现八叉树地图的增量式构建方法，并根据实际需求进行优化和扩展。无论是初学者还是有经验的开发者，都能从中获得有价值的指导和参考。