This forum is disabled, please visit https://forum.opencv.org
 | 1 | initial version |
The cvConDensation code had some bugs in its resampling stage and did not work very well. (Neither did the example in the OpenCV book.) Here is an example that works. Note that the DynamMatr matrix is not automatically initialized, so you have to do that yourself.
// Example of how to use the OpenCV Particle Filter.
//
// Stolen largely from morethantechnical.com's nice mouse_kalman project.
//
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/legacy/legacy.hpp>
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;
}
}
int main (int argc, char * const argv[]) {
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 dim = 2;
int nParticles = 25;
float xRange = 650.0;
float yRange = 650.0;
float minRange[] = { 0, 0 };
float maxRange[] = { xRange, yRange };
CvMat LB, UB;
cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
CvConDensation* condens = cvCreateConDensation(dim, dim, nParticles);
cvConDensInitSampleSet(condens, &LB, &UB);
// The OpenCV documentation doesn't tell you to initialize this
// transition 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(;;) {
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] = 1.0 / (sqrt(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,255), 2);
#endif
}
cvConDensUpdateByTime(condens);
cv::Point statePt(condens->State[0], condens->State[1]);
particleV.push_back(statePt);
// plot points
drawCross( statePt, cv::Scalar(255,255,255), 5 );
drawCross( measPt, cv::Scalar(0,0,255), 5 );
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);
}
imshow( "mouse particle", img );
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}
The cvConDensation code was apparently moved to "legacy" status after 2.3, which is a shame because it's a powerful algorithm. But the code only needed some bugs fixed in the resampling stage. Below is a patch for condens.cpp that you can use. I don't know how to get this into subsequent versions of OpenCV, but will try to figure out how to submit it in bug report and see if anyone else can do that.
$ diff /usr/local/src/OpenCV-2.3.1/modules/legacy/src/condens.cpp condens.cpp
41a42,45
> #include <sys/time.h>
>
> #define max(i,j) (i > j)?i:j
> #define min(i,j) (i < j)?i:j
152a157,162
> // These are used in the resampling algorithm.
> struct CvRandState* randState;
> float randNumber;
>
> // Use this to improve the random number generation.
> struct timeval tv;
173,175c183,200
< icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP,
< ConDens->DP );
< Sum = Sum / ConDens->SamplesNum;
---
>
> // Note that the DynamMatr *must* be initialized by the calling function.
> // The cvCreateConDensation function does not initialize it.
> // Initializing it to an identity matrix would be a good choice.
> icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State,
> ConDens->DP, ConDens->DP );
>
> // Initialize the random number generator.
> gettimeofday(&tv, NULL);
> cvRandInit( randState, 0, Sum, tv.tv_usec);
>
> // We want a record of the span of the particle distribution. The resampled
> // distribution is dependent on this quantity.
> float sampleMax[ConDens->DP], sampleMin[ConDens->DP];
> for (int k = 0; k < ConDens->DP; k++)
> {
> sampleMax[k] = -1.0e35; sampleMin[k] = 1.0e35;
> }
177a203,208
> // The algorithm of the original code always picked the last
> // sample, so was not really a weighted random re-sample. It
> // wasn't really random, either.
>
> // This version resamples according to the weights calculated by
> // the calling program.
179c210,215
< {
---
> {
> // Choose a random number between 0 and the sum of the particles'
> // weights.
> cvbRand(randState, &randNumber, 1);
>
> // Use that random number to choose one of the particles.
181c217,218
< while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1))
---
> while( (ConDens->flCumulative[j] <= randNumber) &&
> (j<ConDens->SamplesNum-1))
185c222,254
< icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] );
---
>
> // Copy the chosen particle.
> icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP,
> ConDens->flNewSamples[i] );
>
> // Keep track of the max and min of the sample particles.
> // We'll use that to calculate the size of the distribution.
> for (int k = 0; k < ConDens->DP; k++)
> {
> sampleMax[k] = max(sampleMax[k], *(ConDens->flNewSamples[i] + k));
> sampleMin[k] = min(sampleMin[k], *(ConDens->flNewSamples[i] + k));
> }
>
> }
>
> /* Reinitializes the structures to update samples randomly */
> for(int k = 0; k < ConDens->DP; k++ )
> {
>
> // The 0.5 in the next line should probably be an adjustable parameter,
> // part of the ConDens structure.
> float diff = 0.5 * (sampleMax[k] - sampleMin[k]);
> diff = max(diff, 0.02 * ConDens->flNewSamples[0][k]);
>
> // Re-seed the random number generation, and set the limits
> // relative to the geometric extent of the distribution.
> gettimeofday(&tv, NULL);
> cvRandInit( &(ConDens->RandS[k]),
> -diff, diff,
> tv.tv_usec + k);
> // We ask for a random number to spin the electronic roulette
> // wheel once or twice.
> cvbRand( ConDens->RandS + k, ConDens->RandomSample + k, 1);
187a257
>
199c269
< ConDens->DP );
---
> ConDens->DP );