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