Dual Quadric表現を用いた動的物体追跡とRGB-D・IMU制約の密結合によるオドメトリ推定

第31回ロボティクスシンポジア 2A2 発表スライド

Transcript

1. Dual Quadric表現を用いた動的物体追跡と RGB-D・IMU制約の密結合によるオドメトリ推定 島田 豊藏*1*2 小出 健司*2 高野瀬 碧輝*2 大石 修士*2 横塚 将志*2 三浦 純*1 *1 豊橋技術科学大学 *2 産業技術総合研究所 1
2. 2 概要
3. 3 概要 3 ⚫ 動的環境に対して極めて頑健なオドメトリ推定システムを提案 ⚫ 周囲物体の継続的な推定と点群・IMU位置推定をタイトカップリング ⚫ 周囲環境の情報を余すことなく位置推定に寄与させる
4. 4 研究背景・目的
5. 5 研究背景・目的 5 静的な環境 マッチング 動的な環境 マッチング 成功！ 失敗... ⚫ 点群SLAMにおけるスキャンマッチングでの例 SLAMは静的な環境を仮定 SLAMは一般に静的環境を仮定 ⇒ 動的環境では破綻
6. 6 研究背景・目的 6 ⚫ 動的な部分はノイズであるとみなし，センサデータから除去 ⚫ 残った静的な情報を用いて位置推定を行う 動的物体への従来の対処 RF-LIO [Qian+, IROS2021] DS-SLAM [Chao+, IROS2018] V3D-SLAM [Dang+, IROS2024]
7. 7 研究背景・目的 ⚫ 動的な部分はノイズであるとみなし，センサデータから除去 ⚫ 残った静的な情報を用いて位置推定を行う ⇒動的物体が多数存在するような極めて動的な環境では？ 7 動的物体への従来の対処 動的要素の検出→除去 残存データ同士で推定 マッチング 困難... 従来手法は本質的に周囲の環境情報を捨てる 極めて動的な環境では静的なデータが十分に残存せず推定は困難
8. 8 研究背景・目的 極めて動的な環境下でも破綻しないオドメトリ推定手法の開発 8 本研究の目的 アプローチ ⚫ 動的物体をノイズとして除去するだけでなく，Object SLAMの枠組みを拡張しランドマークとしてモデル化 ⚫ 周囲環境の情報を余すことなくオドメトリ推定に寄与させる Object SLAMの拡張 移動物体の継続的推定 点群-IMU SLAM 点群ノイズ除去 統合 動的物体の状態とセンサ姿勢を同時に推定 ⚫ 動的物体を点群から適応的に除去 ⚫ 動的物体をランドマーク利用
9. 9 手法
10. 10 Dual Quadric表現 10 Dual Quadric [Nicholson+, RA-L2018] ⚫ Object SLAMにおける物体表現手法のひとつ QuadricSLAM [Nicholson+, RA-L2018] 物体姿勢 物体半径 Dual Quadric ファクタグラフ最適化との相性 ◦ 任意物体のランドマーク化 Object SLAM ⚫ 周囲の物体レベルの情報を直接ランドマークとして利用 ⚫ 物体指向地図を作成 検出結果と物体間を滑らかに制約 物体を3次元楕円体として近似 画像で検出した任意物体を利用可能
11. 11 Dual Quadric表現 11 Dual Quadric [Nicholson+, RA-L2018] ⚫ Object SLAMにおける物体表現手法のひとつ QuadricSLAM [Nicholson+, RA-L2018] 物体姿勢 物体半径 Dual Quadric ファクタグラフ最適化との相性 ◦ 任意物体のランドマーク化 Object SLAM ⚫ 周囲の物体レベルの情報を直接ランドマークとして利用 ⚫ 物体指向地図を作成 検出結果と物体間を滑らかに制約 物体を3次元楕円体として近似 従来手法[Nicholson+, RA-L2018]では姿勢・形状の推定のみに留まる ⇒ 本研究では物体並進速度も推定変数として追加
12. 12 提案手法 12 RGB画像 深度点群 IMU YOLOv11 物体マスク画像 物体画像特徴量 物体バウンディング ボックス 物体抽出 物体観測の対応付け 動的点判別・除去 グラフ最適化 インスタンスセグメンテーション ⚫ 画像・深度点群・IMUを入力としてセンサと物体状態を同時に推定する ⚫ 物体推定結果をもとに点群から適応的に動的物体を除去 ⚫ 除去した箇所を物体観測の制約で補う形で最適化
13. 13 提案手法 13 周囲物体の抽出 YOLOv11 Instance Segmentation インスタンスセグメンテーション 視覚特徴ベクトルはYOLOのFeature Mapとマスク領域を照 合することで抽出
14. 14 提案手法 14 周囲物体の抽出 YOLOv11 Instance Segmentation インスタンスセグメンテーション Dual Quadricの初期パラメータ抽出 Point Cloud 楕円体 フィッティング ⚫ 物体マスク画像と点群情報を照合し物体の3次元点群を抽出 ⚫ 物体点群にフィットするような楕円体を大まかに推定 ⚫ ファクタグラフ最適化における初期値 視覚特徴ベクトルはYOLOのFeature Mapとマスク領域を照 合することで抽出
15. 15 提案手法 15 観測物体と追跡中の物体インスタンスを対応付け ➀ 物体運動 ➁ 視覚特徴の両方を用いてロバストに対応付けを行う 追跡中の物体インスタンス ⚫ 物体姿勢・形状 ⚫ 物体並進速度 ⚫ 視覚特徴分布 各追跡物体が保持する情報 観測物体 ？
16. 16 提案手法 16 観測物体と追跡中の物体インスタンスを対応付け ➀ 物体運動 ➁ 視覚特徴の両方を用いてロバストに対応付けを行う 追跡中の物体インスタンス ⚫ 物体姿勢・形状 ⚫ 物体並進速度 ⚫ 視覚特徴分布 各追跡物体が保持する情報 観測物体 ？ 過去に対応づいた物体の特徴分布を正規分布(768次元)として保持
17. 17 提案手法 17 物体運動を用いた対応候補の絞り込み 出現予測 各追跡物体に対し現在時刻における出現位置を予測 等速運動モデルを仮定
18. 18 提案手法 18 物体運動を用いた対応候補の絞り込み 各追跡物体に対し現在時刻における出現位置を予測 予測位置から一定距離内の観測物体を対応付け候補 等速運動モデルを仮定 候補！ 候補！ 出現予測 観測物体
19. 19 提案手法 19 物体視覚特徴分布を用いた対応決定 観測物体の視覚特徴量から各追跡物体の視覚特徴分布に対する 対数尤度を算出し，最も尤度が高いものを対応付け 対応決定！ 各追跡物体は，対応づいた過去の観測物体の特徴量から視覚特徴分布を構築・保持・更新する ⇒ 視覚特徴の変化に頑健な対応付けが可能
20. 20 提案手法 20 物体視覚特徴分布を用いた対応決定 各追跡物体は，対応づいた過去の観測物体の特徴量から視覚特徴分布を構築・保持・更新する ⇒ 視覚特徴の変化に頑健な対応付けが可能 観測物体の視覚特徴量から各追跡物体の視覚特徴分布に対する 対数尤度を算出し，最も尤度が高いものを対応付け 視覚特徴分布の更新 逐次更新アルゴリズム[B. P. Welford+, 1962] を適用することで分布の更新を行う ⇒ 過去の全特徴量を保持せずコスト小 対応決定！
21. 21 提案手法 21 観測物体の視覚特徴量から各追跡物体の視覚特徴分布に対する 対数尤度を算出し，最も尤度が高いものを対応付け 対応決定！ 動的点除去 対応先の物体が一定以上の速度を持つ ⇒ 観測物体に対応する点群除去 視覚特徴分布の更新 逐次更新アルゴリズム[B. P. Welford+, 1962] を適用することで分布の更新を行う ⇒ 過去の全特徴量を保持せずコスト小 物体視覚特徴分布を用いた対応決定 各追跡物体は，対応づいた過去の観測物体の特徴量から視覚特徴分布を構築・保持・更新する ⇒ 視覚特徴の変化に頑健な対応付けが可能
22. 22 提案手法 22 ファクタグラフ最適化 動的点除去後の点群レジストレーション＋IMU＋物体観測を制約としたファクタグラフ(目的関数)を 構築し，センサ状態と物体状態を同時に最適化する この目的関数を同時に最適化すること(タイトカップリング)により 点群マッチングが困難な環境でも物体観測で補いながら推定可能！
23. 23 IMU事前積分ファクタああああああ 点群レジストレーションファクタああああああああ 提案手法 23 点群・IMU制約 GLIM[Koide+, RAS2024]のオドメトリ推定ファクタグラフを利用 GPUで高速化されたVoxelized GICP誤差[Koide+, RA-L2021]を 直近5秒の全フレーム間で生成 ⇒ ウィンドウ内の点群全体でレジストレーション誤差を最小化 ⇒ 動的点除去後の疎な点群同士の小さな重なりまで評価 IMUデータを積分して得た相対運動と状態から予測される相対 運動の誤差を最小化[Forster+, T-RO2017] ⇒ センサの高速な運動に対して頑健化 センサ状態変数
24. 24 バウンディングボックスファクタあああああああああ 提案手法 24 物体観測制約 画像平面上に投影したDual Quadricに外接するBBoxと観測 BBox間の誤差を最小化[Nicholson+, RA-L2018] Dual Quadric の投影→BBox 化 観測BBox センサ状態変数 物体状態変数
25. 25 形状一貫性ファクタあああああ 物体アンカーファクタあああああ 提案手法 25 物体観測制約 BBoxファクタは物体のスケールを制約できない ⇒ 物体点群情報を用いて3次元相対位置を制約 見えの変化により同じ物体に対してBBoxの大きさが変わる ⇒ 物体形状パラメータが時間変化 ⇒ 物体形状は短期間で一定であると仮定し時間変化を抑制 センサ状態変数 物体状態変数
26. 26 物体速度ファクタああああ 提案手法 26 物体運動制約 物体運動として等速運動モデルを仮定 ⇒ 連続する物体状態と物体並進速度状態を制約 物体並進速度 時刻tでの位置予測 物体位置予測 物体速度変数 物体状態変数
27. 27 速度一定ファクタああああ 提案手法 27 物体運動制約 物体運動は短期間で一定であると仮定 ⇒ 連続する物体速度間に適用し物体並進速度の平滑化を行う 物体速度変数
28. 28 提案手法 28 全体の目的関数 + + + + + + + 点群レジストレーションファクタ IMU事前積分 ファクタ センサ初期姿勢 ファクタ 物体位置を制約 物体形状を制約 物体運動を制約 = 目的関数 バウンディングボックスファクタ 形状一貫性ファクタ 物体運動ファクタ 速度一定ファクタ この目的関数を同時に最適化することで 周囲の物体運動と静的データ同士のマッチングを両方考慮しながら最適化が可能となる 物体アンカーファクタ
29. 29 実験・評価
30. 30 実験・評価 30 実験環境とデバイス 廊下(3ヶ所) • 廊下を一周しながらデータ取得 • センサ前方に常に2～4人の人物 継続的な動的環境 幾何特徴の縮退 室内(1ヶ所) 階段(1ヶ所) • 1～3階を昇降しながらデータ取得 • センサ前方に常に2人の人物 継続的な動的環境 立体的なセンサ運動 • 6人の人物が室内の家具を再配置 • 椅子や机など，人間以外も動的 継続的な動的環境 多種多様な物体クラス 以下3種類の動的環境を用意し評価 データ取得にはMicrosoft AzureKinectを使用 Microsoft AzureKinect
31. 31 実験・評価 31 比較手法 手法名 種別 動的環境への対処 GLIM K. Koide+, RAS, 2024 QuadricSLAM L. Nicholson+, RA-L, 2017 VOOM Y. Wang+, RA-L, 2024 DS-SLAM C. Yu+, IROS, 2018 V3D-SLAM T. Dang+, IROS, 2024 DynaVINS S. Song+, RA-L, 2022 3D LiDAR SLAM Object SLAM Object SLAM RGB-D SLAM RGB-D SLAM Visual -Inertial SLAM
32. 32 実験・評価 32 提案手法の動作(動画)
33. 33 実験・評価 33 推定精度の定量評価 相対姿勢誤差を用いて評価 廊下1 廊下2 廊下3 階段 提案手法が多くのシーケンスで最も高精度 最高精度 次点 提案手法 室内
34. 34 実験・評価 34 アブレーション分析 ⚫ 提案手法から各機能を除いた3条件でアブレーション分析を実施 (評価：相対姿勢誤差) ⚫ ab_1 : 動的点群除去処理を省略 ⚫ ab_2 : 物体状態の推定および動的点群除去を省略 (LiDAR-IMU only) ⚫ ab_3 : 物体状態の推定のみ省略，動的点群除去は実施 廊下1 廊下2 廊下3 階段 周囲物体のランドマーク利用が位置推定精度に寄与 室内 提案手法
35. 35 まとめ 35 まとめ ⚫ 移動物体の状態推定と点群・IMU姿勢推定を密に統合するオドメトリ推定手法を提案 ⚫ 動的な情報を除去するだけでなく，Object SLAMの枠組みを用いたランドマーク化と組み合わせることによ り周囲環境情報を余すことなく自己位置推定に寄与させる ⚫ 評価実験を通して，極めて困難な動的環境において高精度な推定が可能であることを確認