はじめに
※一部の閲覧アプリでは、数式がLaTeX表記のまま表示される場合があります。その際は、記事のURLをSafariやChromeなどのブラウザで直接開いてください。
前回の記事では、車載映像から検出した道路損傷や付属物を位置情報として記録する際に、「撮影位置と地物位置」「検出回数と地物数」を区別する必要があることを説明しました。
今回は、その課題に対する処理方法を考えます。複数フレームに写る対象を追跡し、同じ地物の観測をまとめ、地物の位置を推定してGISへ登録する方法です。
オービタルネットでは、以前から物体検出とカルマンフィルタを組み合わせた追跡・計数のデモを制作してきました。以下は、その取り組みに関する動画です。
https://www.youtube.com/watch?v=I-6_4-u7nC4
https://www.youtube.com/watch?v=hYycJz_csSU
https://www.youtube.com/watch?v=mVVUJhqhCrY
本記事では、この追跡・計数の考え方を、車載映像によるインフラ地物の位置情報化へ展開します。以下の数式と処理構成は解説用に整理したものであり、動画制作当時の実装をそのまま再現したものではありません。
最初に重要な点を整理します。カルマンフィルタは、対象の状態とその不確かさを予測・更新する手法です。どの検出が同じ対象に対応するかを決める処理や、画像座標を地図座標へ変換する処理は、別に必要です。
1. 「検出」「追跡」「地物」を分ける
一つの標識が100フレームに写った場合、検出レコードは100件あっても、台帳に登録する標識は原則として1基です。ただし、遮蔽で追跡が途切れれば、その標識に複数の追跡IDが発行される可能性があります。
| 単位 | 意味 | IDの例 |
|---|---|---|
| 検出 | あるフレーム内の矩形・マスク・特徴点 | detection_id |
| 追跡 | 複数フレーム間で対応付けられた観測列 | track_id |
| 地物 | 現実空間の管理対象 | asset_id |
目標は、検出ごとにGIS地物を増やすことではなく、次の関係を管理することです。
\{D_{t_1},D_{t_2},\ldots,D_{t_n}\}
\longrightarrow T_k\longrightarrow A_j
ここで $D_t$ は検出、$T_k$ は追跡、$A_j$ は地物です。一つの地物に複数の追跡が対応しても構いません。途中で切れた追跡や、別走行の観測を同じ地物へ結び付けるためです。
2. 画像上の動きを状態として表す
まず、矩形を追跡する簡単なモデルを考えます。本記事では、説明用として次の8次元状態を採用します。
\mathbf x_t=
\begin{bmatrix}
u_t&v_t&w_t&h_t&\dot u_t&\dot v_t&\dot w_t&\dot h_t
\end{bmatrix}^{\top}
$u,v$ は矩形中心、$w,h$ は幅と高さで、単位はpixelです。後半の4要素はそれぞれの時間変化率で、単位はpixel/sです。物体検出器から得る観測は、
\mathbf z_t=\begin{bmatrix}u_t&v_t&w_t&h_t\end{bmatrix}^{\top}
とします。これは後述のSORT原論文の状態ベクトルそのものではなく、説明用の選択です。
短い時間では変化率が一定と近似し、
\mathbf x_t=F_t\mathbf x_{t-1}+\mathbf q_t,
\qquad
\mathbf z_t=H\mathbf x_t+\mathbf r_t
F_t=
\begin{bmatrix}
I_4&\Delta t\,I_4\\
0&I_4
\end{bmatrix},
\qquad H=\begin{bmatrix}I_4&0\end{bmatrix}
とモデル化します。$I_4$ は4次の単位行列、$\Delta t$ は実際の撮影時刻差です。間引きやフレーム欠落があれば、常に1/30秒としてはいけません。
$\mathbf q_t$ は運動モデルの誤差、$\mathbf r_t$ は観測誤差で、平均0、共分散をそれぞれ $Q_t,V_t$ とします。標準的な線形カルマンフィルタでは、これらの雑音と初期推定誤差の独立性などを仮定します。[1]
幅・高さを線形に外挿すると、長い欠測で負値になるなどの不整合も起こり得ます。実装では有効範囲の確認や、対数サイズを使うモデルなどを検討します。
3. カルマンフィルタの予測と更新
3.1 次のフレームの状態を予測する
前時刻の推定値と共分散を $\widehat{\mathbf x}_{t-1}^{+},P_{t-1}^{+}$ とすると、
\widehat{\mathbf x}_t^{-}=F_t\widehat{\mathbf x}_{t-1}^{+}
P_t^{-}=F_tP_{t-1}^{+}F_t^{\top}+Q_t
です。上付きの $-$ は観測による更新前、$+$ は更新後を表します。点だけでなく不確かさも予測する点が重要です。
3.2 対応する観測が得られたら更新する
同一対象と対応付けられた観測について、予測との差と、その共分散を計算します。
\boldsymbol\nu_t=\mathbf z_t-H\widehat{\mathbf x}_t^{-},
\qquad S_t=HP_t^{-}H^{\top}+V_t
カルマンゲインを $G_t$ とすると、
G_t=P_t^{-}H^{\top}S_t^{-1}
\widehat{\mathbf x}_t^{+}
=\widehat{\mathbf x}_t^{-}+G_t\boldsymbol\nu_t
となります。共分散は、数値計算での安定性に配慮したJoseph形式で、
P_t^{+}=(I-G_tH)P_t^{-}(I-G_tH)^{\top}+G_tV_tG_t^{\top}
と更新できます。[1][2]
ここで $G_t$ を使ったのは、後述するカメラ内部行列 $K_c$ と区別するためです。
1次元の例では、予測位置100 pixel、予測分散4 pixel²、観測位置106 pixel、観測分散9 pixel²なら、
G=\frac{4}{4+9}=\frac4{13},
\qquad
\widehat x^{+}=100+\frac4{13}(106-100)\approx101.85
となります。この例では予測の不確かさが小さいため、観測値へ一気に移すのではなく、予測に近い位置へ更新されます。これは説明用の数値例です。
なお、検出器の分類スコア0.9は、位置の標準偏差が0.1 pixelであることを意味しません。$V_t$ は、対象距離・サイズ・遮蔽などに応じた実際の位置誤差から設定・検証します。
4. 「どの検出で更新するか」を決める
画像中に複数の標識がある場合、カルマンフィルタの式だけでは、どの検出をどの追跡へ割り当てるかは決まりません。そこでデータ対応付けを行います。
追跡 $i$ と検出 $j$ の組合せについて、
\boldsymbol\nu_{ij}=\mathbf z_{j,t}-H\widehat{\mathbf x}_{i,t}^{-},
\qquad
S_{ij}=HP_{i,t}^{-}H^{\top}+V_{j,t}
d_{ij}^{2}=\boldsymbol\nu_{ij}^{\top}S_{ij}^{-1}\boldsymbol\nu_{ij}
を計算します。$d_{ij}^{2}$ はマハラノビス距離の二乗で、不確かさに対して観測がどれだけ離れているかを表します。
大きすぎる組合せを候補から除く処理をゲーティングと呼びます。4次元の観測でガウス仮定と共分散が適切なら、カイ二乗分布に基づく閾値を利用できます。ただし、その閾値は同一性の保証ではありません。
残る候補に対し、例えば次のような費用を設計します。
c_{ij}=\alpha d_{ij}^{2}
+\beta\left(1-\operatorname{IoU}(B_i^{-},B_j)\right)
+\gamma\,d_{\mathrm{appearance}}(i,j)
$B_i^{-}$ は予測矩形、$B_j$ は検出矩形、IoUは矩形の重なり率です。外観距離は画像特徴の違いを表し、各項の尺度に合わせて重みを設定します。この式は本記事の構成例です。
対応関係 $a_{ij}\in\{0,1\}$ を、
\min_a\left[
\sum_{i,j}c_{ij}a_{ij}
+\lambda_T\sum_i\left(1-\sum_j a_{ij}\right)
+\lambda_D\sum_j\left(1-\sum_i a_{ij}\right)
\right]
\sum_j a_{ij}\leq1,\qquad \sum_i a_{ij}\leq1
として求めます。$\lambda_T,\lambda_D$ は未対応を許すための費用です。禁止した組合せは割り当てず、ダミー行・列などで未対応を扱う割当問題として解きます。無理に全てを対応させないことが大切です。
代表的な追跡手法SORTは、カルマンフィルタによる予測と、IoUを用いたハンガリアン法による対応付けを組み合わせています。[3] 本記事では、そこに不確かさ・外観・後述する地図上の整合性を加える設計を考えます。
5. 追跡IDを発行し、欠測を管理する
対応が決まってから、その検出でカルマンフィルタを更新します。
| 状況 | 処理 |
|---|---|
| 既存追跡に対応する検出がある | 同じtrack_idを維持して更新 |
| どの追跡にも対応しない検出がある | 仮の追跡を開始 |
| 一定の観測条件を満たす | 追跡を確認済みにする |
| 一時的に検出されない | 予測だけで保持し、不確かさを増やす |
| 欠測が長い・視野外に出た | 追跡を終了し、観測履歴を保存 |
確認条件や終了条件は、フレーム数だけでなく経過時間、撮影頻度、画質に合わせて設計します。
予測だけで保持しているフレームは、新たな実観測ではありません。 検出件数に加えたり、その予測点を独立した観測として位置推定へ再投入したりしてはいけません。
track_idの種類数を数えるだけでも不十分です。追跡が途中で切れれば同じ地物が複数IDになるため、最終的な計数にはasset_idへの統合が必要です。
6. 車載カメラでは、止まった地物が画像上で動く
道路損傷や標識は世界座標ではほぼ静止しています。しかし、車両が動けば画像上の位置と大きさは変化します。
カメラの世界位置を $\mathbf C_t$、カメラ座標から世界座標への回転を $R_t$、内部行列を $K_c$、地物の固定点を $\mathbf X_j$ とすると、歪み補正後の投影関係は、
s_{tj}\begin{bmatrix}u_{tj}\\v_{tj}\\1\end{bmatrix}
=K_cR_t^{\top}(\mathbf X_j-\mathbf C_t)
です。[4] $s_{tj}$ は投影のスケールです。
$\mathbf X_j$ が一定でも、$\mathbf C_t,R_t$ が変われば、画像座標は変わります。したがって、画像上の等速モデルは短時間の近似です。接近、旋回、振動が大きい場面では外れやすくなります。
カメラ姿勢や路面モデルを利用できる場合は、路面に対応した画像変換や、世界座標の地物を画像へ再投影する方法で予測を補います。ただし、路面用の一枚の平面変換を、標識板など高さのある全対象に適用することはできません。
7. 追跡した地物を、実空間へ位置付ける
7.1 まず「どの点の位置か」を決める
矩形中心が、全フレームで同じ物理点を表すとは限りません。見え方や遮蔽で矩形が変わるためです。
| 対象 | 位置を定義する例 |
|---|---|
| 標識 | 標識板中心、または支柱の接地点 |
| ポール | 地面との接地点 |
| ポットホール | 路面上へ変換した輪郭と、その代表点 |
| ひび割れ | 路面上の線・領域、または固定グリッドの状態 |
標識板中心と支柱の接地点を混ぜて統合してはいけません。また、画像マスクの重心を投影した点は、一般に投影後の領域の重心とは一致しません。
7.2 路面モデルを使う2次元位置推定
歪み補正後の画像点 $\mathbf p=(u,v,1)^\top$ から、世界座標での単位視線を求めます。
\mathbf d=\frac{R_tK_c^{-1}\mathbf p}{\|R_tK_c^{-1}\mathbf p\|},
\qquad \mathbf X=\mathbf C_t+\lambda\mathbf d
既知の路面平面を $\mathbf n^\top\mathbf X+b=0$ とすれば、代入して、
\lambda=-\frac{\mathbf n^\top\mathbf C_t+b}{\mathbf n^\top\mathbf d}
と求まります。前方の交点であること、視線が平面とほぼ平行でないこと、対象点がその面上にあることを確認します。路面に起伏があれば、局所平面や路面メッシュとの交差に置き換えます。
GISへは水平座標 $(X,Y)$ を記録できます。ただし、出力が2次元であっても、単眼画像からの位置決定には路面などの空間的制約が必要です。
7.3 複数視点による3次元位置推定
同じ物理点を異なる撮影位置から観測できれば、三角測量が可能です。[4] 同一track_idは物体の対応を表しますが、その内部で同じ角や特徴点を対応付ける処理は別途必要です。
ここでは、視線と点との距離を小さくする初期推定を独自に整理します。$A_t=I-\mathbf d_t\mathbf d_t^\top$ とおくと、
\widehat{\mathbf X}
=\arg\min_{\mathbf X}\sum_t w_t\|A_t(\mathbf X-\mathbf C_t)\|^2
です。$w_t$ は観測品質に基づく非負の重みです。正規方程式は、
\left(\sum_t w_tA_t\right)\widehat{\mathbf X}
=\sum_t w_tA_t\mathbf C_t
となります。左辺の行列が十分良条件なら解けます。実装では逆行列を明示的に作るより、線形方程式として解きます。
視線がほぼ平行、車両が停止したまま、対象に向かってほぼ正面から接近するなどの場合は、深度が決まりにくくなります。観測数だけでなく基線・視差が必要です。
この初期値から、画素の再投影誤差を最小化して改善できます。
\widehat{\mathbf X}
=\arg\min_{\mathbf X}
\sum_t\rho\left(
\mathbf e_t^{\top}\Sigma_{p,t}^{-1}\mathbf e_t
\right),
\quad
\mathbf e_t=\mathbf p_t^{(2)}-
\pi\left(K_cR_t^\top(\mathbf X-\mathbf C_t)\right)
$\pi([a,b,c]^\top)=(a/c,b/c)^\top$、$\Sigma_{p,t}$ は画素観測の共分散、$\rho$ は外れ値の影響を抑える関数です。前方深度や視差も検査します。カメラ姿勢も不確かなら、姿勢を含む同時最適化や、その誤差を含む評価が必要です。
ここではカメラ位置がメートル単位の共通座標系で与えられることを仮定しています。単眼SfMだけの相対座標は、そのまま緯度・経度にはならず、実寸スケールと地理座標への整合が必要です。
8. 複数観測の位置を、一つの地物位置へ統合する
路面との交差や測距などから、同一地物の同じ代表点について世界座標の観測 $\mathbf y_t$ が得られる場合を考えます。対象が静止していれば、
\mathbf X_t=\mathbf X_{t-1},
\qquad
\mathbf y_t=\mathbf X_t+\boldsymbol\epsilon_t,
\qquad \operatorname{Cov}(\boldsymbol\epsilon_t)=\Sigma_t
と置けます。理想的な静止モデルでは $F=H=I,Q=0$ として、先ほどのカルマン更新を位置推定にも使えます。3次元なら状態は $(X,Y,Z)$、2次元なら $(X,Y)$ です。
各観測が独立・不偏で、初期事前分布を使わない場合の一括統合は、
P=\left(\sum_t\Sigma_t^{-1}\right)^{-1},
\qquad
\widehat{\mathbf X}=P\sum_t\Sigma_t^{-1}\mathbf y_t
です。全観測を同じ重みで平均するのではなく、位置の不確かさに応じて重み付けします。
ただし、連続フレームではGNSSの偏り、校正誤差、同じSfM推定などを共有します。例として、
\mathbf y_t=\mathbf X+\mathbf b+\boldsymbol\eta_t
で、共通誤差 $\mathbf b$ の共分散を $\Sigma_b$、独立誤差の共分散を $\Sigma_\eta$ とすると、平均の誤差共分散は、
\operatorname{Cov}(\overline{\mathbf y}-\mathbf X)
=\Sigma_b+\frac{\Sigma_\eta}{n}
となります。観測を増やしても共通誤差は残ります。固定した未知の偏りの場合も、平均操作ではその偏りを取り除けません。
平滑化された追跡出力を、生の独立観測として再利用しないことも重要です。 同じフレームを共有する三角測量結果同士も相関します。独立性がない場合は相互共分散を扱うか、元の観測を一括で最適化するなどの設計が必要です。
9. 世界座標で追跡する拡張もできる
前節までの説明では、画像追跡と位置推定を分けました。位置の初期値が得られた後は、静止地物の世界座標を状態とし、画像点を観測とする方法もあります。
\mathbf z_t=h_t(\mathbf X)+\mathbf r_t,
\qquad
h_t(\mathbf X)=\pi\left(K_cR_t^\top(\mathbf X-\mathbf C_t)\right)
$h_t$ は非線形なので、拡張カルマンフィルタでは現在の推定点で線形化します。
H_t=\left.\frac{\partial h_t}{\partial\mathbf X}\right|_{\widehat{\mathbf X}_t^{-}}
第3節の残差を $\mathbf z_t-h_t(\widehat{\mathbf X}_t^{-})$、観測行列を $H_t$ に置き換えて更新します。
これにより、地図上の静止地物を、カメラの移動に応じた画像位置へ予測できます。ただし、同じ物理点を観測することが条件です。外観が変わる矩形中心を無条件にこの観測へ使うことはできません。
また、フィルタを使っても、一枚の画像から未知の距離が決まるわけではありません。三角測量・路面交差・測距などによる初期化と、十分な観測条件が必要です。この方法は第8節の同じ観測による更新へ重ねて追加するのではなく、処理構成の選択肢です。
10. 追跡の断片を地物IDへ統合する
二つの追跡から得た位置を $\widehat{\mathbf X}_i,\widehat{\mathbf X}_j$ とします。共分散が独立と近似できる場合、候補判定に、
d_{\mathrm{map}}^2=
(\widehat{\mathbf X}_i-\widehat{\mathbf X}_j)^\top
(P_i+P_j)^{-1}
(\widehat{\mathbf X}_i-\widehat{\mathbf X}_j)
を使えます。相互共分散 $C_{ij}$ があるなら、差の共分散は $P_i+P_j-C_{ij}-C_{ij}^\top$ です。
ただし、近接だけでは同一地物とは限りません。損傷種別、形状、外観、道路の左右・車線、代表点の定義、撮影時期も併用します。共分散が大きいほど近接候補が広がるため、不確かな位置だけを理由に統合してはいけません。
最終的な地物数は、確認済み・対象範囲内などの登録条件を満たすasset_idの種類数として、
N=|\mathcal A_{\mathrm{accepted}}|
と定義できます。新しい観測は既存のasset_idに追加し、同じ地物の件数を増やしません。
一方、ひび割れのように検出が分割・結合する対象は、一対一の矩形追跡だけでは扱いきれません。共通の路面座標へマスクを投影し、例えば、
\Omega_{\mathrm{damage}}=\bigcup_t\Omega_t
と空間的な和集合を作れば、同じ領域を何度観測しても面積を単純加算せずに済みます。ただし、この式は位置と判定が正しい場合の理想化です。実際には誤検出・投影ずれで領域が膨らむため、品質確認や確率的な統合が必要です。
また、未観測域は無損傷ではありません。損傷率の分母には、評価可能な観測済み路面範囲を使い、グリッド方式など採用した定義を明示します。
11. GISへ保存する内容と検証
最終出力には、点だけでなく根拠と不確かさを残します。
| 区分 | 主な属性 |
|---|---|
| 地物 | asset_id、種類、代表点の定義、geometry |
| 追跡との関係 | 走行ID、track_id、地物IDへの対応 |
| 観測 | detection_id、フレームID、撮影日時、画像参照、カメラ位置・姿勢 |
| 位置推定 | 路面交差・三角測量・測距などの方法、座標系、共分散 |
| 品質 | 再投影誤差、視差、実観測数、外れ値数、検証状況 |
| 点検時点の状態 | 点検日時、損傷程度、確認結果、補修状況 |
2次元の点はPoint、3次元の点はPoint Z、損傷形状はLineStringやPolygonなどとして管理できます。Zを保存する場合は、楕円体高か標高かなど、高さの基準も明示します。緯度・経度の数値をメートル単位の距離式へ直接入れてはいけません。
処理の検証では、画像上の検出精度だけでなく、次を確認します。
- 同一地物でIDが切れる頻度と、別地物へIDが入れ替わる頻度。
- 台帳上の重複登録と、異なる地物の過剰統合。
- 実測した代表点との水平・高さの誤差。
- 共分散から想定した誤差範囲と、実際の誤差の整合。
- 同一区間を速度や走行日を変えて取得した場合の結果の安定性。
位置を推定できない追跡は、無理にカメラ位置へ置かず、「観測はあるが対象位置は未確定」として残します。
おわりに
カルマンフィルタは、複数フレームの情報をつなぐための重要な構成要素です。しかし、同一地物の判定、実空間の位置推定、台帳上の一意性は、それぞれ別の処理として設計する必要があります。
追跡でまとまった観測を、校正・カメラ姿勢・路面モデル・複数視点の幾何関係と組み合わせることで、初めて対象の位置へ結び付けられます。そして追跡の断片や再走行の観測を地物IDへ統合することで、「何回見えたか」を「何がどこに存在するか」へ整理できます。
保存すべきものは、検出のたびに増える地図上の点ではなく、一つの地物の位置・状態と、それを支える複数の観測です。
参考資料
- Welch and Bishop, An Introduction to the Kalman Filter — カルマンフィルタと拡張カルマンフィルタの基礎。
- OpenCV: cv::KalmanFilter — 状態予測・観測更新の式と実装インターフェース。
- Bewley et al., Simple Online and Realtime Tracking — カルマンフィルタと割当問題を組み合わせた複数物体追跡。
- OpenCV: Camera Calibration and 3D Reconstruction — カメラ投影モデルと三角測量。
-
オービタルネット：Youtube —
リアルタイム物体検出とカルマンフィルタを用いた車載動画に写る物体のカウント（キロポストカウント）。 -
オービタルネット：Youtube —
Pedestrian Counter with Deep Learning YOLOv3。 -
オービタルネット：Youtube —
Traffic Counter and Speed Checker (Object Tracking using YOLOv3 and Kalman filter)。
本記事の状態ベクトル、複合対応付け費用、地物IDへの統合構成、数値例は、説明用の設計・導出です。特定の製品や過去のデモの実装仕様、測位精度を表すものではありません。