Friday, September 2, 2011

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

8 comments:

  1. using Equation of Time:
    KFs.statePre.at(0) = x;
    KFs.statePre.at(1) = y;
    KFs.statePre.at(2) = 0;
    KFs.statePre.at(3) = 0;
    KFs.statePre.at(4) = 0;
    KFs.statePre.at(5) = 0;
    KFs.transitionMatrix = *(Mat_(6, 6) << 1,0,1,0,0.5,0, 0,1,0,1,0,0.5, 0,0,1,0,1,0, 0,0,0,1,0,1, 0,0,0,0,1,0, 0,0,0,0,0,1);
    KFs.measurementMatrix = *(Mat_(2, 6) << 1,0,1,0,0.5,0, 0,1,0,1,0,0.5);

    ReplyDelete
  2. is it possible to track multiple object with a kalmanfilter? i like to track some traffic signs in a video for example...

    ReplyDelete
  3. kalman filter does not work with multiple objects. there are many articles on the net you can find as reference.
    the only way kalman filter can be used for multiple object tracking, is if you know exactly what that object is. using meanshift/camshift or anything that can recognize an object in time T and time T+1. always remember kalamn works well assuming you know your object. initial prediction is based on that

    ReplyDelete
  4. let me explain it in a different way:
    if you look at the code: i take an X and Y coordinate from Viola and jones before feeding it to Kalamn, if i had 2 noses, i would get 2 X's and 2 Y's. how do i know that they are from different objects? therefore my recognition should allow me to separate the X and Y for each object before running Kalman

    ReplyDelete
    Replies
    1. well, you can extract each object using Blob analysis. and then apply kalman filter on each object location x, y ...
      Hope i would help.

      Delete
  5. ok I so in my first detection i know where the object (my traffic sign) is. now i would like to follow it over multiple frames. and maybe there are other traffic signs appears. whats the best technique?

    ReplyDelete
    Replies
    1. use blob analysis .. and better to work with Optical flow.

      Delete
  6. hi how to detect car in a video with open cv and visual studio?

    ReplyDelete