構建語義地圖時，最開始用的是 [octomap_server](https://github.com/OctoMap/octomap_mapping)，後面換成了 [semantic_slam: octomap_generator](https://github.com/floatlazer/semantic_slam)，不過還是整理下之前的學習筆記。 ## 一、增量構建八叉樹地圖步驟 為了能夠讓 octomap_server 建圖包實現增量式的地圖構建，需要以下 2 個步驟： ### 1.1 配置 launch 啟動引數 這 3 個引數是建圖必備： - 地圖解析度 `resolution`：用來初始化地圖物件 - 全域性座標系 `frame_id`：構建的全域性地圖的座標系 - 輸入點雲話題 `/cloud_in`：作為建圖的資料輸入，建圖包是把一幀一幀的點雲疊加到全域性座標系實現建圖 ```xml ``` 以下是所有可以配置的引數： - `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」的變換就可以了： ```shell 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](https://github.com/OctoMap/octomap_mapping/blob/kinetic-devel/octomap_server/src/OctomapServer.cpp) 中尋找 TF 的部分： ```cpp 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」的變換： ```shell rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link rslidar ``` 這樣系統中就有「rslidar -> world」的變換了，但是我發的位姿都是 0，所以對建圖測試沒有影響，為了方便啟動，放在 launch 中： `