OpenCV2.1のC++実装を利用したkalman filterサンプル

OpenCV使い始めて８年．
最初の頃はIntel Primitive Library(IPL)とか
Intel Performance Primitives(IPP)とか，
Intelのライブラリが必須だったのに，
いつの間にかそいつらは不要になって単体で
動くようになったOpenCVも今は2.1がリリース
されちゃってます．

2.1で特質すべきはC++の実装があること．
CvSeq*とかCvMat*とかポインタのお化けプログラムから
おさらばしたくてC++のOpenCVにチャレンジしてますが，
サンプルが不足してて苦戦します．

今日はkalman filterのサンプルをC++実装に置き換えて
みたので調子にのってブログにアップしちゃいます．

と言っても１００％C++ライクになっているかといえば
嘘で行列周りでCvMatに戻ったりしてますがその辺は
許してください．
C++にしてよかったと思えるのは今のところ行列の計算
だけですね．

/*
   Tracking of rotating point.
   Rotation speed is constant.
   Both state and measurements vectors are 1D (a point angle),
   Measurement is the real point angle + gaussian noise.
   The real and the estimated points are connected with yellow line segment,
   the real and the measured points are connected with red line segment.
   (if Kalman filter works correctly,
    the yellow segment should be shorter than the red one).
   Pressing any key (except ESC) will reset the tracking with a different speed.
   Pressing ESC will stop the program.
*/
#define CV_NO_BACKWARD_COMPATIBILITY
#include "cv.h"
#include "highgui.h"
#include 
using namespace cv;
int main(int argc, char** argv)
{
    const float A[] = { 1, 1, 0, 1 };
    Mat img(500, 500, CV_8UC3);
    KalmanFilter kalman(2, 1, 0);
    Mat state(2, 1, CV_32FC1);
    Mat process_noise(2, 1, CV_32FC1);
    Mat measurement(1, 1, CV_32FC1);
    RNG rng(-1);
    char code = -1;
    measurement = 0;
    namedWindow("kalman", 1);
    for(;;)
    {
        rng.fill(state, CV_RAND_NORMAL, Scalar(0), Scalar(0.1));
        CvMat transition_matrix = kalman.transitionMatrix;
        memcpy(transition_matrix.data.fl, A, sizeof(A));
        CvMat measurement_matrix = kalman.measurementMatrix;
        cvSetIdentity(&measurement_matrix, cvRealScalar(1) );
        CvMat process_noise_cov = kalman.processNoiseCov;
        cvSetIdentity(&process_noise_cov, cvRealScalar(1e-5) );
        CvMat measurement_noise_cov = kalman.measurementNoiseCov;
        cvSetIdentity(&measurement_noise_cov, cvRealScalar(1e-1) );
        CvMat error_cov_post = kalman.errorCovPost;
        cvSetIdentity(&error_cov_post, cvRealScalar(1));
        rng.fill(kalman.statePost, CV_RAND_NORMAL, Scalar(0), Scalar(0.1));
        for(;;)
        {
            #define calc_point(angle)                                      \
                cvPoint( cvRound(img.cols/2 + img.cols/3*cos(angle)),  \
                         cvRound(img.rows/2 - img.rows/3*sin(angle)))
            float state_angle = state.at(0,0);//->data.fl[0];
            CvPoint state_pt = calc_point(state_angle);
            Mat prediction = kalman.predict();
            float predict_angle = prediction.at(0, 0);//->data.fl[0];
            CvPoint predict_pt = calc_point(predict_angle);
            float measurement_angle;
            CvPoint measurement_pt;
            rng.fill(measurement, CV_RAND_NORMAL, Scalar(0), Scalar(sqrt(kalman.measurementNoiseCov.at(0,0))));

            /* generate measurement */
            measurement = kalman.measurementMatrix * state + measurement;
            measurement_angle = measurement.at(0,0);//->data.fl[0];
            measurement_pt = calc_point(measurement_angle);

            /* plot points */
            #define draw_cross( center, color, d )                                 \
                line(img, Point( center.x - d, center.y - d ),                \
                             Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
                line(img, Point( center.x + d, center.y - d ),                \
                             Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 )

            //cvZero( img );
            img = 0;
            draw_cross( state_pt, CV_RGB(255,255,255), 3 );
            draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );
            draw_cross( predict_pt, CV_RGB(0,255,0), 3 );
            line(img, state_pt, measurement_pt, CV_RGB(255,0,0), 3, CV_AA, 0 );
            line(img, state_pt, predict_pt, CV_RGB(255,255,0), 3, CV_AA, 0 );
            kalman.correct(measurement);
            rng.fill(process_noise, CV_RAND_NORMAL, Scalar(0), Scalar(sqrt(kalman.processNoiseCov.at(0,0))));
            state = kalman.transitionMatrix * state + process_noise;
            imshow("kalman", img);
            code = (char) cvWaitKey( 100 );
            if( code > 0 )
                break;
        }
        if( code == 27 || code == 'q' || code == 'Q' )
            break;
    }
    cvDestroyWindow("Kalman");
    return 0;
}