diff options
| -rwxr-xr-x | ftnoir_filter_kalman/ftnoir_filter_kalman.h | 11 | ||||
| -rw-r--r-- | ftnoir_filter_kalman/kalman.cpp | 35 |
2 files changed, 22 insertions, 24 deletions
diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h index c2947516..f6224530 100755 --- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -24,12 +24,13 @@ class OPENTRACK_EXPORT FTNoIR_Filter : public IFilter public: FTNoIR_Filter(); void reset(); - void filter(const double *target_camera_position, - double *new_camera_position); - double accel_variance; - double noise_variance; + void filter(const double *input, double *output); + // Set accel_stddev assuming moving 0.0->100.0 in dt=0.2 is 3 stddevs: (100.0*4/dt^2)/3. + const double accel_stddev = (100.0*4/(0.2*0.2))/3.0; + // TODO(abo): make noise_stddev a UI setting 0.0->10.0 with 0.1 resolution. + const double noise_stddev = 1.0; cv::KalmanFilter kalman; - double prev_position[6]; + double last_input[6]; QElapsedTimer timer; }; diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp index ab352faa..c5a600af 100644 --- a/ftnoir_filter_kalman/kalman.cpp +++ b/ftnoir_filter_kalman/kalman.cpp @@ -16,10 +16,6 @@ FTNoIR_Filter::FTNoIR_Filter() { // the following was written by Donovan Baarda <abo@minkirri.apana.org.au> // https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b void FTNoIR_Filter::reset() { - // Set accel_variance for moving 0.0->1.0 in dt=0.1. - accel_variance = 400.0f; - // TODO(abo): make noise_variance a UI setting 0.0->1.0. - noise_variance = 0.1; // Setup kalman with state (x) is the 6 tracker outputs then // their 6 corresponding velocities, and the measurement (z) is // the 6 tracker outputs. @@ -42,6 +38,7 @@ void FTNoIR_Filter::reset() { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1); + double accel_variance = accel_stddev * accel_stddev; double a = dt * dt * accel_variance; // dt^2 * accel_variance. double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. double c = 0.5 * b * dt; // (dt^4)/4 * accel_variance. @@ -59,16 +56,16 @@ void FTNoIR_Filter::reset() { 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a); cv::setIdentity(kalman.measurementMatrix); + double noise_variance = noise_stddev * noise_stddev; cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance)); cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4)); for (int i = 0; i < 6; i++) { - prev_position[i] = 0; + last_input[i] = 0; } timer.invalidate(); } -void FTNoIR_Filter::filter(const double* target_camera_position, - double *new_camera_position) +void FTNoIR_Filter::filter(const double* input, double *output) { // Start the timer if it's not running. if (!timer.isValid()) @@ -77,11 +74,11 @@ void FTNoIR_Filter::filter(const double* target_camera_position, auto dt = timer.restart() / 1000.0f; // Note this is a terrible way to detect when there is a new // frame of tracker input, but it is the best we have. - bool new_target = false; - for (int i = 0; i < 6 && !new_target; i++) - new_target = (prev_position[i] != target_camera_position[i]); - + bool new_input = false; + for (int i = 0; i < 6 && !new_input; i++) + new_input = (input[i] != last_input[i]); // Update the transitionMatrix and processNoiseCov for dt. + double accel_variance = accel_stddev * accel_stddev; double a = dt * dt * accel_variance; // dt^2 * accel_variance. double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. double c = 0.5 * b * dt; // (dt^4)/4 * accel_variance. @@ -93,20 +90,20 @@ void FTNoIR_Filter::filter(const double* target_camera_position, kalman.processNoiseCov.at<double>(i+6,i) = b; } // Get the updated predicted position. - cv::Mat output = kalman.predict(); + cv::Mat next_output = kalman.predict(); // If we have new tracker input, get the corrected position. - if (new_target) { + if (new_input) { cv::Mat measurement(6, 1, CV_64F); for (int i = 0; i < 6; i++) { - measurement.at<double>(i) = target_camera_position[i]; - // Save prev_position for detecting new tracker input. - prev_position[i] = target_camera_position[i]; + measurement.at<double>(i) = input[i]; + // Save last_input for detecting new tracker input. + last_input[i] = input[i]; } - output = kalman.correct(measurement); + next_output = kalman.correct(measurement); } - // Set new_camera_position to the output. + // Set output to the next_output. for (int i = 0; i < 6; i++) { - new_camera_position[i] = output.at<double>(i); + output[i] = next_output.at<double>(i); } } |