C++ camshift+kalman滤波源码

将开发过程经常用的一些内容段备份一次，下面的资料是关于C++ camshift+kalman滤波的内容。

#include "cv.h"
#include "highgui.h"
CvRect my_ChangeRect(CvRect A,CvRect B)
{
    if(B.x<A.x)
        B.x=A.x;
    if(B.x+B.width>A.width)
        B.width=A.width-B.x;
    if(B.y<A.y)
        B.y=A.y;
    if(B.y+B.height>A.height)
        B.height=A.height-B.y;
    return(B);
}
{
 
    const float A[] = {1,0,1,0,
        0,1,0,1,
        0,0,1,0,
        0,0,0,1};
    kalman = cvCreateKalman( 4, 2, 0 );
    return kalman;
}
{
    memcpy( kalman->state_post->data.fl, input, sizeof(input));
}
{
    float input[4] = {point2.x, point2.y, point2.x-point1.x, point2.y-point1.y};
    memcpy( mat->data.fl, input, sizeof(input));
    return mat;
}
#define region 50
#define calc_point(kalman) cvPoint( cvRound(kalman[0]),cvRound(kalman[1]))
 
 
 
int select_object = 0;
int track_object;
 
CvPoint origin;
CvPoint point_text;
CvRect selection;
CvRect origin_box;
 
CvRect track_window;
CvConnectedComp track_comp;
float hranges_arr[] = {0,180};
int vmin = 10, vmax = 256, smin = 30;
int start_track=0;
CvRect search_window;
int filename1_n=0;
int frame_count=0;
 
{
    if( !image )
        return;
 
    if( image->origin )
        y = image->height - y;
 
    if( select_object )
    {
        selection.x = MIN(x,origin.x);
        selection.y = MIN(y,origin.y);
        selection.width = selection.x + CV_IABS(x - origin.x);
        selection.height = selection.y + CV_IABS(y - origin.y);
 
        selection.x = MAX( selection.x, 0 );
        selection.y = MAX( selection.y, 0 );
        selection.width = MIN( selection.width, image->width );
        selection.height = MIN( selection.height, image->height );
        selection.width -= selection.x;
        selection.height -= selection.y;
 
    }
 
    switch( event )
    {
    case CV_EVENT_LBUTTONDOWN:
        origin = cvPoint(x,y);
        selection = cvRect(x,y,0,0);
        select_object = 1;
        break;
    case CV_EVENT_LBUTTONUP:
        select_object = 0;
        if( selection.width > 0 && selection.height > 0 )
            track_object = -1; 
        origin_box=selection;
 
 
#ifdef _DEBUG
        printf("n # 鼠标的选择区域："); 
        printf("n X = %d, Y = %d, Width = %d, Height = %d",
            selection.x, selection.y, selection.width, selection.height);
#endif
        break;
    }
}
{
    capture = cvCaptureFromAVI("e:/video/test.mp4");
    if( !capture )
    {
        fprintf(stderr,"Could not initialize capturing...n");
        return -1;
    }
 
    cvNamedWindow( "CamShiftDemo", 1 );
    cvCreateTrackbar( "Vmin", "CamShiftDemo", &vmin, 256, 0 );
    cvCreateTrackbar( "Vmax", "CamShiftDemo", &vmax, 256, 0 );
    cvCreateTrackbar( "Smin", "CamShiftDemo", &smin, 256, 0 );
    kalman= InitializeKalman(kalman);
 
    for(;;)
    {
        int i=0,c;
        frame = cvQueryFrame( capture );
        if( !frame )
            break;
        if( !image )
        {
            image = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
            hsv = cvCreateImage( cvGetSize(frame), 8, 3 );
            hue = cvCreateImage( cvGetSize(frame), 8, 1 );
            mask = cvCreateImage( cvGetSize(frame), 8, 1 );
 
            backproject = cvCreateImage( cvGetSize(frame), 8, 1 ); 
            backproject->origin = frame->origin;
 
        } 
        cvCopy( frame, image, 0 );
        frame_count++;
        if(track_object)
        {
            int _vmin = vmin, _vmax = vmax; 
            cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0),
            if( track_object < 0 )
            {
                float max_val = 0.f;
                cvResetImageROI( mask );
                track_window = origin_box; 
                track_object=1;
                lastpoint = predictpoint = cvPoint(track_window.x + track_window.width/2,
                    track_window.y + track_window.height/2 );
            }
            predictpoint = calc_point(prediction->data.fl); 
            track_window = cvRect(predictpoint.x - track_window.width/2, predictpoint.y - track_window.height/2, 
                track_window.width, track_window.height); 
            track_window=my_ChangeRect(cvRect(0,0,frame->width,frame->height),track_window); 
            search_window = cvRect(track_window.x - region, track_window.y - region, 
            search_window=my_ChangeRect(cvRect(0,0,frame->width,frame->height),search_window);
            cvSetImageROI( hue, search_window );
            cvSetImageROI( mask, search_window );
            cvSetImageROI( backproject, search_window ); 
            cvAnd( backproject, mask, backproject, 0 ); 
            track_window = cvRect(region, region, track_window.width, track_window.height);
            cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
            cvMeanShift( backproject, track_window,
                cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                &track_comp);
            cvResetImageROI( hue);
            cvResetImageROI( mask );
            cvResetImageROI( backproject );
            track_window=track_comp.rect;
            track_window = cvRect(track_window.x + search_window.x, track_window.y +search_window.y, 
                track_window.width, track_window.height);
            measurepoint = cvPoint(track_window.x + track_window.width/2, 
                track_window.y + track_window.height/2 ); 
            realposition->data.fl[0]=measurepoint.x;
            realposition->data.fl[1]=measurepoint.y;
            realposition->data.fl[2]=measurepoint.x - lastpoint.x;
            realposition->data.fl[3]=measurepoint.y - lastpoint.y;
            cvKalmanCorrect( kalman, measurement ); 
            cvRectangle( image , cvPoint(track_window.x,track_window.y),
                cvPoint(track_window.x+track_window.width,track_window.y+track_window.height),
                CV_RGB(255,0,0),2, 8, 0 );
            cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,1.0F,1.0F,0,3,8);
            sprintf(nchar,"ID:%d",i+1);
            point_text=cvPoint(track_window.x,track_window.y+track_window.height);
            cvPutText(image,nchar,point_text,&font,CV_RGB(255,255,0)); 
        }
        if( select_object && selection.width > 0 && selection.height > 0 )
        {
            cvSetImageROI( image, selection );
            cvXorS( image, cvScalarAll(255), image, 0 );
            cvResetImageROI( image );
        }
        cvShowImage( "CamShiftDemo", image );
        c = cvWaitKey(30);
        if( c == 27 )
    }
    cvReleaseCapture( &capture );
    cvDestroyWindow("CamShiftDemo");
    return 0;
}