SuMa

Behley (Bonn) 2018 · 論文

一行要約 — SuMa(Surfel-based Mapping)は、環境をサーフェルマップとして維持し、レンダリングされたレンジ画像ビュー上での射影的フレーム-モデルICPによって各新規スキャンを追跡することでリアルタイムLiDAR SLAMを実現し、手作りの特徴抽出なしでオンラインループ閉じ込みを伴う密なマップのLiDAR SLAMが都市規模で機能することを示した。

問題

レーザーベースのマッピングシステムは、位置合わせの前に大抵3D点群を削減する — 特徴(LOAM)、サブサンプリングされた点群、ボクセルグリッド、あるいはNDTマップ — のに対し、RGB-D SLAM(KinectFusion、ElasticFusion)由来の密なフレーム-モデル手法は利用可能な情報をすべて使う。この密なパラダイムを回転式の屋外LiDARに持ち込むには、(1) 高速なセンサー運動によるスキャン間の大きな変位、(2) 比較的疎な点群、(3) 大規模な環境という3つの課題に対処しなければならず、それらすべてをリアルタイムで、ループ閉じ込みをオフラインの後処理ではなくオンラインで統合しながら行う必要がある。

手法とアーキテクチャ

パイプラインは1スキャンあたり7つのステップを実行する: 前処理、モデルレンダリング、フレーム-モデルICP、マップ更新、ループ閉じ込み検出、ループ閉じ込み検証、そして(別スレッドでの)ポーズグラフ最適化である。

• 頂点/法線マップへの前処理: 各点群 $\mathcal{P}$ は、球面座標を用いた $\Pi:\mathbb{R}^3 \mapsto \mathbb{R}^2$ によって頂点マップ $\mathcal{V}_D$(KITTIのHDL-64Eの場合900×64)に投影される。

  $$u = \tfrac{1}{2}\left(1 - \arctan(y, x)\,\pi^{-1}\right) w, \qquad v = \left(1 - \left(\arcsin(z\, r^{-1}) + f_{\mathrm{up}}\right) f^{-1}\right) h,$$

  ここで $r = \lVert \mathbf{p} \rVert_2$ であり $f = f_{\mathrm{up}} + f_{\mathrm{down}}$ は垂直視野角である。法線マップ $\mathcal{N}_D$ は隣接する頂点の前方差分の外積によって計算される。

• 射影的フレーム-モデルICP: アクティブなサーフェルマップは直前の姿勢でモデルマップ $\mathcal{V}_M, \mathcal{N}_M$ にレンダリングされ、対応点は最近傍探索の代わりにピクセル参照から得られる。点-平面誤差

  $$E(\mathcal{V}_D, \mathcal{V}_M, \mathcal{N}_M) = \sum_{\mathbf{u} \in \mathcal{V}_D} \left( \mathbf{n}_u^{\top}\left( \mathbf{T}^{(k)}_{C_{t-1}C_t}\, \mathbf{u} - \mathbf{v}_u \right) \right)^2$$

  は、$\mathfrak{se}(3)$ 増分 $\delta = (\mathbf{J}^{\top}\mathbf{W}\mathbf{J})^{-1}\mathbf{J}^{\top}\mathbf{W}\mathbf{r}$ を用いたGauss–Newton法、Huber重み付け、外れ値除去(距離>2 mまたは法線角>30°)によって最小化される。

• 安定性フィルタリングを伴うサーフェルマップ: 各サーフェルは位置 $\mathbf{v}_s$、法線 $\mathbf{n}_s$、半径 $r_s$、生成/更新タイムスタンプ、そして二値ベイズフィルタで維持される安定性対数オッズ比を保持する。

  $$l_s^{(t)} = l_s^{(t-1)} + \mathrm{odds}\left(p_{\text{stable}} \cdot e^{-\alpha^2/\sigma_\alpha^2}\, e^{-d^2/\sigma_d^2}\right) - \mathrm{odds}(p_{\text{prior}}),$$

  ここで $\alpha$ は測定値とサーフェルの間の角度、$d$ は距離である。レンダリングされるのは安定なサーフェルのみであり、整合する測定値は指数移動平均($\gamma = 0.9$)によってサーフェルを更新する; 不安定な古いサーフェル(動的物体、雑多なもの)は除去される。

• 姿勢によるマップ変形: サーフェルの座標はその生成姿勢のフレームで表現されるため、ポーズグラフ最適化の後は姿勢を単に更新するだけでマップが補正される — 過去のスキャンの再統合は不要である。

• マップベースのループ閉じ込み: マップはアクティブ部分($t_u \geq t - \Delta_{\text{active}}$、$\Delta_{\text{active}} = 100$)と非アクティブ部分に分割される; オドメトリはアクティブ部分のみを使用し、ループ探索は非アクティブ部分のみを使用する。50 m以内の最も近い非アクティブな姿勢が複数のICP初期値で試され、候補は、マップとスキャンを合成した仮想ビューに対する残差が $E_{\text{map}} < \kappa_{\text{residual}} \cdot E_{\text{odom}}$($\kappa_{\text{residual}} = 1.15$)を満たす場合にのみ受理され、その後5回の連続スキャンにわたって検証されてから制約がgtsamのポーズグラフに追加される。

実験結果

• KITTIオドメトリの訓練セット(相対回転誤差 deg/100 m / 相対並進誤差 %): フレーム-フレームICP 0.9/2.9; フレーム-モデル 0.3/0.7; ループ閉じ込み付きフレーム-モデル 0.3/0.8 — LOAM −/0.8、Stereo LSD-SLAM 0.3/0.9、SOFT-SLAM 0.2/0.7と比較して。ループ閉じ込みはKITTIの相対指標をほとんど変えないが、大域的な軌跡の整合性を目に見えて改善する。
• KITTIテストセット: 回転誤差0.0032 deg/m、並進誤差1.4%(LOAM: 0.0017 deg/m、0.7%)。
• i7-6700 + GTX 960(4 GB)での実行速度: オドメトリ+マップ更新は平均31 ms(最大71 ms); ループ閉じ込みの検出と検証を含めると最大189 ms; スキャンあたり全体で48 ms — 約20 Hzであり、センサーレートの2倍である。
• 報告された失敗モード: 構造化された物体が少ないハイウェイ、そして(交通渋滞のような)常に動いている交通がサーフェルとして誤って統合されてしまう場合 — まさにこれがSuMa++がセマンティクスによって後に対処したギャップである。

SLAMにおける意義

SuMaは、短距離RGB-Dセンサー向けに開拓されたサーフェルベースの密なマッピングの考え方(ElasticFusion)を、はるかに広い距離レンジと不均一な点密度を持つ屋外の回転式LiDARに持ち込んだ。これにより、GPUでレンダリングされたレンジ画像と射影的ICPが標準的なLiDAR追跡機構として確立され、LOAMのような特徴ベースパイプラインに対する密なマップの代替手法が提示された。また、そのマップベースのループ閉じ込み基準は、重なりの少ないスキャンでどのようにループを検証すべきかを示した。そのレンジ画像パイプラインは、セマンティック拡張版のSuMa++や、その後の学習によるLiDARレンジ画像処理を直接可能にした。

関連ノート

• SuMa++
• LOAM
• Range image
• ElasticFusion
• ICP
• TSDF vs Surfel maps