summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_filter_kalman
diff options
context:
space:
mode:
authorDonovan Baarda <abo@minkirri.apana.org.au>2014-10-21 12:39:39 +1100
committerDonovan Baarda <abo@minkirri.apana.org.au>2014-10-22 16:50:32 +1100
commit72cde35f79a6ac69fbbc6341e1ad596fe57db0a0 (patch)
tree4cd55f95e9678c5e305f77380843ba10365a3700 /ftnoir_filter_kalman
parentb0eec42b61e81bd031307618bb28fd1200e4bfa8 (diff)
Rename filter() args to "input" and "output".
Rename other attrs and vars for consistency.
Diffstat (limited to 'ftnoir_filter_kalman')
-rwxr-xr-xftnoir_filter_kalman/ftnoir_filter_kalman.h5
-rw-r--r--ftnoir_filter_kalman/kalman.cpp26
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<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];
+ // 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);
}
}