Line data Source code
1 : #pragma once
2 :
3 : #include <AH/STL/algorithm> // std::partial_sort_copy
4 : #include <AH/STL/array> // std::array
5 : #include <AH/STL/cstdint> // uint8_t
6 :
7 : /// @addtogroup Filters
8 : /// @{
9 :
10 : #if 0
11 :
12 : /**
13 : * @brief Class for Median Filters.
14 : *
15 : * Use the parenthesis or call operator (`operator()`) with the next input of
16 : * the filter as an argument to update the Median filter.
17 : * This operator returns the next output of the filter.
18 : *
19 : * The output equation is:
20 : * @f$ y[n] = \text{median}\Big(x[n], x[n-1],\ \ldots,\ x[n-N+1]\Big) @f$
21 : *
22 : * This version uses std::partial_sort_copy, and is about 4 times slower than
23 : * the version using std::nth_element below. (Tested on an Arduino Leonardo,
24 : * tested with N = 100 and N = 101.)
25 : *
26 : * @tparam N
27 : * The number of previous values to take the median of.
28 : * @tparam T
29 : * The type of the input and output values of the filter.
30 : */
31 : template <uint8_t N, class T = float>
32 : class MedianFilter {
33 : public:
34 : /**
35 : * @brief Construct a new Median Filter (zero initialized).
36 : */
37 : MedianFilter() = default;
38 :
39 : /**
40 : * @brief Construct a new Median Filter, and initialize it with the given
41 : * value.
42 : *
43 : * @param initialValue
44 : * Determines the initial state of the filter:
45 : * @f$ x[-N] =\ \ldots\ = x[-2] = x[-1] = \text{initialValue} @f$
46 : */
47 : MedianFilter(T initialValue) {
48 : std::fill(previousInputs.begin(), previousInputs.end(), initialValue);
49 : }
50 :
51 : /**
52 : * @brief Calculate the output @f$ y[n] @f$ for a given input
53 : * @f$ x[n] @f$.
54 : *
55 : * @f$ y[n] = \text{median}\Big(x[n], x[n-1],\ \ldots,\ x[n-N+1]\Big) @f$
56 : *
57 : * @param x
58 : * The new input @f$ x[n] @f$.
59 : * @return The new output @f$ y[n] @f$.
60 : */
61 : T operator()(T x) {
62 : // Insert the new input into the ring buffer, overwriting the oldest
63 : // input.
64 : previousInputs[index] = x;
65 : if (++index == N)
66 : index = 0;
67 :
68 : // Calculate the median of the buffer by sorting the first half. A copy
69 : // should be made to keep the order of the buffer intact.
70 : const uint8_t halfSize = N / 2 + 1;
71 : std::array<T, halfSize> sorted;
72 : std::partial_sort_copy(previousInputs.begin(), previousInputs.end(),
73 : sorted.begin(), sorted.end());
74 :
75 : // If the length of the ring buffer is odd, then the median is its
76 : // center element, which is the final element of `sorted`.
77 : // If the length of the ring buffer is even, then we need to take the
78 : // average of its two center elements, which are the final two elements
79 : // of `sorted`.
80 : if (N % 2 == 0)
81 : return (sorted.end()[-2] + sorted.end()[-1]) / 2;
82 : else
83 : return sorted.end()[-1];
84 : }
85 :
86 : private:
87 : /// The last index in the ring buffer.
88 : uint8_t index = 0;
89 : /// A ring buffer to keep track of the N last inputs.
90 : std::array<T, N> previousInputs = {};
91 : };
92 :
93 : #else
94 :
95 : /**
96 : * @brief Class for Median Filters.
97 : *
98 : * Use the parenthesis or call operator (`operator()`) with the next input of
99 : * the filter as an argument to update the Median filter.
100 : * This operator returns the next output of the filter.
101 : *
102 : * The output equation is:
103 : * @f$ y[n] = \text{median}\Big(x[n], x[n-1],\ \ldots,\ x[n-N+1]\Big) @f$
104 : *
105 : * @tparam N
106 : * The number of previous values to take the median of.
107 : * @tparam T
108 : * The type of the input and output values of the filter.
109 : */
110 : template <uint8_t N, class T = float>
111 : class MedianFilter {
112 : public:
113 : /**
114 : * @brief Construct a new Median Filter (zero initialized).
115 : */
116 : MedianFilter() = default;
117 :
118 : /**
119 : * @brief Construct a new Median Filter, and initialize it with the given
120 : * value.
121 : *
122 : * @param initialValue
123 : * Determines the initial state of the filter:
124 : * @f$ x[-N] =\ \ldots\ = x[-2] = x[-1] = \text{initialValue} @f$
125 : */
126 2 : MedianFilter(T initialValue) {
127 2 : std::fill(previousInputs.begin(), previousInputs.end(), initialValue);
128 2 : }
129 :
130 : /**
131 : * @brief Calculate the output @f$ y[n] @f$ for a given input
132 : * @f$ x[n] @f$.
133 : *
134 : * @f$ y[n] = \text{median}\Big(x[n], x[n-1],\ \ldots,\ x[n-N+1]\Big) @f$
135 : *
136 : * @param x
137 : * The new input @f$ x[n] @f$.
138 : * @return The new output @f$ y[n] @f$.
139 : */
140 24 : T operator()(T x) {
141 : // Insert the new input into the ring buffer, overwriting the oldest
142 : // input.
143 24 : previousInputs[index] = x;
144 24 : if (++index == N)
145 4 : index = 0;
146 :
147 : // Calculate the median of the buffer by sorting it just enough to find
148 : // the middle element. A copy should be made to keep the order of the
149 : // buffer intact.
150 24 : std::array<T, N> sorted = previousInputs;
151 24 : auto halfWay = sorted.begin() + N / 2;
152 24 : std::nth_element(sorted.begin(), halfWay, sorted.end());
153 :
154 : // If the length of the ring buffer is odd, then the median is its
155 : // center element.
156 : // If the length of the ring buffer is even, then we need to take the
157 : // average of its two center elements.
158 : if (N % 2 == 0) {
159 12 : auto beforeHalfWay = std::max_element(sorted.begin(), halfWay);
160 12 : return (*halfWay + *beforeHalfWay) / 2;
161 : } else {
162 12 : return *halfWay;
163 : }
164 : }
165 :
166 : private:
167 : /// The last index in the ring buffer.
168 : uint8_t index = 0;
169 : /// A ring buffer to keep track of the N last inputs.
170 : std::array<T, N> previousInputs = {{}};
171 : };
172 :
173 : #endif
174 :
175 : /// @}
|
