特徴点対応からVO・SLAMへ進む

この章で学ぶこと

• 自己位置推定、Visual Odometry、SLAMの役割を区別する
• ORB特徴点の対応候補を可視化する
• 単眼の尺度の曖昧さ、ドリフト、再訪検出を説明する
• 物体検出と自己位置推定の総合実験を行う

1. マーカーがない場所ではどうするか

前章では、既知の大きさと位置を持つマーカーを基準にしました。環境中の模様や角を使う場合は、複数の画像で同じ場所を見つけ、カメラの動きを推定するところから始めます。

方法	推定するもの	基準・特徴
固定マーカーによる自己位置推定	既知の世界座標での位置・姿勢	基準マーカーが必要
Visual Odometry（VO）	画像列からのカメラの相対運動	小さな誤差の累積でずれやすい
Visual SLAM	自己位置と環境の地図	地図との対応や再訪による補正も扱う

本教材で実装したのはマーカー基準の自己位置推定です。この章の特徴点実習はSLAMの入口であり、対応点を描くだけではVOやSLAMにはなりません。

2. ORB特徴点を対応付ける

カメラで、模様のある本や箱を含む静止した場面を撮ります。少し横へ移動して、十分に重なる範囲をもう一度撮り、view1.jpg と view2.jpg を保存してください。第6章の撮影機能でも用意できます。

python feature_matching.py view1.jpg view2.jpg --output matches.png

特徴点は位置を再発見しやすい局所的な模様です。ORBでその周辺を記述し、Hamming距離で記述が近い候補を探します。教材は相互に最良の候補になった組を使い、距離の小さい順に最大60組を線で結びます。OpenCVの特徴点マッチング資料

orb = cv2.ORB_create(nfeatures=1500)
key1, desc1 = orb.detectAndCompute(first, None)
key2, desc2 = orb.detectAndCompute(second, None)
matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = matcher.match(desc1, desc2)

似た模様が繰り返す場所や、動いている物体には誤対応が起こります。相互一致を確認しても、正しい幾何学的対応だとは限りません。白い壁では特徴点そのものが足りない場合もあります。

3. 対応点から運動推定へ

校正済みカメラで、静止した立体的な場面を異なる位置から撮った場合、対応点から本質行列を求め、相対回転と並進の方向を推定する方法があります。OpenCVでは findEssentialMat、recoverPose などが入口になります。外れ値を含む対応から頑健に推定するため、RANSACなどの処理も必要です。OpenCVの3次元復元API

ただし、通常の単眼2視点だけでは並進のメートル単位の大きさが決まりません。出てきた並進ベクトルを毎フレーム1mの移動として足してはいけません。純粋な回転、奥行きの変化が乏しい場面、少ない視差、動体の多い場面などでも推定が不安定になります。

VOでは画像列の運動をつなぐため、誤差が積み重なるドリフトが生じます。SLAMでは地図を維持し、以前訪れた場所を認識するループ閉じ込みや、複数の観測を使った最適化で整合性を改善します。メートル単位の尺度を得るには、既知の長さ、ステレオの基線長、深度、適切に統合した慣性計測などの情報が必要です。

発展的な実装として、ORB-SLAM3の公式リポジトリに単眼・ステレオ・RGB-D・Visual-Inertialの例があります。別の依存環境とセンサー設定が必要なため、導入時は公式手順とサンプルデータから始めてください。本教材のPython環境にインストールするだけで動くものではありません。

4. 総合課題：何がどこにあり、自分はどこにいるか

机にYOLOで認識できるコップや本を置き、その付近へID 0のマーカーを固定します。校正したカメラで、物体とマーカーが同時に写る動画 scene.mp4 を保存します。校正と同じ解像度・画角を使い、途中でマーカーを動かさないでください。

python yolo_detect.py --source scene.mp4 --max-frames 0 --output scene-objects
python marker_localization.py --source scene.mp4 --calibration calibration.npz --marker-m 0.080 --max-frames 0 --output scene-camera

マーカーの寸法は自分の値に変更します。同じ保存動画を使うことで、両方のJSONLの frame が同じ画像を指します。別々にライブカメラを実行したログでは、フレーム番号だけで時刻を対応させられません。

両方のログを結合する例です。次を combine_logs.py として保存して実行します。

import json
from itertools import zip_longest

with open("scene-objects/detections.jsonl", encoding="utf-8") as objects, \
     open("scene-camera/poses.jsonl", encoding="utf-8") as poses, \
     open("scene-summary.jsonl", "w", encoding="utf-8") as output:
    for detection_line, pose_line in zip_longest(objects, poses):
        if detection_line is None or pose_line is None:
            raise ValueError("処理フレーム数が違います")
        detection = json.loads(detection_line)
        pose = json.loads(pose_line)
        if detection["frame"] != pose["frame"]:
            raise ValueError("フレーム番号が一致しません")
        output.write(json.dumps({
            "frame": detection["frame"],
            "objects": detection["detections"],
            "relations_2d": detection["relations"],
            "camera_pose": pose,
        }) + "\n")

python combine_logs.py

結果には、物体の種類と画像内の関係、カメラの位置・向きが入ります。物体自体の3次元位置はまだ入りません。 そこまで求めるには、物体上の対応点の奥行きや、既知の床面との交点など、画像の点を3次元へ戻す情報が必要です。

達成を確認する

課題	提出・記録するもの
物体検出	枠付き画像、検出クラス、見逃しと誤検出の例
位置関係	左右・上下・重なりのログと、人の観察との比較
校正	カメラ設定、パターン実寸、各画像の誤差
自己位置推定	座標軸と単位、既知移動量との誤差、見失い時の挙動
特徴点対応	対応線の画像と誤対応の例

最後に、YOLOの枠だけで距離が分からない理由、固定マーカー方式とSLAMの違い、マーカーを見失った後に推定を継続するために必要な仕組みを、自分の実験結果を使って説明してください。