using the opencv particle filter ConDensation [closed]

edit

I had a fair amount of trouble getting the OpenCV ConDensation particle filter working, largely because the transition matrix (DynamMatr) is not initialized by cvCreateConDensation and the english documentation doesn't seem to tell you so. Unfortunately, the uninitialized matrix doesn't make it fail, it just make things work very badly, and I could not find a working example out there. (The http://opencv.jp/sample/estimators.html (Japanese documentation) has an example that uses it, though.)

Posting this code in an attempt to help the next sap like me who tries to implement a filter with the ConDensation API. The API is now relegated to a 'legacy' library, perhaps because it was hard to use before? It works pretty well, actually.

// 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 ...