Thursday, 22 August 2013

OpenCV Kalman Filter stack overflow

OpenCV Kalman Filter stack overflow

I'm trying to implement a kalman filter for 3D tracking in OpenCV 2.2. The
state variables are the coordinates x,y,z followed by the velocities Vx,Vy
and Vz and I can only measure x,y and z.
I used an example from the book Learning OpenCV from O'reilly to get
started, but when I tried to adapt the example to my problem things got a
little confusing.
This is my implementation (I've tried to reduce the code to just the
relevant parts, and I've commented a lot to hopefully ease the reading).
CvKalman* kalman = cvCreateKalman( 6, 3, 0 );
// Setting the initial state estimates to [0,0,0,0,0,0].
CvMat* x_k = cvCreateMat( 6, 1, CV_32FC1 );
cvZero(x_k);
// Setting the a posteriori estimate to zero.
cvZero(kalman->state_post);
// Creating the process noise vector.
CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );
// Creating the measurement vector.
CvMat* z_k = cvCreateMat( 6, 1, CV_32FC1 );
cvZero( z_k );
// Initializing the state transition matrix.
float F_kalman[] = { 1,0,0,0.05,0,0, 0,1,0,0,0.05,0, 0,0,1,0,0,0.05,
0,0,0,1,0,0, 0,0,0,0,0,1 };
memcpy( kalman->transition_matrix->data.fl, F_kalman, sizeof(F_kalman));
// Initializing the other necessary parameters for the filter.
cvSetIdentity( kalman->measurement_matrix);
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-2) );
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));
// Updates the measurement vector with my sensor values, wich were in
the variable xyz (an array of CvScalar).
cvSetReal1D(z_k,0,xyz[i].val[0]);
cvSetReal1D(z_k,1,xyz[i].val[1]);
cvSetReal1D(z_k,2,xyz[i].val[2]);
cvKalmanPredict(kalman,0);
cvKalmanCorrect(kalman,z_k);
The problem is, when I run the code I get a "Unhandled exception at
0x55a3e757 in test.exe: 0xC00000FD: Stack overflow." at the
cvKalmanCorrect line.
Perhaps I've initialized one of the matrices to the wrong expected size,
but I'm really lost on how to check this.
Any thoughts?

No comments:

Post a Comment