13 #include <wpi/Algorithm.h>
14 #include <wpi/circular_buffer.h>
31 explicit MedianFilter(
size_t size) : m_valueBuffer(size), m_size{size} {}
41 wpi::insert_sorted(m_orderedValues, next);
43 size_t curSize = m_orderedValues.size();
47 if (curSize > m_size) {
48 m_orderedValues.erase(std::find(m_orderedValues.begin(),
49 m_orderedValues.end(),
50 m_valueBuffer.pop_back()));
51 curSize = curSize - 1;
55 m_valueBuffer.push_front(next);
57 if (curSize % 2 == 1) {
59 return m_orderedValues[curSize / 2];
62 return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
71 m_orderedValues.clear();
72 m_valueBuffer.reset();
77 std::vector<T> m_orderedValues;