added driver side median filter for sensor values

source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp

@@ -115,6 +115,9 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
 
     IceUtil::Time lastFrameTime = IceUtil::Time::now();
 
+    math::SlidingWindowVectorMedian slidingMedian(mi.res_x * mi.res_y, 21); // inter sample dely ~= 3,7ms, 11 samples ~== 40ms delay
+
+
     while(!sensorTask->isStopped())
     {
         //ARMARX_INFO << deactivateSpam(1) << this << ": receicePeriodicFrame";
@@ -126,6 +129,25 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
             //long end = TimestampVariant::nowLong();
             //cout << end - start << endl;
 
+            std::vector<float> sensorValues;
+            for(int i = 0; i < mi.res_x * mi.res_y; i++)
+            {
+                sensorValues.push_back(data.data->at(i));
+            }
+            slidingMedian.addEntry(sensorValues);
+
+            MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
+            std::vector<float> filteredSensorValues = slidingMedian.getMedian();
+            for (int y = 0; y < mi.res_y; y++)
+            {
+                for (int x = 0; x < mi.res_x; x++)
+                {
+                    (*matrix)(y, x) = filteredSensorValues.at(y * mi.res_x + x);
+                }
+            }
+
+
+            /*
             MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
             for (int y = 0; y < mi.res_y; y++)
             {
@@ -135,6 +157,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
                     (*matrix)(y, x) = val;
                 }
             }
+            */
 
             IceUtil::Time now = IceUtil::Time::now();
 

source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h

@@ -10,6 +10,7 @@
 //#include <Core/util/variants/eigen3/Eigen3LibRegistry.h>
 #include "TextWriter.h"
 #include <Core/observers/variant/TimestampVariant.h>
+#include <Core/util/math/SlidingWindowVectorMedian.h>
 #include <Core/util/variants/eigen3/MatrixVariant.h>
 #include <boost/thread/mutex.hpp>