关于机器人:ROS-八叉树地图构建-使用-octomapserver-建图过程总结

构建语义地图时，最开始用的是 octomap_server，前面换成了 semantic_slam: octomap_generator，不过还是整顿下之前的学习笔记。

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

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

1.1 配置 launch 启动参数

这 3 个参数是建图必备：

• 地图分辨率 resolution：用来初始化地图对象
• 全局坐标系 frame_id：构建的全局地图的坐标系
• 输出点云话题 /cloud_in：作为建图的数据输出，建图包是把一帧一帧的点云叠加到全局坐标系实现建图

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.10" />

    <!-- 增量式构建地图时，须要提供输出的点云帧和动态全局帧之间的 TF 变换 -->
    <param name="frame_id" type="string" value="map" />

    <!-- 要订阅的点云主题名称 /fusion_cloud -->
    <remap from="/cloud_in" to="/fusion_cloud" />
  </node>
</launch>

以下是所有能够配置的参数：

• frame_id (string, default: /map)

  • Static global frame in which the map will be published. A transform from sensor data to this frame needs to be available when dynamically building maps.

• resolution (float, default: 0.05)

  • Resolution in meter for the map when starting with an empty map. Otherwise the loaded file’s resolution is used

• height_map (bool, default: true)

  • Whether visualization should encode height with different colors

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

  • Color for visualizing occupied cells when ~heigh_map=False, in range [0:1]

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

  • 动静构建地图时用于插入点云数据的最大范畴（以米为单位），将范畴限度在有用的范畴内（例如 5m）能够避免虚伪的谬误点。

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

  • 动静构建地图时传感器模型的命中率和未命中率

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

  • 动静构建地图时的最小和最大 clamp 概率

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

  • 不论主题是锁定公布还是每次更改仅公布一次，为了在构建地图（频繁更新）时获得最佳性能，请将其设置为 false，如果设置为 true，在每个地图上更改都会创立所有主题和可视化。

• base_frame_id(string, default: base_footprint)

  • The robot’s base frame in which ground plane detection is performed (if enabled)

• filter_ground(bool, default: false)

  • 动静构建地图时是否应从扫描数据中检测并疏忽地立体，这会将革除高空所有内容，但不会将高空作为障碍物插入到地图中。如果启用此性能，则能够应用 ground_filter 对其进行进一步配置

• ground_filter/distance (float, default: 0.04)

  • 将点（在 z 方向上）宰割为接地平面的间隔阈值，小于该阈值被认为是立体

• ground_filter/angle (float, default: 0.15)

  • 被检测立体绝对于水平面的角度阈值，将其检测为高空

• ground_filter/plane_distance (float, default: 0.07)

  • 对于要检测为立体的立体，从 z = 0 到间隔阈值（来自 PCL 的立体方程的第 4 个系数）

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

  • 要在回调中插入的点的最小和最大高度，在运行任何插入或接地平面滤波之前，将抛弃此距离之外的任何点。您能够以此为根底依据高度进行粗略过滤，然而如果启用了 ground_filter，则此距离须要包含接地平面。

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

  • 最终 map 中要思考的最小和最大占用单元格高度，发送可视化成果和碰撞 map 时，这会疏忽区间之外的所有已占用体素，但不会影响理论的 octomap 示意。

• filter_speckles(bool)

  • 是否滤除斑

1.2 要求 TF 变换

有了全局坐标系和每一帧的点云，然而建图包怎么晓得把每一帧点云插入到哪个地位呢？

因为随着机器人的一直挪动，会一直产生新的点云帧，每个点云帧在全局坐标系中插入的时候都有一个确定的地位，否则构建的地图就不对了，因而须要给建图包提供一个以后帧点云到全局坐标系的位姿，这样建图包能力依据这个位姿把以后取得的点云插入到正确的地位上。

在 ROS 中能够很不便的应用 TF 来示意这个变换，咱们只须要在启动建图包的时候，在零碎的 TF 树中提供「cloud frame -> world frame」的变换就能够了：

cloud frame -> world frame (static world frame, changeable with parameter frame_id)

留神：

• 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::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 变换。

小 Tips：没有 TF 怎么办？

我刚开始建图的时候，我同学录制的 TF 树中并没有「world -> rslidar」的变换，只有「world -> base_link」，所以为了可能测试增量式建图，因为我的点云帧的 frame_id 是 rslidar，因而我就手动公布了一个动态的「base_link -> rslidar」的变换：

rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar

这样零碎中就有「rslidar -> world」的变换了，然而我发的位姿都是 0，所以对建图测试没有影响，为了不便启动，放在 launch 中：

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

如果你也遇到这个问题，能够试试发个动态 TF 做做测试，对于动态 TF 具体技术能够参考之前的文章：ROS 机器人技术 – 动态 TF 坐标帧

二、ColorOctomap 启用办法

为了显示 RGB 色彩，我剖析了下源码，第一步批改头文件，关上正文切换地图类型：OctomapServer.h

// 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

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

target_compile_definitions(${PROJECT_NAME}_color PUBLIC COLOR_OCTOMAP_SERVER)

OctomapServer.cpp 中有 colored_map 的参数：

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

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

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

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

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

2 个外围的八叉树生成函数 insertCloudCallback 和 insertScan 中有对色彩的操作：

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

// NB: Only read and interpret color if it's an occupied node
#ifdef COLOR_OCTOMAP_SERVER 
        m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b);
#endif

三、保留和显示地图

启动 octomap_server 节点后，能够应用它提供的地图保留服务，保留压缩的二进制存储格局地图：

octomap_saver mapfile.bt

保留一个残缺的概率八叉树地图：

octomap_saver -f mapfile.ot

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

sudo apt-get install octovis

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

octovis xxx.ot[bt]

四、源码浏览笔记

在开组会汇报的时候，整顿了以下这个建图包的要害流程，只有 2 个要害的函数也不是很简单，我给代码加了正文，在文末能够下载。

第一步是订阅点云的回调：

第二步是插入单帧点云构建八叉树，这里就不具体介绍过程了，因为波及到八叉树库 octomap 的更新原理：

放一张咱们学院前面的一条小路的建图后果，分辨率是 15cm：

以下是我建图过程的 launch：

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->
    <param name = "resolution" value = "0.15" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <!-- 动态全局地图的 frame_id，但在增量式构建地图时，须要提供输出的点云帧和动态全局帧之间的 TF 变换 -->
    <param name = "frame_id" type = "string" value = "camera_init" />

    <!-- set min to speed up! -->
    <param name = "sensor_model/max_range" value = "15.0" />

    <!-- 机器人坐标系 base_link，滤除高空须要该 frame -->
    <!-- <param name = "base_frame_id" type = "string" value = "base_link" /> -->

    <!-- filter ground plane, distance value should be big! 我的项目并不需要滤除高空 -->
    <!-- <param name = "filter_ground" type = "bool" value = "true" /> -->
    <!-- <param name = "ground_filter/distance" type = "double" value = "1.0" /> -->
    <!-- 宰割高空的 Z 轴阈值 value 值 -->
    <!-- <param name = "ground_filter/plane_distance" type = "double" value = "0.3" /> -->

    <!-- 直通滤波的 Z 轴范畴，保留 [-1.0, 10.0] 范畴内的点 -->
    <!-- <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" />

    <!-- when building map, set to false to speed up!!! -->
    <param name = "latch" value = "false" /> 

    <!-- topic from where pointcloud2 messages are subscribed -->
    <!-- 要订阅的点云主题名称 /pointcloud/output -->
    <!-- 这句话的意思是把以后节点订阅的主题名称从 cloud_in 变为 /pointcloud/output -->
    <remap from = "/cloud_in" to = "/fusion_cloud" />
 
  </node>
</launch>

我做的我的项目代码在这里：AI – Notes: semantic_map