diff options
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.cpp | 22 |
1 files changed, 3 insertions, 19 deletions
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 855cb413..80636f42 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -58,29 +58,13 @@ FTNoIR_Filter::FTNoIR_Filter() : first_run(true) } } -static inline bool nanp(double value) -{ - return std::isnan(value) || std::isinf(value); -} - -static inline double elide_nan(double value, double def) -{ - if (nanp(value)) - { - if (nanp(def)) - return 0; - return def; - } - return value; -} - void FTNoIR_Filter::filter(const double* input, double *output) { if (first_run) { for (int i = 0; i < 6; i++) { - const double f = nanp(input[i]) ? 0 : input[i]; + const double f = input[i]; output[i] = f; last_output[i] = f; smoothed_input[i] = f; @@ -105,7 +89,7 @@ void FTNoIR_Filter::filter(const double* input, double *output) { Map& m = i >= 3 ? rot : trans; - smoothed_input[i] = smoothed_input[i] * (1.-alpha) + elide_nan(input[i], smoothed_input[i]) * alpha; + smoothed_input[i] = smoothed_input[i] * (1.-alpha) + smoothed_input[i] * alpha; const double in = smoothed_input[i]; @@ -116,7 +100,7 @@ void FTNoIR_Filter::filter(const double* input, double *output) const double val = m.getValue(vec_ / thres); const double result = last_output[i] + (vec < 0 ? -1 : 1) * dt * val; - last_output[i] = output[i] = elide_nan(result, last_output[i]); + last_output[i] = output[i] = result; } } |