OpenCV学习之利用Kalman滤波器跟踪一个旋转的点

利用Kalman滤波器跟踪一个旋转的点

#include "cv.h"
#include "highgui.h"
#include <math.h>  
int main(int argc, char** argv) {

    const float A[] = { 1, 1, 0, 1 };

    IplImage* img = cvCreateImage(cvSize(500, 500), 8, 3);
    CvKalman* kalman = cvCreateKalman(2, 1, 0);
    // 状态是角度和角度增量  
    CvMat* state = cvCreateMat(2, 1, CV_32FC1);
    CvMat* process_noise = cvCreateMat(2, 1, CV_32FC1);
    // 只有角度被测量  
    CvMat* measurement = cvCreateMat(1, 1, CV_32FC1);
    CvRandState rng;
    int code = -1;

    cvRandInit(&rng, 0, 1, -1, CV_RAND_UNI);

    cvZero(measurement);
    cvNamedWindow("Kalman", 1);

    for (;;) {

        cvRandSetRange(&rng, 0, 0.1, 0);
        rng.disttype = CV_RAND_NORMAL;

        cvRand(&rng, state);

        memcpy(kalman->transition_matrix->data.fl, A, sizeof(A));
        cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1));
        cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));
        cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1));
        cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));
        // 选择随机的开始状态  
        cvRand(&rng, kalman->state_post);

        rng.disttype = CV_RAND_NORMAL;

        for (;;) {

#define calc_point(angle)                                     \
            cvPoint(cvRound(img->width / 2 + img->width / 3 * cos(angle)),\
                cvRound(img->height / 2 - img->width / 3 * sin(angle)))

                float state_angle = state->data.fl[0];
            CvPoint state_pt = calc_point(state_angle);

            // 预测点的方向  
            const CvMat* prediction = cvKalmanPredict(kalman, 0);
            float predict_angle = prediction->data.fl[0];
            CvPoint predict_pt = calc_point(predict_angle);
            float measurement_angle;
            CvPoint measurement_pt;

            cvRandSetRange(&rng, 0,
                sqrt(kalman->measurement_noise_cov->data.fl[0]), 0);
            cvRand(&rng, measurement);


            cvMatMulAdd(kalman->measurement_matrix,
                state, measurement, measurement);

            measurement_angle = measurement->data.fl[0];
            measurement_pt = calc_point(measurement_angle);


#define draw_cross( center, color, d )                                 \
            cvLine(img, cvPoint(center.x - d, center.y - d), \
                cvPoint(center.x + d, center.y + d), color, 1, 0); \
                cvLine(img, cvPoint(center.x + d, center.y - d), \
                    cvPoint(center.x - d, center.y + d), color, 1, 0)

                cvZero(img);
            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);
            cvLine(img, state_pt, predict_pt, CV_RGB(255, 255, 0), 3, 0);

            // 调整Kalman滤波器的状态  
            cvKalmanCorrect(kalman, measurement);

            cvRandSetRange(&rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0);
            cvRand(&rng, process_noise);
            cvMatMulAdd(kalman->transition_matrix, state, process_noise, state);

            cvShowImage("Kalman", img);
            code = cvWaitKey(100);

            if (code > 0)
                break;
        }
        if (code == 27)
            break;
    }

    return 0;
}