MedianFilter.hpp

Go to the documentation of this file.

#pragma once
 
#include <AH/STL/algorithm> // std::partial_sort_copy
#include <AH/STL/array>     // std::array
#include <AH/STL/cstdint>   // uint8_t
 
 
#if 0
 
template <uint8_t N, class T = float>
class MedianFilter {
  public:
    MedianFilter() = default;
 
    MedianFilter(T initialValue) {
        std::fill(previousInputs.begin(), previousInputs.end(), initialValue);
    }
 
    T operator()(T x) {
        // Insert the new input into the ring buffer, overwriting the oldest
        // input.
        previousInputs[index] = x;
        if (++index == N)
            index = 0;
 
        // Calculate the median of the buffer by sorting the first half. A copy
        // should be made to keep the order of the buffer intact.
        const uint8_t halfSize = N / 2 + 1;
        std::array<T, halfSize> sorted;
        std::partial_sort_copy(previousInputs.begin(), previousInputs.end(),
                               sorted.begin(), sorted.end());
 
        // If the length of the ring buffer is odd, then the median is its
        // center element, which is the final element of `sorted`.
        // If the length of the ring buffer is even, then we need to take the
        // average of its two center elements, which are the final two elements
        // of `sorted`.
        if (N % 2 == 0)
            return (sorted.end()[-2] + sorted.end()[-1]) / 2;
        else
            return sorted.end()[-1];
    }
 
  private:
    uint8_t index = 0;
    std::array<T, N> previousInputs = {};
};
 
#else
 
template <uint8_t N, class T = float>
class MedianFilter {
  public:
    MedianFilter() = default;
 
    MedianFilter(T initialValue) {
        std::fill(previousInputs.begin(), previousInputs.end(), initialValue);
    }
 
    T operator()(T x) {
        // Insert the new input into the ring buffer, overwriting the oldest
        // input.
        previousInputs[index] = x;
        if (++index == N)
            index = 0;
 
        // Calculate the median of the buffer by sorting it just enough to find
        // the middle element. A copy should be made to keep the order of the
        // buffer intact.
        std::array<T, N> sorted = previousInputs;
        auto halfWay = sorted.begin() + N / 2;
        std::nth_element(sorted.begin(), halfWay, sorted.end());
 
        // If the length of the ring buffer is odd, then the median is its
        // center element.
        // If the length of the ring buffer is even, then we need to take the
        // average of its two center elements.
        if (N % 2 == 0) {
            auto beforeHalfWay = std::max_element(sorted.begin(), halfWay);
            return (*halfWay + *beforeHalfWay) / 2;
        } else {
            return *halfWay;
        }
    }
 
  private:
    uint8_t index = 0;
    std::array<T, N> previousInputs = {{}};
};
 
#endif