From 1c6b2b78f86cc415529d3ff6e71180b6b89a1e33 Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Tue, 24 Apr 2018 21:52:19 +0200
Subject: [PATCH] Implemented a mean filter for Environment
---
.../include/environment/StreamMedianFilter.h | 26 +++++++
.../src/environment/StreamMedianFilter.cpp | 77 +++++++++++++++++++
2 files changed, 103 insertions(+)
create mode 100644 modules/catkin_ws/src/car/include/environment/StreamMedianFilter.h
create mode 100644 modules/catkin_ws/src/car/src/environment/StreamMedianFilter.cpp
diff --git a/modules/catkin_ws/src/car/include/environment/StreamMedianFilter.h b/modules/catkin_ws/src/car/include/environment/StreamMedianFilter.h
new file mode 100644
index 00000000..f32c547d
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/environment/StreamMedianFilter.h
@@ -0,0 +1,26 @@
+#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
diff --git a/modules/catkin_ws/src/car/src/environment/StreamMedianFilter.cpp b/modules/catkin_ws/src/car/src/environment/StreamMedianFilter.cpp
new file mode 100644
index 00000000..9657d4e1
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/environment/StreamMedianFilter.cpp
@@ -0,0 +1,77 @@
+#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
+
+}
--
GitLab