Example of how to use the OpenCV Particle Filter (opencv Ver 2.4.9)

This is example of OpenCV particle filter

#include < iostream>
#include < vector>

#include < opencv2/opencv.hpp>
#include < opencv2/legacy/legacy.hpp>

#ifdef _DEBUG           
#pragma comment(lib, "opencv_core249d.lib")   
#pragma comment(lib, "opencv_imgproc249d.lib")   //MAT processing   
#pragma comment(lib, "opencv_objdetect249d.lib") //HOGDescriptor   
//#pragma comment(lib, "opencv_gpu249d.lib")   
//#pragma comment(lib, "opencv_features2d249d.lib")   
#pragma comment(lib, "opencv_highgui249d.lib")   
#pragma comment(lib, "opencv_ml249d.lib")   
//#pragma comment(lib, "opencv_stitching249d.lib");   
//#pragma comment(lib, "opencv_nonfree249d.lib");   
#pragma comment(lib, "opencv_video249d.lib")
#pragma comment(lib, "opencv_legacy249d.lib")
#else   
#pragma comment(lib, "opencv_core249.lib")   
#pragma comment(lib, "opencv_imgproc249.lib")   
#pragma comment(lib, "opencv_objdetect249.lib")   
//#pragma comment(lib, "opencv_gpu249.lib")   
//#pragma comment(lib, "opencv_features2d249.lib")   
#pragma comment(lib, "opencv_highgui249.lib")   
#pragma comment(lib, "opencv_ml249.lib")   
//#pragma comment(lib, "opencv_stitching249.lib");   
//#pragma comment(lib, "opencv_nonfree249.lib");   
#pragma comment(lib, "opencv_video249d.lib")   
#endif   




using namespace std;


#define drawCross( center, color, d ) line( img, cv::Point( center.x - d, center.y - d ), cv::Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); line( img, cv::Point( center.x + d, center.y - d ), cv::Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )


struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

vector< cv::Point> mouseV, particleV;
int counter = -1;

// Define this to proceed one click at a time.
//#define CLICK 1
#define PLOT_PARTICLES 1

void on_mouse(int event, int x, int y, int flags, void* param) {
#ifdef CLICK
 if (event == CV_EVENT_LBUTTONUP) 
#endif
 {
  last_mouse = mouse_info;
  mouse_info.x = x;
  mouse_info.y = y;
  counter = 0;
 }
}

void printMat(CvMat* mat)
{
    
    for(int i=0; i< mat->rows; i++)
    {
        if(i==0)
        {
            for(int j=0; j< mat->cols; j++)  printf("%10d",j+1);
        }

        printf("\n%4d: ",i+1);
        for(int j=0; j< mat->cols; j++)
        {

            printf("%10.2f",cvGet2D(mat,i,j).val[0]);
        }
    }
    
}


int main () {
 cv::Mat img(650, 650, CV_8UC3);
 char code = (char)-1;

 cv::namedWindow("mouse particle");
 cv::setMouseCallback("mouse particle", on_mouse, 0);

 cv::Mat_< float> measurement(2,1); 
 measurement.setTo(cv::Scalar(0));

 int DP = 2;
 int MP = 2;
 int nParticles = 250;
 float xRange = 650.0;
 float yRange = 650.0;

 float minRange[] = { 0, 0, -xRange, -yRange };
 float maxRange[] = { xRange, yRange, xRange, yRange };
 CvMat LB, UB;
 cvInitMatHeader(&LB, DP, 1, CV_32FC1, minRange); 
 cvInitMatHeader(&UB, DP, 1, CV_32FC1, maxRange);

 CvConDensation* condens = cvCreateConDensation(DP, MP, nParticles);

 
 cvConDensInitSampleSet(condens, &LB, &UB); //Lower Bound, Upper Bound

 // The OpenCV documentation doesn't tell you to initialize this
 // matrix, but you have to do it.  For this 2D example, we're just
 // using a 2x2 identity matrix.  I'm sure there's a slicker way to
 // do this, left as an exercise for the reader.
 condens->DynamMatr[0] = 1.0;
 condens->DynamMatr[1] = 0.0;
 condens->DynamMatr[2] = 0.0;
 condens->DynamMatr[3] = 1.0;

 for (int i = 0; i < condens->SamplesNum; i++) {
  cv::Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
  drawCross(partPt , cv::Scalar(255,0,(int)(i * 255.0/(float)condens->SamplesNum)), 2);
 }



 for(;;) {

  if (mouse_info.x < 0 || mouse_info.y < 0) {
   imshow("mouse particle", img);
   cv::waitKey(30);
   continue;
  }

  mouseV.clear();
  particleV.clear();

  for(;;) {
   code = (char)cv::waitKey(100);

   if( code > 0 )
    break;

#ifdef CLICK
   if (counter++ > 0) {
    continue;
   } 
#endif

   measurement(0) = mouse_info.x;
   measurement(1) = mouse_info.y;

   cv::Point measPt(measurement(0),measurement(1));
   mouseV.push_back(measPt);

   // Clear screen
   img = cv::Scalar::all(100);

   for (int i = 0; i < condens->SamplesNum; i++) {

    float diffX =  (measurement(0) - condens->flSamples[i][0])/xRange;
    float diffY =  (measurement(1) - condens->flSamples[i][1])/yRange;

    condens->flConfidence[i] = exp(-100.0f * ((diffX * diffX + diffY * diffY)));

    // plot particles
#ifdef PLOT_PARTICLES
    cv::Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
    drawCross(partPt , 
     cv::Scalar(255,0,(int)(i * 255.0/(float)condens->SamplesNum)), 
     2);
#endif

   }

   cvConDensUpdateByTime(condens);

   cv::Point statePt(condens->State[0], condens->State[1]);
   particleV.push_back(statePt);

   for (int i = 0; i < mouseV.size() - 1; i++) {
    line(img, mouseV[i], mouseV[i+1], cv::Scalar(255,255,0), 1);
   }
   for (int i = 0; i < particleV.size() - 1; i++) {
    line(img, particleV[i], particleV[i+1], cv::Scalar(0,255,0), 1);
   }

   // plot points
   drawCross( statePt, cv::Scalar(255,0,0), 3 );
   drawCross( measPt, cv::Scalar(0,0,255), 3 );

   imshow( "mouse particle", img );
  }

  if( code == 27 || code == 'q' || code == 'Q' )
   break;
 }

 return 0;
}