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


Comments

Popular posts from this blog

(OpenCV Study) Background subtractor MOG, MOG2, GMG example source code (BackgroundSubtractorMOG, BackgroundSubtractorMOG2, BackgroundSubtractorGMG)

OpenCV Stitching example (Stitcher class, Panorama)

Example source code of extract HOG feature from images, save descriptor values to xml file, using opencv (using HOGDescriptor )

Real-time N camera stitching Class.

Optical Flow sample source code using OpenCV

OpenCV Drawing Example, (line, circle, rectangle, ellipse, polyline, fillConvexPoly, putText, drawContours)

Video Stabilization example source code, (using cvFindHomography, cvWarpPerspective functions in openCV)

SICK LMS511 sensor data acquisition interface (source code, C++/MFC)

8 point algorithm (Matlab source code) / The method to get the Fundamental Matrix and the Essential matrix

Image warping (using opencv findHomography, warpPerspective)