カメラを持って歩いたとき、連続する画像の中では壁の角や標識が少しずつ動く。加速度計とジャイロは、その間に端末がどれだけ加速・回転したかを高い周波数で測る。Visual Odometry（VO）は前者から、Visual-Inertial Odometry（VIO）は両方から、現在までの相対的な姿勢・速度・位置を推定する。ドローンの屋内飛行、AR/VRヘッドセット、手持ち3Dスキャナ、移動ロボットで中心になる技術である。

VO/VIOをSLAMと同義に扱うことがあるが、役割は少し違う。オドメトリは短時間で滑らかな相対軌跡を出す前段であり、誤差は原理的に蓄積する。SLAMは地図、再訪認識、ループ閉鎖まで使ってその蓄積誤差を大域的に直す。本稿では地図作成の全体像よりも、フレーム間でどう動きを積み、IMUをどう統合し、いつ推定を疑うべきかに焦点を置く。

車両フロントガラスに取り付けられたMobileyeカメラ車載カメラの例
GNSS受信機を内蔵したXsens MTi-G IMUGNSS/INS統合IMUの例

画像: Car window camera of Mobileye system(Ranbar, CC BY-SA 4.0) / Xsens MTi-G(Kallap85, CC BY-SA 4.0)、Wikimedia Commons。写真は各センサの代表例であり、本文の実装構成と同一製品を意味しない。

0. 30秒要約

1. VOは何を推定し、何を推定しないか

時刻 k におけるカメラまたはIMU座標系 B の世界座標系 W に対する姿勢を、位置 \mathbf{p}_{WB,k} と回転 R_{WB,k} で表す。VOの中心的な出力は、隣接フレームまたはキーフレーム間の相対変換

T_{C_kC_{k+1}}= \begin{bmatrix}R_{C_kC_{k+1}}&\mathbf{t}_{C_kC_{k+1}}\\\mathbf{0}^\mathsf{T}&1\end{bmatrix}\in SE(3)

である。特徴点を追跡する方式では、対応点からEssential MatrixをRANSACで推定し、回転と並進方向を復元する。すでに三次元点を持つステレオ/RGB-D/地図ベース方式では、2D–3D対応にPnPを使う。Direct VOでは明示的な記述子の照合より、露光補正後の画素値が投影先でも一致するよう姿勢を最適化する。

どの方式でも、VOは「自分が前フレームからどれだけ動いたか」を積分するデッドレコニングである。外部地図やループ閉鎖を使わなければ、相対姿勢のわずかな誤差は消えない。位置誤差は概ね時間と距離に伴って増加し、長い廊下を往復して出発点に戻っても、軌跡は閉じない。これは実装の失敗だけでなく、局所観測を逐次積む問題の性質である。

VIOにおけるカメラとIMUの融合カメラが低頻度の視覚拘束を、IMUが高頻度の運動拘束を供給し、スライディングウィンドウ最適化が状態軌跡を推定する流れ。 カメラ画像・特徴点・直接法 IMU角速度・加速度（高頻度） 事前積分 + 視覚残差スライディングウィンドウ最適化 / フィルタ 状態推定姿勢・位置・速度重力・バイアス・尺度 校正・時刻同期・外部パラメータ

単眼スケール問題

校正済み単眼の二視点対応は E=[\mathbf{t}]_\times R を拘束するが、\mathbf{t} の長さは決めない。全ての3D点と並進を s>0 倍にしても画像投影は変わらないためである。単眼VOの姿勢は正確そうに見えても、位置の単位は任意で、しかもフレーム間ごとに尺度が揺らぐと軌跡の形まで壊れる。

既知基線 B のステレオなら視差 d から Z=fB/d と深度を得られる。RGB-Dならセンサの深度が尺度を持つ。VIOでは加速度の単位がm/s²であり、重力の大きさも既知に近いため、十分に運動を励起できれば視覚の任意尺度を物理尺度へ合わせられる。ただしIMUを付けただけで常に尺度が出るわけではない。静止、等速直線、短すぎる区間、誤った時刻ずれでは尺度とバイアスが相互に紛れる。

2. カメラ観測：特徴点法と直接法

特徴点ベースVOはORB、SIFT、FAST+KLTなどで追跡した対応を用いる。新しいフレームの2D観測 \mathbf{z}_{ij} とランドマーク \mathbf{P}_j に対し、投影残差は

\mathbf{r}_{C,ij}=\mathbf{z}_{ij}-\pi\left(K\,T_{CW,i}\mathbf{P}_j\right)

である。\pi([X,Y,Z]^\mathsf{T})=(X/Z,Y/Z)^\mathsf{T} は透視除算である。RANSACは誤対応を外し、Huber損失などは残った外れに過度に引かれないようにする。特徴点方式は露光の変化に比較的強く、幾何の可視化とデバッグがしやすい。

Direct法は、対応する画素または小パッチの輝度残差を最小化する。露光パラメータを a_i,b_i とすれば、点 \mathbf{u} の残差を概略

r_I=I_j\left(\pi(T_{C_jW}T_{WC_i}\pi^{-1}(\mathbf{u},d))\right)-e^{a_j-a_i}I_i(\mathbf{u})-(b_j-e^{a_j-a_i}b_i)

のように書ける。テクスチャのある多くの画素を利用できる反面、輝度一定の仮定、ローリングシャッター、ブラー、オート露出、反射に敏感である。半直接法は粗い追跡に画素、後段に特徴を使うなど、両者の中間を取る。

方式 主な観測 強み 苦手な条件 代表例
特徴点ベースVO キーポイントの再投影 説明しやすい、照明変化に比較的強い 低テクスチャ、特徴の反復 ORB-SLAM系、VINS-Mono
Direct / Semi-direct 画素・パッチの輝度 高密度な情報、サブピクセル追跡 露出変化、ブラー、反射 DSO、SVO
単眼VIO 画像 + IMU 軽量、尺度を観測可能 初期化・同期に敏感 VINS-Mono、OpenVINS
ステレオVIO 左右画像 + IMU 即時の尺度、初期化が安定 消費電力、左右同期・キャリブレーション VINS-Fusion、Basalt

3. IMUは何を測り、なぜそのまま積分できないか

IMUはジャイロの角速度 \tilde{\boldsymbol\omega} と加速度計の比力 \tilde{\mathbf{a}} を出力する。理想値ではなく、バイアス \mathbf{b}_g,\mathbf{b}_a と白色雑音 \mathbf{n}_g,\mathbf{n}_a を含む。

\tilde{\boldsymbol\omega}=\boldsymbol\omega+\mathbf{b}_g+\mathbf{n}_g, \qquad \tilde{\mathbf{a}}=R_{BW}(\ddot{\mathbf{p}}_{WB}-\mathbf{g})+\mathbf{b}_a+\mathbf{n}_a

ここで加速度計は世界加速度そのものではなく、重力を引いた比力を測る点が重要である。姿勢が分からなければ重力をどの向きに足すかも分からず、わずかなバイアスを二重積分すると位置は急速に漂う。連続時間の運動方程式は

\dot R_{WB}=R_{WB}[\tilde{\boldsymbol\omega}-\mathbf{b}_g-\mathbf{n}_g]_\times, \quad \dot{\mathbf{v}}_{WB}=\mathbf{g}+R_{WB}(\tilde{\mathbf{a}}-\mathbf{b}_a-\mathbf{n}_a), \quad \dot{\mathbf{p}}_{WB}=\mathbf{v}_{WB}

となる。1秒後の誤差だけを見ればIMUは滑らかで優秀に見えるが、数分の単独航法ではバイアスと姿勢誤差が位置を大きく壊す。VIOの本質は、画像でIMUの長期ドリフトを補正し、IMUで画像が少ない短時間の運動と回転をつなぐことにある。

4. IMU事前積分：キーフレーム間の運動を圧縮する

カメラは20〜60 Hz、IMUは100〜1000 Hzという構成が多い。キーフレーム i と j の間には数十〜数百のIMU計測がある。最適化で姿勢・バイアスを少し更新するたびに全サンプルを積分し直すのは高価である。ForsterらのIMU preintegrationは、ある線形化点のバイアスの周りで相対回転・速度変化・位置変化を事前に積分し、ヤコビアンと共分散も保持する。

重力と世界速度を除いた量を \Delta R_{ij},\Delta\mathbf{v}_{ij},\Delta\mathbf{p}_{ij} と書けば、予測は概略

R_j\simeq R_i\Delta R_{ij},\quad \mathbf{v}_j\simeq\mathbf{v}_i+\mathbf{g}\Delta t_{ij}+R_i\Delta\mathbf{v}_{ij},
\mathbf{p}_j\simeq\mathbf{p}_i+\mathbf{v}_i\Delta t_{ij}+\frac12\mathbf{g}\Delta t_{ij}^2+R_i\Delta\mathbf{p}_{ij}

となる。バイアスが更新されたときは、保存した \partial\Delta/\partial\mathbf{b} を用いて一次補正し、大きく変化したときだけ再積分する。回転はベクトルを単純に足さず、SO(3) 上の指数写像または四元数で合成する。共分散 \Sigma_{ij} を持つことで、荒いIMUと精密なカメラ残差を同じ目的関数内で重み付けできる。

スライディングウィンドウ型VIOの代表的な目的関数は

\min_{\mathcal X}\sum_{(i,j)\in\mathcal{B}}\left\|\mathbf{r}_{I,ij}\right\|^2_{\Sigma_{ij}^{-1}} +\sum_{(i,l)\in\mathcal C}\rho\left(\left\|\mathbf{r}_{C,il}\right\|^2_{\Sigma_{C}^{-1}}\right) +\left\|\mathbf{r}_{\text{prior}}\right\|^2

である。状態集合 \mathcal X には各キーフレームの姿勢、位置、速度、ジャイロ/加速度バイアス、ランドマークの逆深度、必要なら時刻ずれや外部パラメータを含める。古い状態を周辺化してpriorへ畳み込むことで、計算量を一定に保つ。周辺化は線形化点に依存するため、状態を大きく更新し続けると整合性を失いやすい。First-Estimate Jacobian（FEJ）などの扱いは、単なる高速化ではなく観測不能な自由度を偽って観測しないための設計である。

5. フィルタ型と最適化型の違い

VIOは大別して、誤差状態拡張カルマンフィルタ（EKF）系と、固定長の非線形最適化系に分かれる。前者は現在状態と共分散を逐次更新し、遅延とメモリを抑えやすい。MSCKFは特徴点を状態として長く保持せず、複数視点の拘束として消去することで軽量にする。OpenVINSはこの系統を再現性と拡張性を意識して実装している。

後者は複数キーフレームとランドマークを同時に最適化する。VINS-Monoは特徴点追跡、IMU事前積分、スライディングウィンドウ、ループ閉鎖を含む代表例である。複数フレームへ再投影誤差を分配できるため精度面で有利なことが多い一方、周辺化、ソルバ、遅延、初期化失敗からの復帰を丁寧に設計する必要がある。

観点 EKF/MSCKF系 スライディングウィンドウ最適化系
推定の形式 状態と共分散を逐次更新 複数時刻の因子グラフを反復最小化
計算・遅延 予測しやすく低遅延にしやすい キーフレーム数と反復に依存
特徴点の扱い 観測拘束として消去しやすい 逆深度などを明示状態に置きやすい
長所 組込み向け、共分散を扱いやすい 再投影を広く整合させやすい
注意点 線形化と整合性、観測可能性 周辺化誤差、CPU負荷、再線形化

どちらが常に優れるわけではない。要求レイテンシ、センサ周波数、CPU、画角、動作環境、保守体制で選ぶ。論文の軌跡誤差だけを比べ、利用したカメラモデル、キャリブレーション、データセットの動き、再局在の有無を無視すると、実装選定を誤る。

6. 初期化：最初の数秒に何を決めるか

VIO開始直後には、世界座標の向き、初期速度、ジャイロバイアス、加速度バイアス、重力、単眼の尺度、カメラとIMUの相対姿勢が未確定である。画像だけで二視点/多視点の相対構造を作り、その視覚運動とIMU積分を整合させて、速度・重力方向・尺度・バイアスを解くのが典型的な流れである。

静止区間は平均ジャイロから初期角速度バイアス、加速度の平均方向から重力方向の手掛かりを得られる。しかし加速度計は車両の線形加速も重力と見分けない。初期化中に端末をゆっくり前後へ動かすだけでなく、複数軸で回転・並進させる「励起」を含めると、尺度とバイアスの相関をほどきやすい。反対に机上でほとんど動かない、等速で一方向へ進む、見えているのが平面だけ、といった開始は観測不足になりやすい。

初期化の成功判定は単なる最適化収束ではない。推定尺度が正で妥当か、重力の大きさが約 9.81\,\mathrm{m/s^2} に近いか、バイアスがセンサ仕様から大きく外れていないか、初期三角測量点の視差と正深度率が十分かをチェックする。失敗時に古い状態を引きずるより、画像トラックとIMUバッファをリセットして再初期化する方が、後段を壊しにくい。

7. キャリブレーションと時刻同期

VIOの性能を支配するのはアルゴリズム名よりも、センサモデルの正しさである。必要な校正は少なくとも次の三つである。

Kalibrはカメラ・IMUの時間と外部パラメータを推定する広く使われるツールである。だが一度求めた値を永久不変とみなすのは危険で、フォーカス変更、筐体の組み直し、温度、センサのファームウェア時刻処理で条件が変わり得る。IMUノイズ密度とバイアスランダムウォークも、データシートの典型値を写すだけでなくAllan varianceなどで確認すると、フィルタの過信頼を避けられる。

ローリングシャッターのカメラは、1フレームを一時刻で撮らない。激しい回転では上端と下端の画素が異なる姿勢で観測され、グローバルシャッター前提の再投影残差は系統的に外れる。短露光・グローバルシャッターが最も確実で、難しければ行時刻を含む観測モデルとIMU補償を検討する。左右カメラや深度センサを増やす場合も、ハードウェアトリガによる同期が望ましい。

8. 実装を選ぶ：VINS-Mono、OpenVINS、関連システム

VINS-Monoは単眼カメラとIMUの最適化型VIOとして広く参照される実装である。オンラインの外部パラメータ・時刻ずれ校正、特徴追跡、事前積分、スライディングウィンドウ、ループ閉鎖を備える。単にポーズを出すだけでなく、研究用に一連の構成を読めることが利点である。ステレオやGPSを組み合わせたい場合はVINS-Fusionが候補になる。

OpenVINSはMSCKF/EKF系を中心に、FEJや一貫性を明示して設計されたオープンな実装である。フィルタ型VIOの状態・共分散・キャリブレーションの扱いを検証したい場合に向く。BasaltはステレオVIOと高性能な最適化基盤を備え、EuRoCなどで実験される。スマートフォンやARでは、プラットフォームが提供するVIOを使う方がカメラ時刻・IMU時刻・デバイス固有キャリブレーションへアクセスでき、アプリ実装では現実的なことが多い。

実装/系統 推定スタイル センサ構成の例 向く目的 導入時の確認事項
VINS-Mono 最適化・固定長窓 単眼 + IMU VIOの全体構成を学ぶ、研究試作 ROS世代、校正形式、初期化条件
VINS-Fusion 最適化・固定長窓 単眼/ステレオ + IMU、GPS可 複数センサの試作 座標系と絶対観測の重み
OpenVINS EKF/MSCKF 単眼/ステレオ + IMU 組込み寄り、フィルタ検証 ノイズ値、FEJ、状態設計
Basalt 最適化 ステレオ + IMU 高性能なVIO基盤 ビルド環境、カメラモデル
自前OpenCV + Ceres 任意 任意 要件特化、学習 再投影・事前積分・周辺化の検証

既存実装を比較する際は、デモで軌跡が表示されたことを成功としない。入力画像の解像度と実周波数、IMUの単位・座標軸、時刻の基準、外部校正、初期化失敗時の挙動、追跡が切れた後の再局在、CPU使用率、メモリ、ライセンスを実機で確認する。特にROSのメッセージ時刻を「受信時刻」で埋めると、推定器には正しそうな順序でも物理的には誤った時間間隔が渡る。

9. 最小の処理フロー

  1. カメラ内外部・時刻を校正し、画像とIMUを共通の時刻基準でバッファする。
  2. IMU到着ごとにバイアス補正済み角速度・比力で状態を予測し、共分散または事前積分を更新する。
  3. 画像到着時に前フレーム/キーフレームから特徴点を追跡し、前後チェック、RANSAC、マスクで外れ値を除く。
  4. 初期化前なら視差と励起を判定し、視覚構造と慣性を整合させる。条件が悪ければ開始を延期する。
  5. 初期化後は視覚再投影残差とIMU残差を融合し、姿勢・位置・速度・バイアス・点を更新する。
  6. 古いキーフレームを周辺化し、品質ゲートを落ちた観測を破棄する。必要なら再局在または安全なリセットへ遷移する。

この流れでは予測と補正の区別が重要である。画像が一時的に失われてもIMU予測は出力を継続できるが、その不確かさは増えている。アプリケーション側は「ポーズがある/ない」の二値ではなく、追跡状態、共分散または品質スコア、最終視覚更新からの時間を受け取り、AR表示・制御速度・安全停止を段階的に切り替えるべきである。

10. 評価指標：何を測れば比較できるか

絶対軌跡誤差（ATE）は、推定位置列を真値軌跡へ剛体またはSim(3)整列した後の差である。

\mathrm{ATE}_{\mathrm{RMSE}}=\sqrt{\frac1N\sum_{k=1}^{N}\left\|\mathbf{p}^{\mathrm{gt}}_k-(R\hat{\mathbf{p}}_k+\mathbf{t})\right\|^2}

単眼VOは尺度不定なので、Sim(3)整列でスケールも合わせたかを必ず併記する。VIOやステレオの尺度誤差を見たいのにSim(3)整列すれば、その重要な誤差を隠してしまう。SE(3)整列か、開始姿勢だけを合わせるか、軌跡を無整列で比較するかは用途に応じて選ぶ。

相対姿勢誤差（RPE）は一定時間または距離 \Delta の局所ドリフトを測る。推定相対変換 \hat T_{k,k+\Delta} と真値 T^{\mathrm{gt}}_{k,k+\Delta} の差

E_k=(T^{\mathrm{gt}}_{k,k+\Delta})^{-1}\hat T_{k,k+\Delta}

から並進誤差（m/mや%）と回転誤差（deg/m、deg/s）を求める。長距離でのATEが小さくても、短時間のジッタが大きければARや飛行制御に向かない。逆に滑らかでも長距離で漂う軌跡は地図作成に不利である。

指標 主に分かること 落とし穴
ATE RMSE/中央値 軌跡全体の位置整合 整列方法で値が大きく変わる
RPE（距離/時間区間） 局所ドリフト、制御への影響 区間長を明記しないと比較不能
回転誤差 ジャイロ・視覚姿勢の品質 yawとroll/pitchを混ぜると原因が隠れる
スケール誤差 単眼VIO/ステレオの物理尺度 Sim(3)整列後には評価できない
追跡率・失敗率 現場での可用性 失敗後の自動再初期化を隠さない
遅延・CPU・電力 組込み実装の実用性 オフライン処理だけでは判断不可

EuRoC MAV、TUM VI、UZH-FPV、KITTIなどは比較に使われる公開データセットである。ただし屋内の手持ちカメラ、車載、競技ドローンでは画角、照明、速度、振動、露出が異なる。ベンチマークの順位を実機性能へ直輸入せず、自分のログに近い経路、速度、失敗条件で評価する。真値が取れない現場でも、閉路の始終点差、既知距離、壁面の再投影、外部モーションキャプチャの一部区間で監査できる。

11. 代表的な失敗条件と対策

低テクスチャ、暗所、ブラー

白壁、暗い廊下、逆光、急旋回では視覚残差の勾配が不足する。特徴点が少ないのにRANSACしきい値を緩めれば、誤対応を増やすだけである。露光時間を短くする照明・センサ設計、広角またはステレオ化、特徴再検出、予測不確かさの増大を使用側へ通知することが対策になる。

動的物体と反射

VIOは多くの画像点が静的背景に属することを仮定する。人混み、車載の前走車、鏡、ガラス、水面はその仮定を破る。ロバスト損失とRANSACだけでは背景が少数になるケースを救えない。動体の意味領域を除外する、深度やフローで複数運動を分ける、固定物だけを優先する設計が必要になる。

IMU飽和、振動、温度ドリフト

ドローンや小型ロボットのモータ振動は加速度計へ強い帯域ノイズを入れ、衝撃はレンジを飽和させる。フィルタのノイズを小さく設定してIMUを過信すると、画像補正を拒み発散する。防振、適切な帯域処理、飽和フラグ、温度を含むバイアスモデル、実測したノイズパラメータを用いる。生のIMUを平滑化しすぎると短い加速を失う点にも注意する。

時刻ずれと座標系の取り違え

5 msの時刻ずれは高速回転中には大きな画角差に相当する。画像時刻が露光開始・中央・受信のどれか、IMU時刻がサンプル時刻か到着時刻かを明確にする。右手/左手系、重力の符号、T_{BC} と T_{CB}、カメラ光軸の向きを取り違えると、最適化は数値的に収束しても物理的に誤る。静止テスト、既知の90度回転、短い直線移動を使う単体試験を用意する。

初期化と再局在の混同

初期化はまだ地図・尺度がない状態からの推定開始、再局在は既知地図または過去のキーフレームへ戻る処理である。純粋なVO/VIOは通常、追跡が切れれば新たな局所座標系で再開始する。ループ閉鎖付きSLAMの再局在能力をVOのドリフト性能と混同して評価してはいけない。製品では、追跡喪失時に古い座標を使い続けるか、安全に停止するか、アンカーを無効化するかを明確に決める。

12. 最近の方向性

近年はSuperPoint、LightGlue、LoFTRなど学習ベースの特徴・対応推定が、従来の局所記述子で厳しい見た目変化へ有効な場合がある。またDROID-SLAMのように深いネットワークと反復的な幾何最適化を組み合わせる方式も現れた。しかし、学習器が多くの対応を返しても、尺度不定性、平面縮退、時刻同期、IMUバイアスは物理的な問題として残る。学習出力をロバスト幾何、再投影誤差、慣性整合で検証する構成は今も基本である。

イベントカメラは高速・高ダイナミックレンジで、通常カメラがブラーや暗部で苦しい条件を補える可能性がある。LiDARやUWB、GNSS、車輪オドメトリをVIOへ加えるマルチモーダル推定も、長期ドリフトと厳環境対策として実用化が進む。センサを増やすほど性能が自動的に上がるわけではなく、外部姿勢、時刻、故障検知、重みの調整が新たな失敗面になる。

13. まとめ

VOは画像から短時間の相対運動を復元する有用なオドメトリだが、単眼では尺度がなく、局所誤差は蓄積する。VIOはIMUの高頻度な運動拘束と視覚の長期補正を組み合わせ、尺度、姿勢、速度、バイアスを同時に推定する。その中核は正しいセンサモデル、IMU事前積分、十分な運動励起を使う初期化、視覚・慣性残差のロバストな融合である。

実装では、まず校正・時刻・座標系を検証し、次に視差、インライア分布、再投影誤差、バイアス、重力、追跡状態を観測可能なログにする。VINS-MonoやOpenVINSは有力な出発点だが、撮影条件・レイテンシ・再初期化方針・安全要件に合わせて評価する必要がある。最終的に信頼すべきなのは軌跡の見栄えではなく、どの条件で尺度と姿勢が観測でき、どの条件で不確かさが増えるかを説明できる設計である。

参考資料（一次資料・公式ドキュメント）

#Visual Odometry #VIO #IMU #Preintegration #VINS-Mono #OpenVINS #State Estimation #Robotics