cv::KalmanFilter の使い方【OpenCV/C++】〜カルマンフィルタで物体追跡〜

OpenCV for C++
📌 準備: OpenCV の環境構築がまだの方はこちら → C++ 環境構築ガイド / C#(OpenCvSharp)セットアップ

cv::KalmanFilter の使い方【OpenCV/C++】〜カルマンフィルタで物体追跡〜

cv::KalmanFilter は、ノイズを含む観測値から状態の最良推定値を求めるフィルタです。物体追跡において「検出が1フレーム途切れた」「座標がジッタする」といった問題をシンプルなコードで解消できます。

最小の構成イメージ:

cv::KalmanFilter kf(4, 2, 0);   // 状態4次元(x,y,vx,vy), 観測2次元(x,y)
cv::Mat pred  = kf.predict();   // 次フレームの状態を予測
cv::Mat meas  = (cv::Mat_<float>(2,1) << mx, my);
cv::Mat est   = kf.correct(meas); // 観測で補正 → 最良推定値

動作環境

  • OpenCV 4.x
  • コンパイル: g++ -std=c++17 main.cpp $(pkg-config --cflags --libs opencv4) -o main

環境構築がまだの方は環境構築ガイドを先にどうぞ。


基本の使い方

円軌道上を動く点(真値)にガウスノイズを加えた「観測点」をカルマンフィルタで平滑化する最小サンプルです。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <random>

int main()
{
    // --- カルマンフィルタの初期化 ---
    // 状態ベクトル: [x, y, vx, vy]  (位置 + 速度)
    // 観測ベクトル: [x, y]
    const int stateSize = 4;
    const int measSize  = 2;
    const int ctrlSize  = 0;
    cv::KalmanFilter kf(stateSize, measSize, ctrlSize, CV_32F);

    // 状態遷移行列 A: 等速直線運動モデル
    // x' = x + vx*dt,  y' = y + vy*dt  (dt=1フレーム)
    kf.transitionMatrix = (cv::Mat_<float>(4, 4) <<
        1, 0, 1, 0,
        0, 1, 0, 1,
        0, 0, 1, 0,
        0, 0, 0, 1);

    // 観測行列 H: 状態から観測への射影 (x,y だけ取り出す)
    kf.measurementMatrix = (cv::Mat_<float>(2, 4) <<
        1, 0, 0, 0,
        0, 1, 0, 0);

    // プロセスノイズ共分散 Q (小さいほど予測モデルを信頼)
    cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-4));

    // 観測ノイズ共分散 R (大きいほど観測を疑う)
    cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1e-1));

    // 事後誤差共分散の初期値
    cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1));

    // 状態初期値 (画面中央)
    kf.statePost = (cv::Mat_<float>(4, 1) << 320, 240, 0, 0);

    // --- シミュレーション: 円軌道 + ガウスノイズ ---
    std::mt19937 rng(42);
    std::normal_distribution<float> noise(0.0f, 10.0f); // σ=10px

    cv::Mat canvas(480, 640, CV_8UC3);
    cv::Mat meas(measSize, 1, CV_32F);

    std::cout << "frame, true_x, true_y, obs_x, obs_y, est_x, est_y\n";

    for (int t = 0; t < 120; ++t)
    {
        // 真値: 円軌道
        float angle  = t * 3.0f * CV_PI / 180.0f;
        float true_x = 320 + 150 * std::cos(angle);
        float true_y = 240 + 150 * std::sin(angle);

        // 観測値: 真値 + ノイズ
        float obs_x = true_x + noise(rng);
        float obs_y = true_y + noise(rng);

        // 予測ステップ
        cv::Mat pred = kf.predict();

        // 補正ステップ
        meas.at<float>(0) = obs_x;
        meas.at<float>(1) = obs_y;
        cv::Mat est = kf.correct(meas);

        float est_x = est.at<float>(0);
        float est_y = est.at<float>(1);

        std::cout << t      << ", "
                  << true_x << ", " << true_y << ", "
                  << obs_x  << ", " << obs_y  << ", "
                  << est_x  << ", " << est_y  << "\n";

        // 描画
        canvas = cv::Scalar(30, 30, 30);
        cv::circle(canvas, {(int)obs_x,  (int)obs_y},  5, {0, 100, 255}, -1); // 観測 (橙)
        cv::circle(canvas, {(int)est_x,  (int)est_y},  5, {0, 255,   0}, -1); // 推定 (緑)
        cv::circle(canvas, {(int)true_x, (int)true_y}, 5, {255, 255,  0}, -1); // 真値 (黄)
        cv::putText(canvas, "Orange: observed  Green: estimated  Yellow: true",
                    {10, 460}, cv::FONT_HERSHEY_SIMPLEX, 0.5, {200,200,200}, 1);

        cv::imshow("KalmanFilter demo", canvas);
        if (cv::waitKey(30) == 27) break; // ESC で終了
    }

    cv::destroyAllWindows();
    return 0;
}

実行結果

ウィンドウに3色の点が表示されます。

frame, true_x, true_y, obs_x, obs_y, est_x, est_y
0, 470, 240, 464.83, 252.219, 457.934, 251.637
1, 469.794, 247.85, 477.008, 256.547, 483.133, 256.658
...

橙点(観測)がジッタするのに対し、緑点(推定)は真値(黄)に追従して滑らかに動きます。フレームが進むにつれてフィルタが収束し、推定精度が向上します。


引数と戻り値

コンストラクタ

引数 型 説明
dynamParams int 状態ベクトルの次元数
measureParams int 観測ベクトルの次元数
controlParams int 制御ベクトルの次元数(不要なら 0)
type int 行列の型(CV_32F または CV_64F)

主なメンバ変数(行列)

メンバ サイズ 意味
transitionMatrix state × state 状態遷移行列 A
measurementMatrix meas × state 観測行列 H
processNoiseCov state × state プロセスノイズ共分散 Q
measurementNoiseCov meas × meas 観測ノイズ共分散 R
errorCovPost state × state 事後誤差共分散 P
statePost state × 1 事後状態推定(初期値設定に使う)

主なメソッド

メソッド 戻り値 説明
predict() cv::Mat (state×1) 予測ステップ。事前状態推定を返す
correct(measurement) cv::Mat (state×1) 補正ステップ。観測を受け取り事後推定を返す

実践例:検出が途切れたフレームの補間

物体検出器の出力は毎フレーム得られるとは限りません。検出ありのフレームは correct()、検出なしのフレームは predict() のみ呼ぶことで位置を補間できます。

#include <opencv2/opencv.hpp>
#include <vector>
#include <optional>

// 検出結果を模擬: 偶数フレームのみ観測が得られるシナリオ
std::optional<cv::Point2f> get_detection(int frame)
{
    if (frame % 2 == 0)
        return cv::Point2f(200.0f + frame * 1.5f, 240.0f);
    return std::nullopt; // 奇数フレームは検出なし
}

int main()
{
    cv::KalmanFilter kf(4, 2, 0, CV_32F);
    kf.transitionMatrix = (cv::Mat_<float>(4, 4) <<
        1, 0, 1, 0,
        0, 1, 0, 1,
        0, 0, 1, 0,
        0, 0, 0, 1);
    kf.measurementMatrix = (cv::Mat_<float>(2, 4) <<
        1, 0, 0, 0,
        0, 1, 0, 0);
    cv::setIdentity(kf.processNoiseCov,     cv::Scalar::all(1e-3));
    cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(5.0));
    cv::setIdentity(kf.errorCovPost,        cv::Scalar::all(1));
    kf.statePost = (cv::Mat_<float>(4, 1) << 200, 240, 1.5, 0);

    cv::Mat meas(2, 1, CV_32F);
    cv::Mat canvas(480, 640, CV_8UC3);

    for (int t = 0; t < 100; ++t)
    {
        // 予測は毎フレーム必ず呼ぶ
        cv::Mat pred = kf.predict();

        auto det = get_detection(t);
        cv::Mat est;

        if (det.has_value())
        {
            // 検出ありのフレームは correct() で補正
            meas.at<float>(0) = det->x;
            meas.at<float>(1) = det->y;
            est = kf.correct(meas);
        }
        else
        {
            // 検出なし: 予測値をそのまま使用 (correct() は呼ばない)
            est = pred;
        }

        float ex = est.at<float>(0);
        float ey = est.at<float>(1);

        canvas = cv::Scalar(30, 30, 30);
        // 検出なしフレームは推定値のみ表示
        if (det.has_value())
            cv::circle(canvas, {(int)det->x, (int)det->y}, 7, {0, 100, 255}, -1);
        cv::circle(canvas, {(int)ex, (int)ey}, 5, {0, 255, 0}, -1);

        cv::imshow("Interpolation demo", canvas);
        if (cv::waitKey(30) == 27) break;
    }

    cv::destroyAllWindows();
    return 0;
}

⚠️ predict() を呼ばずに correct() だけ呼ぶ、あるいは predict() を2回連続で呼ぶと誤差共分散が正しく更新されません。1フレームにつき必ず predict → (optional) correct の順で呼ぶことが重要です。


つまずきポイント

① 行列の型不一致による assertion

コンストラクタで CV_32F を指定した場合、measurementMatrix や transitionMatrix への代入も必ず CV_32F(cv::Mat_<float>)の行列で行う必要があります。CV_64F の行列を代入したり、correct() に CV_64F の観測を渡したりすると、内部の行列積(gemm)で assertion エラーが発生します。

// NG: CV_64F の Mat を CV_32F のフィルタに渡している
kf.transitionMatrix = cv::Mat::eye(4, 4, CV_64F); // → assertionエラー

// OK
kf.transitionMatrix = cv::Mat::eye(4, 4, CV_32F);

② predict() の呼び忘れ・二重呼び出し

predict() を呼ばずに correct() を呼んでも内部的に動作はしますが、誤差共分散が一貫した状態で更新されません。逆に predict() を1フレームに2回呼ぶと誤差共分散が余分に膨らみます。ループ構造を「1フレーム = predict 1回 + correct 0〜1回」に固定してください。

③ Q と R のチューニングを間違える

processNoiseCov (Q) を大きくするとモデルより観測を信頼し、推定が観測に追従しやすくなります(ジッタが残る)。逆に measurementNoiseCov (R) を大きくするとモデル予測を信頼し、推定が滑らかになります(追従が遅れる)。チューニングはアプリの要件(追従速度 vs 平滑度)に合わせて行ってください。値の大きさは絶対値ではなく Q と R の比率が効きます。


関連する関数


まとめ

cv::KalmanFilter は状態遷移行列・観測行列・ノイズ共分散を設定し、predict() → correct() を毎フレーム呼ぶだけで使えます。検出が欠けたフレームは predict() の結果で補間でき、追跡の途切れを最小限に抑えられます。Q と R の比率調整がチューニングの核心です。

🛠 画像処理のプロが開発するSDK/API

本ブログを運営するスワローインキュベートは、OpenCV ベースのなりすまし判定SDK/API(C++製・OpenCV 4.10)を開発しています。顔認証システムへの組み込み実績多数。

→ 顔認証のなりすまし対策 設計チェックリスト(当ブログ)
→ なりすまし判定SDK/APIの詳細を見る
→ API仕様書・サンプルコード

タイトルとURLをコピーしました