ROS 八叉樹地圖構(gòu)建 - 使用 octomap_server 建圖過(guò)程總結(jié)！

之前的構(gòu)建語(yǔ)義地圖工作，最開始用的是 octomap_server：

https://github.com/OctoMap/octomap_mapping

后面換成了 semantic_slam octomap_generator：

https://github.com/floatlazer/semantic_slam)

不過(guò)還是整理下之前的學(xué)習(xí)筆記。

一、增量構(gòu)建八叉樹地圖步驟

為了能夠讓 octomap_server 建圖包實(shí)現(xiàn)增量式的地圖構(gòu)建，需要以下 2 個(gè)步驟：

1.1 配置 launch 啟動(dòng)參數(shù)

這 3 個(gè)參數(shù)是建圖必備：

• 地圖分辨率 resolution：用來(lái)初始化地圖對(duì)象
• 全局坐標(biāo)系 frame_id：構(gòu)建的全局地圖的坐標(biāo)系
• 輸入點(diǎn)云話題 /cloud_in：作為建圖的數(shù)據(jù)輸入，建圖包是把一幀一幀的點(diǎn)云疊加到全局坐標(biāo)系實(shí)現(xiàn)建圖

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    
    <param name="resolution" value="0.10" />

    
    <param name="frame_id" type="string" value="map" />

    
    <remap from="/cloud_in" to="/fusion_cloud" />
  node>
launch>

以下是所有可以配置的參數(shù)：

• 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))
• 動(dòng)態(tài)構(gòu)建地圖時(shí)用于插入點(diǎn)云數(shù)據(jù)的最大范圍（以米為單位），將范圍限制在有用的范圍內(nèi)（例如 5m）可以防止虛假的錯(cuò)誤點(diǎn)。
• sensor_model/[hit|miss] (float, default: 0.7 / 0.4)
• 動(dòng)態(tài)構(gòu)建地圖時(shí)傳感器模型的命中率和未命中率
• sensor_model/[min|max] (float, default: 0.12 / 0.97)
• 動(dòng)態(tài)構(gòu)建地圖時(shí)的最小和最大 clamp 概率
• latch (bool, default: True for a static map, false if no initial map is given)
• 不管主題是鎖定發(fā)布還是每次更改僅發(fā)布一次，為了在構(gòu)建地圖（頻繁更新）時(shí)獲得最佳性能，請(qǐng)將其設(shè)置為 false，如果設(shè)置為 true，在每個(gè)地圖上更改都會(huì)創(chuàng)建所有主題和可視化。
• 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)
• 動(dòng)態(tài)構(gòu)建地圖時(shí)是否應(yīng)從掃描數(shù)據(jù)中檢測(cè)并忽略地平面，這會(huì)將清除地面所有內(nèi)容，但不會(huì)將地面作為障礙物插入到地圖中。如果啟用此功能，則可以使用 ground_filter 對(duì)其進(jìn)行進(jìn)一步配置
• ground_filter/distance (float, default: 0.04)
• 將點(diǎn)（在 z 方向上）分割為接地平面的距離閾值，小于該閾值被認(rèn)為是平面
• ground_filter/angle (float, default: 0.15)
• 被檢測(cè)平面相對(duì)于水平面的角度閾值，將其檢測(cè)為地面
• ground_filter/plane_distance (float, default: 0.07)
• 對(duì)于要檢測(cè)為平面的平面，從 z = 0 到距離閾值（來(lái)自 PCL 的平面方程的第 4 個(gè)系數(shù)）
• pointcloud_[min|max]_z (float, default: -/+ infinity)
• 要在回調(diào)中插入的點(diǎn)的最小和最大高度，在運(yùn)行任何插入或接地平面濾波之前，將丟棄此間隔之外的任何點(diǎn)。您可以以此為基礎(chǔ)根據(jù)高度進(jìn)行粗略過(guò)濾，但是如果啟用了 ground_filter，則此間隔需要包括接地平面。
• occupancy_[min|max]_z (float, default: -/+ infinity)
• 最終 map 中要考慮的最小和最大占用單元格高度，發(fā)送可視化效果和碰撞 map 時(shí)，這會(huì)忽略區(qū)間之外的所有已占用體素，但不會(huì)影響實(shí)際的 octomap 表示。
• filter_speckles(bool)
• 是否濾除斑

1.2 要求 TF 變換

有了全局坐標(biāo)系和每一幀的點(diǎn)云，但是建圖包怎么知道把每一幀點(diǎn)云插入到哪個(gè)位置呢？

因?yàn)殡S著機(jī)器人的不斷移動(dòng)，會(huì)不斷產(chǎn)生新的點(diǎn)云幀，每個(gè)點(diǎn)云幀在全局坐標(biāo)系中插入的時(shí)候都有一個(gè)確定的位置，否則構(gòu)建的地圖就不對(duì)了，因此需要給建圖包提供一個(gè)當(dāng)前幀點(diǎn)云到全局坐標(biāo)系的位姿，這樣建圖包才能根據(jù)這個(gè)位姿把當(dāng)前獲得的點(diǎn)云插入到正確的位置上。

在 ROS 中可以很方便的使用 TF 來(lái)表示這個(gè)變換，我們只需要在啟動(dòng)建圖包的時(shí)候，在系統(tǒng)的 TF 樹中提供「cloud frame -> world frame」的變換就可以了：

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

注意：

• cloud frame：就是輸入點(diǎn)云話題中 head.frame_id，比如 Robosense 雷達(dá)的 frame_id = rslidar
• world frame：這是全局坐標(biāo)系的 frame_id，在啟動(dòng) launch 中需要手動(dòng)給定，比如我給的是 map

如果你給 world frame id 指定的是輸入點(diǎn)云的 frame_id，比如 fusion_cloud.head.frame_id == wolrd_frame_id == rslidar，則只會(huì)顯示當(dāng)前幀的八叉樹，而不會(huì)增量構(gòu)建地圖，這點(diǎn)要注意了，可以自己測(cè)試看看。

那么為了增量式建圖，還需要在系統(tǒng)的 TF 樹中提供「rslidar -> world」的變換，這個(gè)變換可以通過(guò)其他的 SLAM 等獲得，比如我測(cè)試時(shí)候的一個(gè) 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;
  }

總體來(lái)說(shuō)這個(gè)建圖包使用起來(lái)還是挺簡(jiǎn)單的，最重要的就是要寫清楚輸入點(diǎn)云話題和 TF 變換。

小 Tips：沒(méi)有 TF 怎么辦？

我剛開始建圖的時(shí)候，我同學(xué)錄制的 TF 樹中并沒(méi)有「world -> rslidar」的變換，只有「world -> base_link」，所以為了能夠測(cè)試增量式建圖，因?yàn)槲业狞c(diǎn)云幀的 frame_id 是 rslidar，因此我就手動(dòng)發(fā)布了一個(gè)靜態(tài)的「base_link -> rslidar」的變換：

rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar

這樣系統(tǒng)中就有「rslidar -> world」的變換了，但是我發(fā)的位姿都是 0，所以對(duì)建圖測(cè)試沒(méi)有影響，為了方便啟動(dòng)，放在 launch 中：

如果你也遇到這個(gè)問(wèn)題，可以試試發(fā)個(gè)靜態(tài) TF 做做測(cè)試，關(guān)于靜態(tài) TF 詳細(xì)技術(shù)可以參考之前的文章：ROS 機(jī)器人技術(shù) - 靜態(tài) TF 坐標(biāo)幀

二、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
// 打開這個(gè)注釋
#define COLOR_OCTOMAP_SERVER

#ifdef COLOR_OCTOMAP_SERVER
  typedef pcl::PointXYZRGB PCLPoint;
  typedef pcl::PointCloud
     
       PCLPointCloud;
      
  
      typedef octomap::ColorOcTree OcTreeT;
      

      #else
      
  
      typedef pcl::PointXYZ PCLPoint;
      
  
      typedef pcl::PointCloud
      
        PCLPointCloud;
       
  
       typedef octomap::OcTree OcTreeT;
       

       #endif
       

      
     

CMakeList.txt 文件中有 COLOR_OCTOMAP_SERVER 的編譯選項(xiàng)：

target_compile_definitions(${PROJECT_NAME}_color PUBLIC COLOR_OCTOMAP_SERVER)

OctomapServer.cpp 中有 colored_map 的參數(shù)：

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

地圖默認(rèn)是按照高度設(shè)置顏色，如果要設(shè)置為帶顏色的地圖，就要禁用 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 個(gè)核心的八叉樹生成函數(shù) insertCloudCallback 和 insertScan 中有對(duì)顏色的操作：

#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

三、保存和顯示地圖

啟動(dòng) octomap_server 節(jié)點(diǎn)后，可以使用它提供的地圖保存服務(wù)，保存壓縮的二進(jìn)制存儲(chǔ)格式地圖：

octomap_saver mapfile.bt

保存一個(gè)完整的概率八叉樹地圖：

octomap_saver -f mapfile.ot

安裝八叉樹可視化程序 octovis 來(lái)查看地圖：

sudo apt-get install octovis

安裝后重啟終端，使用以下命令顯示一個(gè)八叉樹地圖：

octovis xxx.ot[bt]

四、源碼閱讀筆記

在開組會(huì)匯報(bào)的時(shí)候，整理了以下這個(gè)建圖包的關(guān)鍵流程，只有 2 個(gè)關(guān)鍵的函數(shù)也不是很復(fù)雜，我給代碼加了注釋，在文末可以下載。

第一步是訂閱點(diǎn)云的回調(diào)：

第二步是插入單幀點(diǎn)云構(gòu)建八叉樹，這里就不詳細(xì)介紹過(guò)程了，因?yàn)樯婕暗桨瞬鏄鋷?kù) octomap 的更新原理：

放一張我們學(xué)院后面的一條小路的建圖結(jié)果，分辨率是 15cm：

以下是我建圖過(guò)程的 launch：

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    
    <param name = "resolution" value = "0.15" />

    
    
    <param name = "frame_id" type = "string" value = "camera_init" />

    
    <param name = "sensor_model/max_range" value = "15.0" />

    
    

    
 
    
    
 

    
    
    

    

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

    
    <param name = "latch" value = "false" /> 

    
    
    
    <remap from = "/cloud_in" to = "/fusion_cloud" />
 
  node>
launch>

我做的項(xiàng)目代碼在這里 AI - Notes: semantic_map：

https://github.com/DLonng/AI-Notes/tree/master/SemanticMap/semantic_map_ws