From 72cde35f79a6ac69fbbc6341e1ad596fe57db0a0 Mon Sep 17 00:00:00 2001 From: Donovan Baarda Date: Tue, 21 Oct 2014 12:39:39 +1100 Subject: Rename filter() args to "input" and "output". Rename other attrs and vars for consistency. --- ftnoir_filter_kalman/ftnoir_filter_kalman.h | 5 ++--- ftnoir_filter_kalman/kalman.cpp | 26 ++++++++++++-------------- 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h index c2947516..8ede4c7d 100755 --- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -24,12 +24,11 @@ class OPENTRACK_EXPORT FTNoIR_Filter : public IFilter public: FTNoIR_Filter(); void reset(); - void filter(const double *target_camera_position, - double *new_camera_position); + void filter(const double *input, double *output); double accel_variance; double noise_variance; 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..a03aad3c 100644 --- a/ftnoir_filter_kalman/kalman.cpp +++ b/ftnoir_filter_kalman/kalman.cpp @@ -62,13 +62,12 @@ void FTNoIR_Filter::reset() { 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,10 +76,9 @@ 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 a = dt * dt * accel_variance; // dt^2 * accel_variance. double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. @@ -93,20 +91,20 @@ void FTNoIR_Filter::filter(const double* target_camera_position, kalman.processNoiseCov.at(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(i) = target_camera_position[i]; - // Save prev_position for detecting new tracker input. - prev_position[i] = target_camera_position[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(i); + output[i] = next_output.at(i); } } -- cgit v1.2.3 From 9b3c936af725353b1da97cc32aacef623d0e9d07 Mon Sep 17 00:00:00 2001 From: Donovan Baarda Date: Tue, 21 Oct 2014 13:33:31 +1100 Subject: Set (accel|noise)_variance from (accel|noise)_stddev settings. Use better estimates for filter settings giving less filtering. Fix broken old reference to target_camera_position. --- ftnoir_filter_kalman/ftnoir_filter_kalman.h | 6 ++++-- ftnoir_filter_kalman/kalman.cpp | 9 ++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h index 8ede4c7d..f6224530 100755 --- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -25,8 +25,10 @@ public: FTNoIR_Filter(); void reset(); void filter(const double *input, double *output); - double accel_variance; - double noise_variance; + // 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 last_input[6]; QElapsedTimer timer; diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp index a03aad3c..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 // 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,6 +56,7 @@ 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++) { @@ -80,6 +78,7 @@ void FTNoIR_Filter::filter(const double* input, double *output) 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. @@ -96,7 +95,7 @@ void FTNoIR_Filter::filter(const double* input, double *output) if (new_input) { cv::Mat measurement(6, 1, CV_64F); for (int i = 0; i < 6; i++) { - measurement.at(i) = target_camera_position[i]; + measurement.at(i) = input[i]; // Save last_input for detecting new tracker input. last_input[i] = input[i]; } -- cgit v1.2.3