オドメトリは、歩いた距離を足し続けるほど少しずつずれる。ロボットが部屋を一周して出発点へ戻ってきたとき、地図上の現在位置が出発点から数十cmずれていても、直前のフレームだけを見ているシステムは間違いに気づけない。Loop Closure（ループ閉合）は、現在の景色が過去の景色と同じだと発見し、その「同じ場所」という拘束を使って軌跡を一周分つなぎ直す。
0. 30秒要約
- ループ閉合は、場所候補の検索と、幾何学的に同じ場所かを確認する二段階で構成する。
- Bag-of-Words、グローバル画像記述子、学習ベースPlace Recognitionで候補を高速検索し、特徴点の再照合とRANSACで誤検出を除く。
- 検出した拘束をPose Graphへ追加し、SE(2)/SE(3)のグラフ最適化で過去の姿勢を滑らかに修正する。局所地図は再投影誤差を保つ。
- 季節、照明、家具の移動、似た廊下は誤ループを生む。時間間隔、移動距離、インライア数、地図の整合を複数条件で検証する。
- ループ閉合は「地図が一瞬で正解になる魔法」ではない。誤検出を受け入れると地図全体が誤って変形するため、保守的な閾値と取消可能な設計が重要である。
1. ループ拘束を数式で書く
図1 — 再訪問の一致をループ辺として追加し、ポーズグラフの累積ドリフトを全体最適化で分配する。
時刻iの姿勢をT_i\in SE(3)、時刻jで同じ場所を再訪したときの相対姿勢観測をZ_{ij}とする。ループ拘束の残差は
で表せる。\mathrm{Log}は剛体変換を6次元の微小回転・並進へ写す対数写像である。オドメトリ辺\mathcal E_oとループ辺\mathcal E_lを合わせたPose Graph最適化は
となる。\Omega_{ij}は情報行列、\rhoはHuberなどのロバスト損失。ループ辺が1本加わるだけで、長い軌跡に蓄積した誤差をグラフ全体へ配分できる。
2. 候補を探す：画像を「単語の袋」にする
Bag-of-Words（BoW）は、局所記述子を量子化して視覚単語のヒストグラムにし、過去キーフレームとの類似度を逆文書頻度（IDF）で計算する。ORB-SLAM系では、軽量なORB記述子と語彙木を組み合わせ、毎フレーム全地図を比較せず候補を数件へ絞る。
グローバル記述子は、画像全体を一つのベクトルへ圧縮して近い景色を検索する。学習ベースのNetVLAD、CosPlace、EigenPlacesなどは、照明や視点変化に頑健な表現を学習する一方、学習地域と異なる建物・農地・工場では性能が変わる。候補検索は速さ、幾何検証は正確さを担当し、どちらか一方へ依存しない。
3. 幾何検証：似ているだけでは閉じない
候補画像から記述子を再照合し、対応点を得る。単眼ならEssential/Fundamental Matrix、既知地図点があればPnP、平面テクスチャならホモグラフィをRANSACで推定する。インライア数、再投影誤差、深度が正か、視点差が物理的かを確認し、候補が同じ場所である確率を上げる。
誤ループが怖いのは、グラフ最適化が「誤った観測でも整合する形」へ地図を変形してしまうからだ。似た二つの廊下、窓の並び、農地の畝、工場の棚は、BoWだけでは区別しにくい。時間的に近すぎるフレームを候補から除き、複数キーフレームで連続して一致すること、独立したセンサ（IMU/LiDAR/GNSS）と整合することを要求する。
4. Pose Graphと地図の更新
ループ辺を追加した後、まずキーフレーム姿勢だけをPose Graphで最適化し、地図点を姿勢へ追従させる。大規模な地図では、全点を毎回Bundle Adjustmentするより、局所地図と大域姿勢を分けた方がリアルタイム性を保ちやすい。最適化が終わったら、現在のmap→odom変換を更新し、連続性を重視するodom→base_linkを急に飛ばさない設計にする。
ROS 2のTF2で言えば、局所オドメトリがodom→base_linkを発行し、SLAMの大域補正がmap→odomを調整する。ループ閉合の瞬間にロボット本体が飛んで見えないよう、補正を時間的に平滑化する。ただし、地図と実世界のずれを隠すために平滑化し過ぎると、ナビゲーションが古い位置を使い続けるため、補正量と適用時間をログに残す。
5. 失敗しやすい環境
| 条件 | なぜ難しいか | 対策 |
|---|---|---|
| 季節・昼夜の変化 | 色・影・植生が変わる | 学習記述子、構造特徴、LiDAR併用 |
| 動的な人・車 | 同じ場所でも配置が違う | 動的物体マスク、静的背景のみ照合 |
| 反復模様 | 間違った場所も似る | 幾何検証、距離制約、複数フレーム確認 |
| 長い廊下・棚 | 視差と固有性が小さい | IMU、人工マーカー、UWB |
| 大きな照明変化 | 輝度記述子が変わる | HDR/露光補正、学習型Place Recognition |
| 急旋回・ブラー | 対応点が減る | IMU予測、キーフレーム間隔調整 |
6. 実装チェックリスト
- キーフレームの画像、時刻、姿勢、特徴記述子を保存する。
- 直近フレームを候補から除き、BoW/グローバル記述子で過去候補を数件検索する。
- 対応点を幾何検証し、インライア数・再投影誤差・深度正値を閾値判定する。
- 別時刻の複数フレームで同じ候補が続くか確認する。
- Pose Graphへループ辺を追加し、ロバスト最適化を実行する。
- 補正量、却下理由、計算時間をログへ残す。誤ループは取消可能にする。
7. 研究の方向性
最新研究では、画像検索を大規模自己教師学習で頑健化する方向、LiDAR・カメラ・イベントカメラの記述子を共有する方向、長期地図を季節ごとに更新する方向が進んでいる。NeRFや3D Gaussian Splattingのような新しい地図表現を場所認識へ使う試みもあるが、推論速度、記憶容量、動的物体、ライセンス、再現性を確認する必要がある。
工場や農地では、同じ形の通路・畝が繰り返すため、画像だけのループ閉合は曖昧になりやすい。作業機の経路、IMU、車輪オドメトリ、RTK-GNSS、地図上の作業境界を事前知識として使い、候補の探索範囲を制約すると誤検出を減らせる。研究プロトタイプを現場へ持ち込むときは、誤ったループを検出したら地図を凍結し、手動で再初期化できる運用を最初から用意する。
8. まとめ
Loop Closureは、過去の景色を思い出す画像検索と、同じ場所だと証明する幾何検証、誤差をグラフ全体へ配る最適化の三段階である。BoWや学習記述子のスコアだけで閉じず、対応点、センサ整合、時間的な連続性を確認する。ループを正しく扱えば、オドメトリのドリフトを地図全体で修正し、長時間走るロボットの自己位置を一貫させられる。