www.pudn.com > TractPoint.rar > TractPoint.cpp

// TractPoint.cpp : Defines the entry point for the application. 
// 
 
#include "stdafx.h" 
#include "cv.h" 
#include "highgui.h" 
#include  
 
 
int APIENTRY WinMain(HINSTANCE hInstance, 
                     HINSTANCE hPrevInstance, 
                     LPSTR     lpCmdLine, 
                     int       nCmdShow) 
{ 
 	// TODO: Place code here. 
	  /* A matrix data */ 
    const float A[] = { 1, 1, 0, 1 }; 
 
    IplImage *img = cvCreateImage( cvSize(500,500), 8, 3 ); 
    CvKalman* kalman = cvCreateKalman( 2, 1, 0 ); 
    /* state is (phi, delta_phi) - angle and angle increment */ 
    CvMat* state = cvCreateMat( 2, 1, CV_32FC1 ); 
    CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 ); 
    /* only phi (angle) is measured */ 
    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)); 
        /* choose random initial state */ 
        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); 
 
            /* predict point position */ 
            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 ); 
 
            /* generate measurement */ 
            cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement ); 
 
            measurement_angle = measurement->data.fl[0]; 
            measurement_pt = calc_point(measurement_angle); 
 
            /* plot points */ 
            #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 ); 
 
            /* adjust Kalman filter state */ 
            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 current simulation by pressing a key */ 
                break; 
        } 
        if( code == 27 ) /* exit by ESCAPE */ 
            break; 
    } 
 
   // return 0; 
	//cvRelease (&img); 
	return 0; 
}