Zenn
🗺️

[Han+,2019]よく見かけるSDFを最新に保ち続ける手法FIESTAの論文紹介

に公開
マップ
robot
sdf
tech

FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots

  • (論文内Fig.1 b)

https://www.youtube.com/watch?v=pgRi8LOnT6Y

なぜこの論文を選んだか

  • 移動や制御の勉強をしていると時々SDF(Signed Distance Fields)はFIESTAで作りましたという論文を見かける
  • SDFがあれば障害物までどれくらい余裕があるかやどちらに避ければ衝突を回避できるかの情報も得られ，捗る
  • 動的な情報更新のある状況でどのように周辺環境の情報を管理するのかの1つ具体例として知っておいて損はないと思い読むに至った

一言でいうと

高速かつ正確にSDFをアップデートできるデータ構造とアルゴリズムを考えた

論文リンク

著者/当時の所属機関

  • Luxin Han さん/ 香港科技大学(HKUST)
    • 大学ご卒業後，DJI社→現在はXPENG社で自動運転をやられている
    • Google scholarは登録なし
  • Fei Gao さん / 香港科技大学
    • 2025年だけでもう８本（4/27現在）共著があり，どれも面白そうな論文ばかり
    • 現在は浙江大学(Zhèjiāng Dàxué)の助教をされている
  • Boyu Zhou さん / 香港科技大学
    • 現在は南方科技大学（Southern University of Science and Technology (SUSTech))の助教をされている
  • Shaojie Shen さん / 香港科技大学(現在も)
    • 単眼＋IMUの自己位置推定システムでバズりまくっている
    • 助教の先生で現在も所属は変わらず
    • 現在 Senior editor for ICRA 2024-2026と Associate editor IJRR 2023-2024 をやられている
    • DJI社との共同研究Labも監督してるらしい出典

学会/投稿年

  • IROS 2019

技術や手法のキモは？

お題

深度測定値と自己位置所与な前提で，グローバルなESDF(Euclidean Signed Distance Field)マップをインクリメンタルに高速構築・更新したい

  • Input
    • 自己位置推定情報（世界座標系）\in {\rm SE}(3)
    • 深度測定値（ステレオ／RGB-Dカメラまたは単眼深度推定など）
  • Output
    • 世界座標系上のESDFマップ
      • 任意の座標(x,y,z)に対してESDF値とその勾配が計算できる

提案手法

  • STEP1: Raycasting
    • 論文での記述がないので，私の思う一般論で補足
    • Lay（図中点線）からわかる情報をOccupancy情報に変換
      • 貫通したところがFree space(図中白)，ぶつかった所が障害物（図中濃い灰色），それ以外は情報なし
      • 図は，私が手動でLaycastingしたイメージ図
  • STEP2: Update Occupancy
    • Raycastでの観測情報をOccupancy Grid Mapに反映させ，ESDFマップの方に追加／削除したい障害物のマスをキューに追加する
    • この項目についてほとんど記述がないが，私の思う一般論で補足
      • ベイズフィルタ的に考えて各グリッドの占有確率をアップデートする
        • 神ブログに紹介は譲る（@akinamiさんの記事は絵も多くわかりやすく，かえるも可愛い）
  • STEP3: ESDF Update Intilialization
    • 次に続く本アップデートに備えての準備
      • ESDFマップに障害物を追加したい
        -
        • ESDFマップの当該グリッドの距離を０で上書きする
        • 次に続く更新の起点キューにそのグリッドを登録
      • ESDFマップから障害物を削除したい
        • その障害物を最近傍として登録していたESDFマップ上の点全てを逆引き
        • それらのESDF値を∞に，最近傍をIdeal Point(null)に初期化
        • 初期化されたグリッド群の隣接グリッドのうち，他の最近傍を持つものがあれば，その最近傍を自らの最近傍とし，SDF値を書き換える．そしてこれを後段の更新の起点のキューに追加する
  • STEP4: ESDF Update
    • 本アップデート．前段で指定した起点のキューから幅優先探索でESDFマップを更新する
      • 起点のキューからポップされたら，隣接グリッドへ自分の最近傍点の情報を渡し，最近傍距離の書き換えが生じた際にはそのグリッド自体も新たな起点キューに加える
      • これを繰り返すと，書き換えが必要な箇所のみにWave Front Propagationしていく挙動となる
      • キューはFIFO(複雑度は\Theta(k),kは更新対象セル)でも距離の優先度キュー(n\log (n))でもOK
      • マップを覆う波の伝搬の波源のイメージ
    • 逐次情報が増える際に起こる問題と対処方法
      • 未観測領域にSDF値を伝播できないため，後から分かったフリースペースでのSDF値が過大評価されてしまう
      • Patch Codeを追加し，対処
        • 隣接グリッドに書かれた最近傍に対して自らと最近傍計算し，書き換えが必要な場合はその値を書き換え，自分自身を新たにキューに追加する
        • こうすることで，既存障害物からの距離も伝播するようになる

先行研究へのツッコミ

  • Voxblox [7] は TSDF → ESDF 変換に幅優先探索（波の伝搬的な動きをする）を用い，折れ線距離（quasi-Euclidean）および TSDF 投影距離による誤差を含む
    • 課題１：折れ線距離となるイメージ
    • 課題２：TSDF投影誤差の説明
      • lay上の障害物しか考慮しないので誤差がのる
  • Felzenszwalb & Huttenlocher [15] の全体 ESDF 一括計算はリアルタイム更新を想定せず，インクリメンタル更新コストが高い
  • 局所スライディングマップ方式 [6] は過去マップ情報を破棄するため，グローバル経路計画には不向き ​
    • 自分を中心とする球のバッファで周辺のみの障害物情報を保持，移動に際しては現在情報をずらして再利用する手法

どうやって有効だと検証した？

  • オープン実データセットに対して，Voxbloxとの比較
    • 更新にかかる平均時間と，精度を計測(ボクセルサイズを変えて実験)
    • データセットは，屋内２分前後のもので自己位置がモーションキャプチャ，深度はkinect/ステレオカメラ
    • GTは，データセットの全点群でOccupancy Grid Mapを作成→KD-treeで最近傍取得を高速化し，全グリッドに対してRMSE(平均2乗誤差)を計算
    • 結果
      • 計算時間と精度において同時に提案手法が優位となった（両方一桁くらい差をつけた）．理由としては，Voxbloxに比べる近似が少ないため正確となったといえそう
  • シミュレーションと実機でドローンを飛ばしてみた
    • 同一ボード(インテルi7-5500Uを搭載した機上コンピュータ)で20Hz， 最適化ベースの動作計画手法（Covariant Hamiltonian Optimization for Motion Planning(CHOMP)）でESDFを利用して同時にちゃんと動いた
    • 未知環境でのリアルタイムマッピング＋再計画が可能という結論
    • 3D LiDARはVelodyne VLP-16

議論はある？

  • 最適性の限界
    • 幅優先探索ベースの更新は離散化したグリッド表現を用いるため理論的に完全最適にならず残差誤差が発生（Section V-E, Fig. 4） ​
  • 性能・メモリのトレードオフ
    • グリッドの考慮する隣接数やインデックス構造選択による影響が大きく，実環境に応じたパラメータ調整が必須 ​
  • 動的障害物／スケーラビリティ
    • Patch Code箇所のオーバーヘッドや大規模環境での性能維持は課題となる

次に読むべき論文は？

感想

  • SDFは障害物回避に便利というつかみで，やっぱりそうだよねとなった
  • 勾配計算時に有限差分使わなくて良いので利用時に高速というメリットもありそう（最近傍の座標がわかるので解析的に勾配が出る）
  • 今までSDF作る時は，全点VS全グリッドの総当たりか，Fast Marching Methodを使って作っていたが，アルゴリズムとデータ構造を頑張ればいい感じにリアルタイムなSDFが作れそうということが分かり嬉しい
  • 今回は自己位置推定とDepthは所与だったが，それらも同時に推定する問題設定に挑戦している猛者論文があれば読んでみたい（自分がやりたいとは言っていない）
  • 2019年と割と古いので，この論文の後続研究がどうなっているかも気になる

補足

各データ構造について整理してみる

  • Occupancy Grid と Voxel infomation structure
  • Indexing Data Structure
    • (x,y,z)に対応するグリッドのインスタンスにアクセスできるようにしたい
    • hash値が未登録の場合はデフォルト値を返せば良い
    • 開始時，hash tableは空
    • 追加はray castで観測された時にハッシュテーブルに追加していく
    • 原理上は任意の広さのマップにも対応できる

3D-LiDAR情報 Velodyne VLP-16

データセット情報

  • （GPTo4-high miniに調べてもらったもの）
  1. Cow & Lady データセット
  • シーン：室内の小さなオフィス空間に，「牛（Cow）の置物」「マネキン（Lady）」「机や椅子などの事務的アクセサリ」を配置した環境
  • センサー構成：Microsoft Kinect v1（RGB＋深度）
  • 収録内容：
    • ROS bag（data.bag）に深度点群 /camera/depth_registered/points
    • Vicon モーションキャプチャによる６DoF ポーズ /kinect/vrpn_client/estimated_transform
    • キャリブレーション情報＋Leica MS50 ランサースキャナによる高精度構造 GT（PLY 点群）
  • 長さ：142 秒（2 分 22 秒）
  • データ量：未圧縮で約25.9 GB（圧縮後 4.5 GB）、メッセージ数 16 410 件
  1. EuRoC MAV データセット
    • シーン：
      1. Machine Hall（工場内のマシンホール）
      2. Vicon Room（モーションキャプチャ付き実験室）
    • センサー構成：
      • ステレオカメラ：Aptina MT9V034（WVGA モノクロ，グローバルシャッタ，20 FPS，752×480）
      • IMU：ADIS16448（角速度・加速度，200 Hz）
      • Ground-Truth：Vicon（6DoF），Leica MS50（3Dレーザトラッカ）
    • シーケンス数：全11シーケンス
      • Machine Hall：MH_01_easy (約1 分14 秒), MH_02_medium (約1 分53 秒), MH_03_difficult (約1 分50 秒)
      • Vicon Room：V1_01_easy (約1 分10 秒), V1_02_medium (約1 分47 秒), V1_03_difficult (約1 分27 秒)
        V2_01_easy (約1 分30 秒), V2_02_medium (約1 分20 秒), V2_03_difficult (約1 分37 秒)
    • 用途：ビジュアル‐慣性 SLAM／マッピング評価用の標準ベンチマーク (kmavvisualinertialdatasets – ASL Datasets)

残された疑問

  • Q.Doubly linked listにするうまみは？
    • 本文には追加・削除がO(1)と書いてある
    • もし，子のみが書かれたデータ構造で持つ場合，一度頭にもどってから，子を辿って自分を探さなければいけないのでO（リストサイズ）かかってしまうのに比べると早いということと思われる
  • Q.リアル実験の自己位置はどうやって出したの？
    • 記述なし．３DLiDARを積んでるので，点群ベースSLAMが走ってると思われる

Discussion