Nose Tracking using Kalman Filter and Viola and Jones Classifier

class atsKalman
{
public:
    atsKalman()
    {
        KalmanFilter KF(4, 2, 0);
        Mat_<float> state(4, 1); /* (x, y, Vx, Vy) */
        Mat processNoise(4, 1, CV_32F);
        Mat_<float> measurement(2,1);
        measurement.setTo(Scalar(0));

        KFs = KF;
        measurements = measurement;
    }
    void setKalman(int x, int y)
    {
        KFs.statePre.at<float>(0) = x;
        KFs.statePre.at<float>(1) = y;
        KFs.statePre.at<float>(2) = 0;
        KFs.statePre.at<float>(3) = 0;
        KFs.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,0,0,   0,1,0,0,  0,0,1,0,  0,0,0,1);

        setIdentity(KFs.measurementMatrix);
        setIdentity(KFs.processNoiseCov, Scalar::all(1e-4));
        setIdentity(KFs.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(KFs.errorCovPost, Scalar::all(.1));

       
    }
    Point step1()
    {
        Mat prediction = KFs.predict();
        Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
        return predictPt;
    }
    Point step2()
    {
        Mat estimated = KFs.correct(measurements);
        Point statePt(estimated.at<float>(0),estimated.at<float>(1));
        return statePt;
    }
    void changeMeasure(int x,int y)
    {
        measurements(0) = x;
        measurements(1) = y;
    }
private:
    KalmanFilter KFs;
    Mat_<float> measurements;
};

Viola and Jones Classifier is used to detect the nose. the X and Y coordinate of the found nose is sent to the first step of Kalman which is the prediction part. The second step of measurement correction completes the kalman filter.

class atsViolaJones
{
public:
    atsViolaJones()
    {
        storage = cvCreateMemStorage( 0 );
        cascade = ( CvHaarClassifierCascade* )cvLoad("haarcascade_mcs_nose.xml", 0, 0, 0 );
    }
    cv::Point Find(cv::Mat image )
    {
        IplImage imgs =image;
        IplImage* img = &imgs;

       
       
        int i;
        CvSeq *faces = cvHaarDetectObjects(
        img,
        cascade,
        storage,
        1.1,
        3,
        0,
        cvSize( 40, 40 ) );

        //get one image
        CvRect *r = ( CvRect* )cvGetSeqElem( faces, 0 );

        cv::Point found;
        found.x = (r->x + r->width);
        found.y =  (r->y + r->height);

        return found;
    }
    ~atsViolaJones()
    {
        cvReleaseHaarClassifierCascade( &cascade );
        cvReleaseMemStorage( &storage );
    }
private:
    CvHaarClassifierCascade *cascade;
    CvMemStorage *storage;
};

UPDATED class to OPENCV 2.2:

class atsViolaJones
{
public:
    atsViolaJones(String filename)
    {
        if (filename == "")
            filename = "haarcascade_mcs_nose.xml";
        else
        {
        if( !face_cascade.load( filename ) ){ printf("--(!)Error loading\n"); };
        }
    }
    cv::Point Find(cv::Mat frame  )
    {
        std::vector<Rect> faces;
        Mat frame_gray;

        cvtColor( frame, frame_gray, CV_BGR2GRAY );
        equalizeHist( frame_gray, frame_gray );

        face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30) );

        cv::Point found;
        found.x = faces[0].x; //Face[0] is the first face found
        found.y =  faces[0].y;
       
        return found;
    }
private:
    CascadeClassifier face_cascade;

};

This is the main function

int main (int argc, char * const argv[]) {
    Mat img(500, 500, CV_8UC3);

    //init the stuff
    atsKalman ats;
    atscameraCapture movie;
    atsViolaJones haar;

    char code = (char)-1;
   
   
    for(;;)
    {
       
        //init kalman
        ats.setKalman(0,0);
       

       
        mousev.clear();
        kalmanv.clear();
       
        for(;;)
        {
            //get camera
            img = movie.ats_getImage();

           
            //Step 1
            Point predictPt = ats.step1();

            //object X Y coordinte from another method
            cv::Point found;
            found = haar.Find(img);
            int x = found.x;
            int y = found.y;

            //
            ats.changeMeasure(x,y);
           
            Point measPt(x,y);
            mousev.push_back(measPt);

            //Step 2
            Point statePt = ats.step2();
            kalmanv.push_back(statePt);
           
            // plot points
            #define drawCross( center, color, d )                                 \
            line( img, Point( center.x - d, center.y - d ),                \
            Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
            line( img, Point( center.x + d, center.y - d ),                \
            Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )


            drawCross( statePt, Scalar(0,255,255), 5 );
            drawCross( measPt, Scalar(0,0,255), 5 );
           
            for (int i = 0; i < mousev.size()-1; i++) {
                line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
            }
            for (int i = 0; i < kalmanv.size()-1; i++) {
                line(img, kalmanv[i], kalmanv[i+1], Scalar(0,255,0), 1);
            }
           
           
            imshow( "kalman", img );

            //imshow("kalman",haar.Find(img));
            code = (char)waitKey(100);
           
            if( code > 0 )
                break;
        }
        if( code == 27 || code == 'q' || code == 'Q' )
            break;
    }
   
    return 0;
}