Implemented a mean filter for Environment

modules/catkin_ws/src/car/include/environment/StreamMedianFilter.h

+#ifndef STREAMMEDIANFILTER_H
+#define STREAMMEDIANFILTER_H
+
+#include <vector>
+
+namespace car
+{
+class StreamMedianFilter
+{
+public:
+    StreamMedianFilter(const int windowSize);
+
+    ~StreamMedianFilter();
+
+    int moveWindow(int nextValue);
+
+private:
+    std::vector<int> currentWindow;
+
+    std::vector<int> sortedIndexList;
+
+    int currentIndex;
+};
+    
+}
+#endif

modules/catkin_ws/src/car/src/environment/StreamMedianFilter.cpp

+#include "StreamMedianFilter.h"
+#include <iostream>
+#include <numeric>
+
+namespace car
+{
+StreamMedianFilter::StreamMedianFilter(const int windowSize)
+    : currentWindow()
+    , sortedIndexList(windowSize, 0)
+    , currentIndex(0)
+{
+    currentWindow.reserve(windowSize);
+    std::iota(sortedIndexList.begin(), sortedIndexList.end(), 0);
+}
+
+StreamMedianFilter::~StreamMedianFilter() {}
+
+int StreamMedianFilter::moveWindow(int nextValue)
+{
+    if (currentWindow.size() < currentWindow.capacity()) currentWindow.push_back(nextValue);
+    currentWindow[currentIndex] = nextValue;
+
+    bool foundOldIndex = false;
+    for (int i = 0; i < currentWindow.size(); i++)
+    {
+        if (sortedIndexList[i] == currentIndex) {foundOldIndex = true; continue;}
+        if (foundOldIndex)
+        {
+            if (nextValue < currentWindow[sortedIndexList[i]])
+            {
+                sortedIndexList[i-1] = currentIndex;
+                break;
+            } else
+            {
+                sortedIndexList[i-1] = sortedIndexList[i];
+                if (i+1 == currentWindow.size()) {sortedIndexList[i] = currentIndex; break;}
+            }
+        } else
+        {
+            if (nextValue < currentWindow[sortedIndexList[i]]) 
+            {
+                for (int j = currentWindow.size()-1; j > i; j--)
+                {
+                    if (sortedIndexList[j] == currentIndex) {foundOldIndex = true;}
+                    if (foundOldIndex)
+                    {
+                        sortedIndexList[j] = sortedIndexList[j-1];
+                    } 
+                }
+                sortedIndexList[i] = currentIndex;
+                break;
+            } 
+        }
+    }
+    currentIndex = (currentIndex+1)%currentWindow.capacity();
+
+    return currentWindow[sortedIndexList[currentWindow.size()/2]];
+}
+}
+
+int main()
+{
+    car::StreamMedianFilter filter(5);
+
+    std::cout << "> " << filter.moveWindow(1) << std::endl; //  1
+    std::cout << "> " << filter.moveWindow(-1) << std::endl;// -1  1
+    std::cout << "> " << filter.moveWindow(-5) << std::endl;// -5 -1  1
+    std::cout << "> " << filter.moveWindow(5) << std::endl; // -5 -1  1  5
+    std::cout << "> " << filter.moveWindow(7) << std::endl; // -5 -1  1  5  7
+    std::cout << "> " << filter.moveWindow(9) << std::endl; // -5 -1  5  7  9
+    std::cout << "> " << filter.moveWindow(4) << std::endl; // -5  4  5  7  9
+    std::cout << "> " << filter.moveWindow(-4) << std::endl;// -4  4  5  7  9
+    std::cout << "> " << filter.moveWindow(0) << std::endl; // -4  0  4  7  9
+    std::cout << "> " << filter.moveWindow(-2) << std::endl;// -4 -2  0  4  9
+    std::cout << "> " << filter.moveWindow(3) << std::endl; // -4 -2  0  3  4
+
+}