ROS 八叉樹地圖構建 - 使用 octomap_server 建圖過程總結！

構建語義地圖時，最開始用的是 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