コンピュータビジョン分野において、動く物体を正確に追跡することは、自動運転、監視システム、スポーツ分析など、多岐にわたるアプリケーションの基礎となります。本稿では、カルマンフィルターを活用し、バウンディングボックスによって表現される複数物体を効率的に追跡するための技術と、その具体的なPython実装について詳しく解説します。

カルマンフィルターは、ノイズを含む観測データからシステムの真の状態を推定するための強力なアルゴリズムです。物体追跡の文脈では、フレームごとの検出結果（バウンディングボックス）を観測値として利用し、物体の位置や速度といった状態量を推定・更新します。

状態空間の定義と運動モデル

本実装におけるカルマンフィルターは、各追跡オブジェクトに対して8次元の状態ベクトルを管理します。この状態ベクトルは、バウンディングボックスの中心座標 (x, y)、アスペクト比 (aspect_ratio)、高さ (height)、そしてそれらの速度成分で構成されます。具体的には、[center_x, center_y, aspect_ratio, height, velocity_x, velocity_y, velocity_aspect, velocity_height]となります。

物体の運動モデルとしては、シンプルな等速モデルを採用します。これは、短時間においては物体の速度が一定であると仮定するものです。観測モデルは線形であり、バウンディングボックスの位置情報は状態空間から直接観測されると見なされます。

追跡のデータ関連付けにおいて、マハラノビス距離を用いたゲーティング処理の閾値として、以下のカイ二乗分布の95パーセンタイル値が利用されます。

import numpy as np
import scipy.linalg

# カイ二乗分布の95パーセンタイル値 (自由度Nに対して)
CHI2_95_PERCENTILE = {
    1: 3.8415, 2: 5.9915, 3: 7.8147, 4: 9.4877,
    5: 11.070, 6: 12.592, 7: 14.067, 8: 15.507, 9: 16.919
}

カルマンフィルターの実装

BoundingBoxStateEstimatorクラスは、カルマンフィルターの主要な機能をカプセル化しています。これには、フィルターの初期化、状態の予測、観測値による状態の更新、そして観測空間への状態の投影が含まれます。

class BoundingBoxStateEstimator:
    """
    画像空間におけるバウンディングボックスの追跡のためのカルマンフィルター。

    8次元の状態空間:
        [center_x, center_y, aspect_ratio, height,
         velocity_x, velocity_y, velocity_aspect, velocity_height]

    バウンディングボックスの中心座標、アスペクト比、高さ、
    およびそれぞれの速度を含みます。物体の動きは等速モデルに従うと仮定し、
    バウンディングボックスの位置は状態空間の直接的な観測値として扱われます (線形観測モデル)。
    """

    def __init__(self):
        # 位置、アスペクト比、高さの4つの状態成分
        state_component_count = 4
        # 時間ステップ (デルタt)
        time_step = 1.0 

        # 状態遷移行列 (F) の設定
        # 状態ベクトルは [p_x, p_y, p_a, p_h, v_x, v_y, v_a, v_h]
        # Fは 8x8 行列:
        # [ I  | dT*I ]
        # [ 0  |  I   ]
        self.state_transition_matrix = np.eye(2 * state_component_count, 2 * state_component_count)
        for i in range(state_component_count):
            self.state_transition_matrix[i, state_component_count + i] = time_step

        # 観測行列 (H) の設定
        # 観測値は [p_x, p_y, p_a, p_h] の4次元
        # Hは 4x8 行列:
        # [ I  |  0 ]
        self.observation_matrix = np.eye(state_component_count, 2 * state_component_count)

        # モデルの不確実性を制御する重み係数
        self.position_uncertainty_scale = 1.0 / 20.0
        self.velocity_uncertainty_scale = 1.0 / 160.0

    def initialize_track(self, initial_measurement):
        """
        与えられた初期観測値から新しい追跡オブジェクトを初期化します。

        Parameters
        ----------
        initial_measurement : ndarray
            バウンディングボックスの観測座標 (center_x, center_y, aspect_ratio, height)。

        Returns
        -------
        (ndarray, ndarray)
            新しい追跡オブジェクトの平均ベクトル (8次元) と共分散行列 (8x8次元)。
            未観測の速度成分は0で初期化されます。
        """
        # 速度成分は初期観測値の形状に合わせて0で初期化
        initial_state_mean = np.concatenate((initial_measurement, np.zeros_like(initial_measurement)))

        # 共分散行列の初期化値を設定
        # 位置、アスペクト比、高さの不確実性は観測された高さに基づいてスケーリング
        # 速度の不確実性にはより大きな重みを割り当て
        current_height = initial_measurement[3] # 観測された高さ
        initial_std_devs = [
            2 * self.position_uncertainty_scale * current_height, # center_x
            2 * self.position_uncertainty_scale * current_height, # center_y
            1e-2, # aspect_ratio (小さな初期不確実性)
            2 * self.position_uncertainty_scale * current_height, # height
            10 * self.velocity_uncertainty_scale * current_height, # velocity_x
            10 * self.velocity_uncertainty_scale * current_height, # velocity_y
            1e-5, # velocity_aspect (非常に小さな不確実性)
            10 * self.velocity_uncertainty_scale * current_height  # velocity_height
        ]
        initial_state_covariance = np.diag(np.square(initial_std_devs))
        return initial_state_mean, initial_state_covariance

    def predict_next_state(self, current_mean, current_covariance):
        """
        カルマンフィルターの予測ステップを実行し、次時刻の状態を推定します。

        Parameters
        ----------
        current_mean : ndarray
            前時刻のオブジェクト状態の8次元平均ベクトル。
        current_covariance : ndarray
            前時刻のオブジェクト状態の8x8次元共分散行列。

        Returns
        -------
        (ndarray, ndarray)
            予測された状態の平均ベクトルと共分散行列。
        """
        # 予測された平均: x_pred = F * x_curr
        predicted_mean = np.dot(self.state_transition_matrix, current_mean)

        # 運動ノイズ共分散 (Q) の計算
        # 各状態成分の標準偏差を設定。高さ情報に基づいて動的にスケーリング
        height_val = current_mean[3]
        position_std_components = [self.position_uncertainty_scale * height_val] * 2 + \
                                  [1e-2, self.position_uncertainty_scale * height_val]
        velocity_std_components = [self.velocity_uncertainty_scale * height_val] * 2 + \
                                  [1e-5, self.velocity_uncertainty_scale * height_val]
        
        motion_noise_std_devs = np.concatenate((position_std_components, velocity_std_components))
        motion_noise_covariance = np.diag(np.square(motion_noise_std_devs))

        # 予測された共分散: P_pred = F * P_curr * F^T + Q
        predicted_covariance = np.linalg.multi_dot((
            self.state_transition_matrix, current_covariance, self.state_transition_matrix.T
        )) + motion_noise_covariance

        return predicted_mean, predicted_covariance

    def project_to_measurement_space(self, state_mean, state_covariance):
        """
        現在の状態分布を観測空間に投影します。

        Parameters
        ----------
        state_mean : ndarray
            状態の平均ベクトル (8次元配列)。
        state_covariance : ndarray
            状態の共分散行列 (8x8次元)。

        Returns
        -------
        (ndarray, ndarray)
            投影された平均と共分散行列。
        """
        # 観測空間への投影された平均: z_pred = H * x_pred
        projected_mean = np.dot(self.observation_matrix, state_mean)

        # 観測ノイズ共分散 (R) の計算
        # 各観測成分の標準偏差。高さ情報に基づいてスケーリング
        height_val = state_mean[3]
        measurement_noise_std_devs = [self.position_uncertainty_scale * height_val] * 2 + \
                                     [1e-1, self.position_uncertainty_scale * height_val]
        measurement_noise_covariance = np.diag(np.square(measurement_noise_std_devs))

        # 観測空間での共分散: S = H * P_pred * H^T + R
        projected_covariance = np.linalg.multi_dot((
            self.observation_matrix, state_covariance, self.observation_matrix.T
        )) + measurement_noise_covariance

        return projected_mean, projected_covariance

    def batch_predict_states(self, means, covariances):
        """
        複数のオブジェクト状態に対してカルマンフィルターの予測ステップをバッチ処理で実行します。

        Parameters
        ----------
        means : ndarray
            前時刻のオブジェクト状態のN x 8次元平均行列 (Nはオブジェクト数)。
        covariances : ndarray
            前時刻のオブジェクト状態のN x 8 x 8次元共分散行列。

        Returns
        -------
        (ndarray, ndarray)
            予測された状態の平均ベクトルと共分散行列。
        """
        # 予測された平均 (N x 8): predicted_means = F * means_T を転置
        predicted_means = np.dot(means, self.state_transition_matrix.T)

        # 各状態の運動ノイズ共分散 (Q) をバッチで計算
        height_components = means[:, 3] # 各オブジェクトの高さ成分
        pos_stds_batch = np.array([
            self.position_uncertainty_scale * height_components,
            self.position_uncertainty_scale * height_components,
            1e-2 * np.ones_like(height_components), # アスペクト比の標準偏差
            self.position_uncertainty_scale * height_components
        ])
        vel_stds_batch = np.array([
            self.velocity_uncertainty_scale * height_components,
            self.velocity_uncertainty_scale * height_components,
            1e-5 * np.ones_like(height_components), # 速度アスペクト比の標準偏差
            self.velocity_uncertainty_scale * height_components
        ])
        
        # 標準偏差の二乗を連結し、各オブジェクトの対角要素として使用
        motion_noise_squared_stds = np.vstack((pos_stds_batch, vel_stds_batch)).T
        batch_motion_noise_covariance = np.array([np.diag(s) for s in motion_noise_squared_stds])

        # 予測された共分散 (N x 8 x 8): P_pred = F P F^T + Q
        # np.einsum を用いて効率的にバッチ処理
        term1 = np.einsum('ij,njk->nik', self.state_transition_matrix, covariances)
        predicted_covariances = np.einsum('nik,kj->nij', term1, self.state_transition_matrix.T) + batch_motion_noise_covariance

        return predicted_means, predicted_covariances

    def update_state(self, predicted_mean, predicted_covariance, actual_measurement):
        """
        カルマンフィルターの更新 (修正) ステップを実行します。

        Parameters
        ----------
        predicted_mean : ndarray
            予測された状態の平均ベクトル (8次元)。
        predicted_covariance : ndarray
            予測された状態の共分散行列 (8x8次元)。
        actual_measurement : ndarray
            4次元の観測ベクトル (center_x, center_y, aspect_ratio, height)。

        Returns
        -------
        (ndarray, ndarray)
            観測によって修正された状態分布。
        """
        # 観測空間への投影された状態と共分散を取得
        projected_mean_meas_space, projected_covariance_meas_space = self.project_to_measurement_space(
            predicted_mean, predicted_covariance
        )

        # カルマンゲイン (K) の計算: K = P_pred * H^T * S^-1
        # S は観測空間での共分散 (projected_covariance_meas_space)
        cholesky_factor, lower_flag = scipy.linalg.cho_factor(
            projected_covariance_meas_space, lower=True, check_finite=False
        )
        
        # P_pred * H^T を計算
        covariance_x_observation_transpose = np.dot(predicted_covariance, self.observation_matrix.T)
        
        # カルマンゲインを解く
        kalman_gain = scipy.linalg.cho_solve(
            (cholesky_factor, lower_flag), covariance_x_observation_transpose, check_finite=False
        ).T

        # 観測残差 (イノベーション): innovation = z_actual - z_pred
        innovation = actual_measurement - projected_mean_meas_space

        # 状態の更新: x_new = x_pred + K * innovation
        updated_mean = predicted_mean + np.dot(kalman_gain, innovation)
        
        # 共分散の更新: P_new = P_pred - K * S * K^T
        updated_covariance = predicted_covariance - np.linalg.multi_dot((
            kalman_gain, projected_covariance_meas_space, kalman_gain.T
        ))
        return updated_mean, updated_covariance

    def calculate_gating_distance(self, state_mean, state_covariance, measurements_batch,
                                  position_only=False, distance_metric='mahalanobis'):
        """
        状態分布と観測値間のゲーティング距離を計算します。
        適切な距離閾値は `CHI2_95_PERCENTILE` から取得できます。
        `position_only` がFalseの場合、カイ二乗分布は4自由度を持ち、そうでない場合は2自由度です。

        Parameters
        ----------
        state_mean : ndarray
            単一の状態分布の平均ベクトル (8次元)。
        state_covariance : ndarray
            単一の状態分布の共分散行列 (8x8次元)。
        measurements_batch : ndarray
            N個の観測値のN x 4次元行列。各観測値は (center_x, center_y, aspect_ratio, height) 形式。
        position_only : Optional[bool]
            Trueの場合、距離計算はバウンディングボックスの中心位置のみに対して行われます。
        distance_metric : str
            使用する距離メトリック ('gaussian' または 'mahalanobis')。

        Returns
        -------
        ndarray
            長さNの配列。i番目の要素には、(state_mean, state_covariance) と `measurements_batch[i]`
            の間の二乗距離が含まれます。
        """
        # 観測空間への投影
        projected_mean, projected_covariance = self.project_to_measurement_space(state_mean, state_covariance)

        if position_only:
            # 位置のみを考慮する場合、平均と共分散、観測値の最初の2要素を使用
            projected_mean = projected_mean[:2]
            projected_covariance = projected_covariance[:2, :2]
            measurements_batch = measurements_batch[:, :2]

        # 残差ベクトル (イノベーション)
        residuals = measurements_batch - projected_mean

        if distance_metric == 'gaussian':
            # ユークリッド距離の二乗 (簡略化された距離)
            return np.sum(residuals * residuals, axis=1)
        elif distance_metric == 'mahalanobis':
            # マハラノビス距離の二乗 d_M^2 = r^T S^-1 r
            cholesky_factor = np.linalg.cholesky(projected_covariance)
            # z = S^(-1/2) r を計算。S = L L^T なので、L z = r を解く
            z = scipy.linalg.solve_triangular(
                cholesky_factor, residuals.T, lower=True, check_finite=False,
                overwrite_b=True
            )
            # d_M^2 = z^T z
            squared_mahalanobis_distance = np.sum(z * z, axis=0)
            return squared_mahalanobis_distance
        else:
            raise ValueError(f"無効な距離メトリックが指定されました: {distance_metric}")