第6回3D勉強会＠関東「SLAM 勉強会」講演資料 https://3dvision.connpass.com/event/156241/

1. Cartographer を用いた
3D SLAM
千葉工業大学 未来ロボット技術研究センター
原 祥尭（HARA, Yoshitaka）
3D勉強会 2019-12-15
@ystk_hara

2. 自己紹介（略歴）
1
3D勉強会 2019-12-15
2007年：筑波大 油田･坪内･大矢研 修士
2007年～2011年：日立製作所 日立研究所
（旧、機械研究所）
2012年～2015年：学振特別研究員DC1
2015年：筑波大 坪内･大矢研 博士
2015年～：千葉工大 未来ロボット技術研究センター（fuRo）
北陽電機さまと共同開発
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

3. 自己紹介（ROS 解説）
2
3D勉強会 2019-12-15
“ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発”,
RSJ 第94回ロボット工学セミナー, 2015.
https://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar
“ROS を用いた自律移動ロボットのシステム構築”,
RSJ 第99回ロボット工学セミナー, 2016.
https://www.slideshare.net/hara-y/ros-nav-rsj-seminar
“解説：ROS を用いた自律走行”,
日本ロボット学会誌, vol. 35, no. 4, 2017.
https://www.jstage.jst.go.jp/article/jrsj/35/4/35_35_286/_article/-char/ja/
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

4. 自己紹介（SLAM 解説）
3
3D勉強会 2019-12-15
日経 Robotics 連載 “SLAM とは何か”, 全18回, 2016～2017.
“SLAM とは何か：自己位置推定と地図構築の全体像”, RSJ セミナー, 2018.
“ロボティクスにおける SLAM 手法と実用化例”, SSII OS, 2019.
https://www.slideshare.net/hara-y/slam-ssii-os-2019
“SLAM の基礎とオープンソース”, RSJ セミナー, 2019.
https://www.slideshare.net/hara-y/slam-rsj-seminar-2019
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

5. 各種の SLAM アプローチ4
3D勉強会 2019-12-15

6. Cartographer で構築した3次元地図による自律走行
5
3D勉強会 2019-12-15
https://youtu.be/njw9BPQUoa0
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

7. つくばチャレンジ全チームの SLAM アプローチ
6
3D勉強会 2019-12-15
Laser SLAM が圧倒的多数、
ただグラフベースはまだ少ない
RGB-D、Visual SLAM は少数、
屋外リアルワールドは難しい
SLAM 不使用チームの戦略
 オドメトリ地図で教示再生
 GNSS（GPS など）
 トポロジカル地図
 End-to-End 深層学習
13
25
8
2
2
15
0 5 10 15 20 25 30
Laser スキャンマッチング
Laser ベイズフィルタ
Laser グラフベース SLAM
RGB-D SLAM
Visual SLAM
SLAM 不使用
その他の統計は [原+, “つくばチャレンジ2018技術調査アンケートの報告”] を参照
https://www.slideshare.net/hara-y/tsukuba-challenge-2018-survey
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

8. 自律走行や SLAM のオープンソース利用状況
7
3D勉強会 2019-12-15
ROS で標準的な navigation と
slam_gmapping が多い
Autoware も多い
Cartographer は意外に少ない
（試用して諦めたとの声も）
製品である Stencil の利用も
Visual SLAM は RTAB-Map
と ORB-SLAM のみで少ない
18
20
1
1
8
2
1
1
2
1
0 5 10 15 20 25
ROS navigation
ROS slam_gmapping
ROS hector_slam
ROS slam_karto
Autoware
Cartographer
MRPT
Kaarta Stencil
RTAB-Map
ORB-SLAM
その他の統計は [原+, “つくばチャレンジ2018技術調査アンケートの報告”] を参照
https://www.slideshare.net/hara-y/tsukuba-challenge-2018-survey
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

9. オープンソース Laser SLAM の比較
8
3D勉強会 2019-12-15
 上記以外にも数多くの手法が存在する
 ループ検出とグラフ最適化によるループ閉じ込みが、特に広大な環境において重要
[友納, 原, “SLAM の現状と今後の展望”, システム制御情報学会誌, 2020]
Laser SLAM (LIDAR SLAM) と Visual SLAM (Camera SLAM) の全体像を解説
分類 次元 地図形式
スキャン歪み
対策
ループ検出 グラフ最適化
GMapping RBPF 2D 占有格子 × 非明示的 ×
Autoware
(ndt_mapping)
スキャン
マッチング
3D 点群 × × ×
Velodyne
SLAM
スキャン
マッチング
3D 点群 ○ × ×
LOAM
スキャン
マッチング
3D 点群 ○ × ×
Karto SLAM グラフベース 2D 占有格子 × 粗密探索 SPA
Cartographer グラフベース 2D, 3D 占有格子 ○ 分枝限定法 Ceres Solver
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

10. Cartographer を用いた SLAM9
3D勉強会 2019-12-15

11. Cartographer のアルゴリズム概要
10
3D勉強会 2019-12-15
 Voxel Filter
 固定サイズで点群を間引き
 Adaptive Voxel Filter
 最大サイズと最小点数で動的に間引き
 Local Scan Matching（逐次SLAM）
 Ceres スキャンマッチング／
相関スキャンマッチングから選択
 Ceres は初期位置からの局所マッチング
 Global Scan Matching（ループ検出）
 相関スキャンマッチングで大域探索
 Pose Adjustment（グラフ最適化）
 SPA でポーズ調整
 拘束の外れ値はM推定で対処
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

12. Cartographer は高性能だがハードルも高い
11
3D勉強会 2019-12-15
公式ドキュメント（順次更新されているが…）
https://google-cartographer.readthedocs.io/en/latest/
https://google-cartographer-ros.readthedocs.io/en/latest/
システムが複雑
コード行数：約6万行（約500ファイル）
パラメータ数：約100個
パラメータ調整が難しい（開発者ら自身が以下のように説明）
“アルゴリズム内部の十分な理解が必要で、パラメータ調整は本当に困難”
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

13. 3次元 Cartographer のデフォルトパラメータで構築
12
3D勉強会 2019-12-15
占有格子地図の平行投影
大域的な整合性が破綻、
局所的にはそれなり
黄色のループ拘束がほぼ
生成されていない
つくば市役所
研究学園駅前公園
再訪位置が同じ位置に
マッピングされず、
ループが閉じていない
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

14. パラメータ調整によるループ閉じ込みの実現
13
3D勉強会 2019-12-15
ループ拘束が適切
に生成されるよう
パラメータ調整
3次元は処理負荷大、
3～24時間程度
(Core i7 8750H)
処理時間が長く、
試行錯誤は困難
→自動化したいが…
つくば市役所
研究学園駅前公園
1,000 m
走行距離 6 km、約2時間走行
ループ閉じ込みによって
全体の整合性が取れている
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

15. 3次元点群地図の出力14
3D勉強会 2019-12-15

16. 3次元点群地図の出力
15
3D勉強会 2019-12-15
Cartographer 本体は、3次元地図を出力しない
 内部では、サブマップで構築された3次元占有格子地図を持つ
 サブマップの可視化は可能だが、ファイル出力機能はない
Assets Writer という後処理の枠組みにより、3次元点群地図を構築可能
 入力：
ポーズグラフ（pbstream）
スキャン点群（rosbag）
 パイプライン処理を設定する
 “write_pcd” 機能で点群地図を
PCDファイルに出力できる
Cartographer Assets Writer
Front-end
(Sequential/Loop
Registration)
Back-end
(Pose Graph
Optimization)
Various
Filtering
“write_pcd”
Scan
Cloud
IMU
Odom
Pose
Graph
Scan
Cloud Cloud
Map
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

17. 移動物体を除去するパイプラインを新たに追加
16
3D勉強会 2019-12-15
ボクセルを用いたバイナリベイズフィルタで移動物体を除去（新規実装）
生の点群地図には、歩行者や他ロボットなどの移動物体が残る
従来のバイナリベイズフィルタ
では、地面も誤って消えてしまう
→グリッド分割に伴う本質的問題
ビーム入射角を考慮したモデル
により、地面の点群を適切に残す
[AR, 2020 (accepted)] X, Y
Z
LIDAR
hit (occupied)
miss (free)
unknown
地面はビームが浅い照射となるため、
多数のビームが通過して自由空間と誤判定される
側面図
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

18. フィルタリングによる移動物体除去の比較
17
3D勉強会 2019-12-15
生の点群
頻度占有モデル（従来手法）
ベイズ占有モデル（従来手法）
入射角を考慮したベイズ占有モデル
移動物体やノイズが含まれる
歩行者などの軌跡が残る
地面が消えてしまい、ノイズは残ってしまう
移動物体は消えるが、地面も消えてしまう
移動物体が消え、地面は残る
[AR, 2020 (accepted)]
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

19. Cartographer を利用して構築した3次元地図
18
3D勉強会 2019-12-15
colored by intensity
自動で地図構築、手動での修正なし
ビーム入射角を考慮した移動物体除去
[AR, 2020 (accepted)]
受光強度で地面の舗装／芝生／白線などを区別できる
つくば市役所
研究学園駅前公園
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

20. Cartographer のパラメータ調整19
3D勉強会 2019-12-15

21. パラメータ調整の指針
20
3D勉強会 2019-12-15
確認すべきパラメータファイル
 cartographer/configuration_files/trajectory_builder_3d.lua
 cartographer/configuration_files/pose_graph.lua
 cartographer_ros/configuration_files/backpack_3d.lua
逐次 SLAM、ループ検出、グラフ最適化の順で調整すると良い
特にループ検出で十分な数のループ拘束を生成することが重要、
ループ拘束の外れ値はグラフ最適化（ポーズ調整）のM推定で対処
RViz で逐次拘束（サブマップ内）とループ拘束（サブマップ間）の
可視化を切り替えながら確認（重なって表示されると見にくいので）
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

22. 各パラメータの説明（センサ関連）
21
3D勉強会 2019-12-15
 TRAJECTORY_BUILDER_3D.min/max_range
TRAJECTORY_BUILDER_3D.high/low_resolution_adaptive_voxel_filter.max_range
LIDAR の仕様をベースに調整、ただし high_resolution を長くすると処理が重い
 TRAJECTORY_BUILDER_3D.voxel_filter_size
後述の submaps.high_resolution と整合するように調整
 TRAJECTORY_BUILDER_3D.num_accumulated_range_data（スキャン歪み対策）
LIDAR 点群のスキャン分割に合わせて調整、全周スキャン点群ごとに配信の場合は“1”、
センサハンドラでのスキャン分割（全周ごとではなく分割して配信）が必要
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

23. 各パラメータの説明（サブマップ関連）
22
3D勉強会 2019-12-15
 TRAJECTORY_BUILDER_3D.submaps.num_range_data
サブマップのサイズ、整合性が保てる範囲で大きい方がループ検出しやすい
 TRAJECTORY_BUILDER_3D.submaps.high/low_resolution
TRAJECTORY_BUILDER_3D.submaps.high_resolution_max_range
サブマップの解像度や反映する点群の最大距離
 TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit/miss_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.num_free_space_voxels
サブマップを構築する逆計測モデル（占有と自由のバイナリベイズフィルタ）
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

24. 各パラメータの説明（逐次 SLAM）
23
3D勉強会 2019-12-15
 TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation/rotation_weight
Ceres スキャンマッチングの初期位置（オドメトリ）からのペナルティ、
大きいとマッチングせず小さいと軌跡がジグザグになる
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

25. 各パラメータの説明（ループ検出、グラフ最適化）
24
3D勉強会 2019-12-15
 POSE_GRAPH.optimize_every_n_nodes
グラフ最適化（ポーズ調整）の頻度、グラフ最適化しないとサブマップ間の形状はズレる
 POSE_GRAPH.constraint_builder.sampling_ratio
POSE_GRAPH.constraint_builder.max_constraint_distance
ループ拘束を生成する軌跡ノードをサンプリングするレートと範囲、
サンプリングした軌跡ノードに対してループ検出する
 POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy/z_search_window
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.angular_search_window
ループ検出する相関スキャンマッチングの探索範囲、逐次 SLAM の誤差に合わせて調整
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

26. 各パラメータの説明（ループ検出、グラフ最適化）
25
3D勉強会 2019-12-15
 POSE_GRAPH.constraint_builder.min_score
ループ拘束を採用する最小スコア（0.0～1.0）
 POSE_GRAPH.constraint_builder.loop_closure_translation/rotation_weight
グラフ最適化でのループ拘束の重み
 POSE_GRAPH.optimization_problem.odometry_translation/rotation_weight
POSE_GRAPH.optimization_problem.acceleration/rotation_weight
グラフ最適化でのオドメトリや IMU の重み
 POSE_GRAPH.optimization_problem.huber_scale（拘束の外れ値対策）
グラフ最適化でのM推定の Huber loss
各種 SLAM Cartographer 3次元地図出力 パラメータ調整

27. まとめ26
3D勉強会 2019-12-15

28. まとめ
27
3D勉強会 2019-12-15
つくばチャレンジ全チームの SLAM アプローチの統計
→屋外リアルワールドでは、現状は Laser SLAM の方が強力
オープンソース Laser SLAM の比較
3次元 Cartographer を用いた地図構築の実例
Assets Writer の後処理による、3次元点群地図の出力
ビーム入射角を考慮したバイナリベイズフィルタによる移動物体除去
Cartographer のパラメータ調整ノウハウ
各種 SLAM Cartographer 3次元地図出力 パラメータ調整