ロボットが「いま自分がどこにいるか」を知る方法はいくつもありますが、多くは専用センサや、床・天井にマーカを敷き詰める準備を必要とします。
slam_tenchi は、そうした準備なしに、床の“模様”そのものを手がかりにします。下向きカメラ1台で床を撮り、同じ座標系で地図（MAPPING）と自己位置（LOCALIZATION）を出す ― それが「床SLAM」です。尺度はカメラ高さ（0.35m）が既知なので、得られるのは実寸の SE2 姿勢（x, y, yaw）です。
紹介映像
何をするか
対象は、屋内の有限な床です。下向きカメラが床を捉え、その模様（タイル・傷・テクスチャ）を特徴として使います。尺度（実寸）はカメラの取り付け高さ 0.35m が既知であることから確定するので、ピクセルではなくメートル単位の位置が出せます。
出力は SE2 姿勢、すなわち平面上の x, y, yaw の3自由度です。地図も自己位置も同じ座標系で表されるため、「作った地図の上で、いま自分がどこにいるか」を一貫して扱えます。
処理の流れ
床の画像から姿勢を得るまでは、一続きのパイプラインです。
床は平面なので、2枚の画像の対応関係はホモグラフィで表せます。ここから移動量を復元し、キーフレーム（画像＋特徴＋SE2姿勢）をつないで地図を作っていきます。
2つのモード ― 地図作成と自己位置
slam_tenchi は、役割の違う2つのモードを持ちます。
- MAPPING（地図作成） ― フレーム間VOで動きを積み、一定の移動（0.3m / 12°）ごとにキーフレームを追加。ループ検出とマーカ制約で pose graph を最適化し、地図として保存します。
- LOCALIZATION（自己位置） ― 大域探索で初期化し、以降は近傍キーフレームで追跡。複数キーフレームを最小二乗で照合します。
ここが要点です。自己位置は「一度合わせて積算する」のではなく、毎フレーム地図に照合します。 だからオドメトリのようなドリフトが積み上がりません。うまく解けないフレームは無理に推測せず、未解決として扱います。
カメラが見ているもの
実際の入力は、下向きカメラに写る床です。
軌跡の読み方
評価では、地図作成時の軌跡と自己位置推定時の軌跡を並べて見ます。
マーカとのハイブリッド
床模様だけでも動きますが、より高い絶対精度が要るときのために、マーカとのハイブリッドも用意しています。
- レイアウトがあるときだけ ― マーカ配置図（
markers.yaml：ID / family / x, y, yaw）を渡したときに有効化。 - 検出フレームは絶対姿勢を優先 ― マーカが見えたフレームは、その絶対姿勢を優先して使います。地図作成時は見えたマーカを記録し（
found_markers.json）、自己位置推定時は姿勢と見えているIDを同時に返します。 - 無ければ床模様だけ ― マーカが無い環境では、これまで通り床の模様だけで動きます。
- 推測しない設計 ― マーカを検出したのに誤差（RMSE）が閾値を超えた場合は、無理に値を返さず例外で停止します。「それらしい嘘」を出さないための安全側の設計です。
合成マーカでの絶対姿勢は 0.18mm で PASS しています。
数字で見る正しさ
精度は、閾値がcmオーダーのところ、実測はmmで収まっています。
| 指標 | 実測 | 判定 |
|---|---|---|
| Gazebo 地図（対 真値） | 4.5mm | PASS |
| Gazebo 自己位置 | 6.1mm（未解決 0%） | PASS |
| 合成マーカ 絶対姿勢 | 0.18mm | PASS |
| 処理時間（CUDA・中央値） | 62ms/frame | ― |
Python から呼ぶ
導入はエディタブルインストール、呼び出しは数行です。
pip install -e .
from floor_slam import FloorSlam, list_image_paths
import cv2
# 地図をつくる
with FloorSlam.mapping(config_path="config/default.yaml") as slam:
for p in list_image_paths("frames"):
fr = slam.process(cv2.imread(str(p)))
print(fr.pose.x, fr.pose.y, fr.pose.yaw, fr.source)
slam.finalize_map("datasets/maps/my_scene")
# 作った地図で自己位置を出す
slam = FloorSlam.load_map("datasets/maps/gazebo_floor_v1")
fr, ms = slam.process_with_timing(bgr) # SE2姿勢 + 処理時間(ms)
マーカ配置図を使うときは、FloorSlam.mapping(marker_layout="markers.yaml") のように渡すだけです。
まとめ
- 床模様 1枚で、地図と自己位置 ― 下向きカメラ1台。専用マーカの敷設は不要。
- MAPPING ― ホモグラフィ VO でキーフレーム地図を作る。
- LOCALIZATION ― 毎フレーム地図に照合。積算しないからドリフトが溜まらない。
- マーカがあれば絶対姿勢を優先、無ければ模様だけ。推測せず、危ういときは止まる。
- Gazebo で 地図 4.5mm / 自己位置 6.1mm、CUDA 62ms/frame。Pythonから数行で。
床SLAM「slam_tenchi」について、デモやご相談を承っています。