構(gòu)建語(yǔ)義地圖時(shí)，最開(kāi)始用的是 octomap_server督暂，后面換成了 semantic_slam: octomap_generator，不過(guò)還是整理下之前的學(xué)習(xí)筆記穷吮。

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

為了能夠讓 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">
    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.10" />

    <!-- 增量式構(gòu)建地圖時(shí)八回，需要提供輸入的點(diǎn)云幀和靜態(tài)全局幀之間的 TF 變換 -->
    <param name="frame_id" type="string" value="map" />

    <!-- 要訂閱的點(diǎn)云主題名稱 /fusion_cloud -->
    <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 樹(shù)中提供「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)前幀的八叉樹(shù)特碳，而不會(huì)增量構(gòu)建地圖，這點(diǎn)要注意了企量，可以自己測(cè)試看看测萎。

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

image

我找了下源代碼 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)單的，最重要的就是要寫(xiě)清楚輸入點(diǎn)云話題和 TF 變換恕汇。

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

我剛開(kāi)始建圖的時(shí)候或辖，我同學(xué)錄制的 TF 樹(shù)中并沒(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 中：

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

如果你也遇到這個(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 顏色财喳，我分析了下源碼，第一步修改頭文件斩狱，打開(kāi)注釋切換地圖類(lèi)型：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
// 打開(kāi)這個(gè)注釋
#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 的編譯選項(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ù)生成函數(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è)完整的概率八叉樹(shù)地圖：

octomap_saver -f mapfile.ot

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

sudo apt-get install octovis

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

octovis xxx.ot[bt]

四、源碼閱讀筆記

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

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

image

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

image

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

image

以下是我建圖過(guò)程的 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 -->
    <!-- 靜態(tài)全局地圖的 frame_id，但在增量式構(gòu)建地圖時(shí)缕题，需要提供輸入的點(diǎn)云幀和靜態(tài)全局幀之間的 TF 變換 -->
    <param name = "frame_id" type = "string" value = "camera_init" />

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

    <!-- 機(jī)器人坐標(biāo)系 base_link截歉，濾除地面需要該 frame -->
    <!-- <param name = "base_frame_id" type = "string" value = "base_link" /> -->

    <!-- filter ground plane, distance value should be big! 項(xiàng)目并不需要濾除地面 -->
    <!-- <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] 范圍內(nèi)的點(diǎn) -->
    <!-- <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 -->
    <!-- 要訂閱的點(diǎn)云主題名稱 /pointcloud/output -->
    <!-- 這句話的意思是把當(dāng)前節(jié)點(diǎn)訂閱的主題名稱從 cloud_in 變?yōu)?/pointcloud/output -->
    <remap from = "/cloud_in" to = "/fusion_cloud" />
 
  </node>
</launch>

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

image