CvConDensation - Conditional Density Propagation

i am currently looking in to different options to improve the tracking of an object.
a friend explain the usefullness of opencv’s CvConDensation to me. so we put together a small example.

I made it work with ofxCv but some parts of the code need ofxOpenCv (OF 007). So i had to add the ofxOpenCv headers too. Ideally i would make it work only with ofxCv, i.e. only with opencv 2.x .

… the Condensation is an algorithm (Conditional Density Propagation) which allows quite general representations of probability. Experimental results show that this increased generality does indeed lead to a marked improvement in tracking performance. In addition to permitting high-quality tracking in clutter, the simplicity of the Condensation algorithm also allows the use of non-linear motion models more complex than those commonly used in Kalman filters

some links:
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL-COPIES/ISARD1/condensation.html
http://opencv.willowgarage.com/documentation/c/motion-analysis-and-object-tracking.html#cvCreateConDensation

a simple example that tracks the x/y position of a horizontal line in a video. its takes two points, add the condensation, checks the grey value of n points along the line, uses the average to determine the confidence. the confidence is what helps CvConDensation to calculate a new set of “random” points that better fit the motion and shape of the object.

  
#include "testApp.h"  
  
//[http://opencv.willowgarage.com/documentation/c/motion-analysis-and-object-tracking.html#cvCreateConDensation](http://opencv.willowgarage.com/documentation/c/motion-analysis-and-object-tracking.html#cvCreateConDensation)  
  
  
using namespace ofxCv;  
using namespace cv;  
  
ofVideoPlayer 		vidPlayer;  
  
Mat sourceMat;  
Mat dstMat;  
  
double refPtA[2];  
double refPtB[2];  
double refLength;  
CvMat lowerBound;  
CvMat upperBound;  
CvConDensation* conDens;  
  
int dim;  
  
void testApp::setup() {  
	  
  
	vidPlayer.loadMovie("tige.mov");  
	vidPlayer.play();  
	  
	// B----A  
    // These are the two reference points that  
    // represent the line  
    // This would be the value from any type of prior initialization  
	refPtA[0] = 160.0;  
	refPtA[1] = 120.0;  
	refPtB[0] = 100.0;  
	refPtB[1] = 120.0;  
  
	  
	refLength = 60;  
  
  
	sourceMat = toCv(vidPlayer); //convert video imput to cv::mat  
	  
	dstMat = Mat(240,320,CV_8UC3);  
	  
  
    // Initialize condensation (2 params = 2 dimensions)  
	dim = 2;  
  
	initCondens();  
}  
  
  
void testApp::initCondens(){  
	conDens = cvCreateConDensation(dim,dim,30);  
      
    // Initialize the search boundaries  
	  
	lowerBound = cvMat(dim,1,CV_MAT32F,NULL);  
	upperBound = cvMat(dim,1,CV_MAT32F,NULL);  
	  
	cvmAlloc(&lowerBound);  
    cvmAlloc(&upperBound);  
	  
	for (int i=0 ; i<dim ; i++) {  
        // we assume the movement should stay between +/- 10 pixels  
        lowerBound.data.fl[i] = -10.0f;  
        upperBound.data.fl[i] =  10.0f;  
	}  
	  
	cvConDensInitSampleSet(conDens, &lowerBound, &upperBound);  
	  
	  
}  
  
// Compute the cost along the line  
//float testApp::computeLineCost(IplImage* img, double pt1[2], double pt2[2]) {  
float testApp::computeLineCost(double pt1[2], double pt2[2]) {  
      
    float dLambda = 0.01; // 100 points between the two vectors  
    int nbPt = (int)(1.0/dLambda);  
      
    double ptx, pty;  
      
    float totalValue = 0.0;  
      
    for ( int i=0 ; i<nbPt ; i++ ) {  
        float value = 0.0;  
          
        // (x',y') = (x1,y1) * lambda ((x2,y2) - (x1,y1))  
        ptx = pt1[0] + i*dLambda*(pt2[0]-pt1[0]);  
        pty = pt1[1] + i*dLambda*(pt2[1]-pt1[1]);  
		  
        // Here I simply round the values, the best  
        // would be to interpolate the values sub-pixel  
        int x = (int)round(ptx);  
        int y = (int)round(pty);  
          
		unsigned char r = sourceMat.at<cv::Vec3b>(y,x)[0];  
		unsigned char g = sourceMat.at<cv::Vec3b>(y,x)[1];  
		unsigned char b = sourceMat.at<cv::Vec3b>(y,x)[2];  
		  
        value = (r+g+b)/3.0/255.0; // bring the grayscale between 0 and 1  
        totalValue += value;  
    }  
      
	// A probability between 0 and 1  
    return (totalValue/nbPt);  
}  
  
// Compute the probabilities of all particules sampled for the current frame  
void testApp::computeParticuleProbabilities(CvConDensation* condDens, double refPtA[2], double refPtB[2]) {  
      
    double newPtA[2];  
    double newPtB[2];  
      
    float bestProb = 0.0;  
    int bestIdx = -1;  
      
    // For each particule  
    for (int p=0 ; p<condDens->SamplesNum ; p++) {  
        float prob = 1.0f;  
          
        // Retrieve the sample translation from the particule  
        // These are the two params that we randomly sample to do the tracking  
        double paramDTx = condDens->flSamples[p][0];  
        double paramDTy = condDens->flSamples[p][1];  
          
		cout<<"paramDT "<<paramDTx<<" "<<paramDTy<<endl;  
		  
        // Apply the particule dTx param on both points  
        newPtA[0] = refPtA[0] + paramDTx;  
        newPtB[0] = refPtB[0] + paramDTx;  
          
        // Apply the particule dTy param on both points  
        newPtA[1] = refPtA[1] + paramDTy;  
        newPtB[1] = refPtB[1] + paramDTy;  
          
        prob = computeLineCost(newPtA, newPtB);  
        condDens->flConfidence[p] = prob;  
          
        // Get the best scoring particule  
        if (prob > bestProb) {  
            bestIdx = p;  
            bestProb = prob;  
        }  
          
		cv::circle(dstMat, cvPoint(newPtA[0]-30, newPtA[1]), 3,  cvScalar(0, 255, 255), 1, 8, 0);  
          
    }  
      
    // The best scoring particule modifies the reference points  
      
    // Apply the particule dTx param on both points  
    refPtA[0] += condDens->flSamples[bestIdx][0];  
    refPtB[0] += condDens->flSamples[bestIdx][0];  
	  
    // Apply the particule dTy param on both points  
    refPtA[1] += condDens->flSamples[bestIdx][1];  
    refPtB[1] += condDens->flSamples[bestIdx][1];  
	  
	cv::line(dstMat, cvPoint((int)refPtA[0], (int)refPtA[1]), cvPoint((int)refPtB[0], (int)refPtB[1]), cvScalar(0,0,255), 2, 8, 0);  
}  
  
void testApp::update() {  
	  
	bool bNewFrame = false;  
	  
	  
	vidPlayer.idleMovie();  
	bNewFrame = vidPlayer.isFrameNew();  
	  
	  
	if(bNewFrame) {  
		  
		sourceMat = toCv(vidPlayer);  
		  
        // Update the probabilities on the new frame  
		computeParticuleProbabilities(conDens,refPtA, refPtB);  
        cvConDensUpdateByTime(conDens);  
  
        cout << "------------------------------" << std::endl;  
		  
	}  
}  
  
void testApp::draw() {  
	ofSetColor(255);  
	ofSetLineWidth(2);  
  
	drawMat(sourceMat,0,0);  
	  
	drawMat(dstMat, 0,240);  
}  
  
void testApp::mousePressed(int x, int y, int button) {  
	  
}  
  
void testApp::keyPressed(int key) {  
	if(key == 'i'){  
		cvReleaseConDensation(&conDens);  
	  
		refPtA[0] = 160.0;  
		refPtA[1] = 120.0;  
		refPtB[0] = 100.0;  
		refPtB[1] = 120.0;  
		  
		vidPlayer.setFrame(0);  
		  
		initCondens();  
		  
	}  
		  
}  

i noticed some times the statistical model does not get properly initialized, which results in condDens->flSamples[p][0] or [1] getting strange values.
but re-initialization the system solves that. cvReleaseConDensation(&conDens);