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;}