added matrix reduction filters

source/RobotAPI/interface/observers/ObserverFilters.ice

@@ -34,6 +34,25 @@ module armarx
     {
 
     };
+
+    ["cpp:virtual"]
+    class MatrixMaxFilterBase extends DatafieldFilterBase
+    {
+
+    };
+
+    ["cpp:virtual"]
+    class MatrixMinFilterBase extends DatafieldFilterBase
+    {
+
+    };
+
+    ["cpp:virtual"]
+    class MatrixAvgFilterBase extends DatafieldFilterBase
+    {
+
+    };
+
 };
 
 #endif

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -17,7 +17,7 @@ set(LIB_NAME       RobotAPICore)
 set(LIB_VERSION    0.1.0)
 set(LIB_SOVERSION  0)
 
-set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ${Simox_LIBRARIES})
+set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants ${Simox_LIBRARIES})
 
 
 set(LIB_FILES
@@ -41,6 +41,7 @@ set(LIB_HEADERS
     RobotStatechartContext.h
     observerfilters/PoseMedianFilter.h
     observerfilters/OffsetFilter.h
+    observerfilters/MatrixFilters.h
     checks/ConditionCheckEqualsPose.h
     checks/ConditionCheckEqualsPoseWithTolerance.h
     checks/ConditionCheckMagnitudeChecks.h

source/RobotAPI/libraries/core/RobotAPIObjectFactories.h

@@ -32,6 +32,7 @@
 #include <Ice/Ice.h>
 #include <RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h>
 #include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
 
 namespace armarx
 {
@@ -170,6 +171,9 @@ namespace armarx
 
                 add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
                 add<armarx::OffsetFilterBase, armarx::filters::OffsetFilter>(map);
+                add<armarx::MatrixMaxFilterBase, armarx::filters::MatrixMaxFilter>(map);
+                add<armarx::MatrixMinFilterBase, armarx::filters::MatrixMinFilter>(map);
+                add<armarx::MatrixAvgFilterBase, armarx::filters::MatrixAvgFilter>(map);
 
                 return map;
             }

source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h

+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    ArmarX::
+* @author     Simon Ottenhaus ( simon.ottenhaus at kit dot edu)
+* @date       2015
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
+#ifndef _ARMARX_ROBOTAPI_MATRIXFILTERS_H
+#define _ARMARX_ROBOTAPI_MATRIXFILTERS_H
+
+#include <Core/observers/filters/DatafieldFilter.h>
+#include <RobotAPI/interface/observers/ObserverFilters.h>
+#include <Core/util/variants/eigen3/MatrixVariant.h>
+
+
+namespace armarx
+{
+    namespace filters
+    {
+
+        class MatrixMaxFilter :
+                public MatrixMaxFilterBase,
+                public DatafieldFilter
+        {
+        public:
+            MatrixMaxFilter() { }
+
+            VariantBasePtr calculate(const Ice::Current &) const
+            {
+                VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+                MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+                return new Variant(matrix->toEigen().maxCoeff());
+            }
+            ParameterTypeList getSupportedTypes(const Ice::Current &) const
+            {
+                ParameterTypeList result;
+                result.push_back(VariantType::MatrixFloat);
+                return result;
+            }
+        };
+
+        class MatrixMinFilter :
+                public MatrixMinFilterBase,
+                public DatafieldFilter
+        {
+        public:
+            MatrixMinFilter() { }
+
+            VariantBasePtr calculate(const Ice::Current &) const
+            {
+                VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+                MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+                return new Variant(matrix->toEigen().minCoeff());
+            }
+            ParameterTypeList getSupportedTypes(const Ice::Current &) const
+            {
+                ParameterTypeList result;
+                result.push_back(VariantType::MatrixFloat);
+                return result;
+            }
+        };
+
+        class MatrixAvgFilter :
+                public MatrixAvgFilterBase,
+                public DatafieldFilter
+        {
+        public:
+            MatrixAvgFilter() { }
+
+            VariantBasePtr calculate(const Ice::Current &) const
+            {
+                VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+                MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+                return new Variant(matrix->toEigen().mean());
+            }
+            ParameterTypeList getSupportedTypes(const Ice::Current &) const
+            {
+                ParameterTypeList result;
+                result.push_back(VariantType::MatrixFloat);
+                return result;
+            }
+        };
+
+    }
+}
+
+#endif // _ARMARX_ROBOTAPI_MATRIXFILTERS_H

source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h

@@ -27,6 +27,7 @@
 #include <RobotAPI/interface/observers/ObserverFilters.h>
 #include <Core/observers/filters/DatafieldFilter.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
+#include <Core/util/variants/eigen3/MatrixVariant.h>
 
 namespace armarx {
     namespace filters {
@@ -56,7 +57,7 @@ namespace armarx {
             {
 
                 VariantPtr newVariant;
-                if( initialValue && dataHistory.size() > 0)
+                if(initialValue && dataHistory.size() > 0)
                 {
                     VariantTypeId type = dataHistory.begin()->second->getType();
                     VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
@@ -95,6 +96,13 @@ namespace armarx {
                         Eigen::Vector3f newValue =  pos->toEigen() - intialPos->toEigen();
                         newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
                     }
+                    else if(type == VariantType::MatrixFloat)
+                    {
+                        MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+                        MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
+                        Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
+                        newVariant = new Variant(new MatrixFloat(newMatrix));
+                    }
                 }
                 return newVariant;
 
@@ -107,6 +115,7 @@ namespace armarx {
                 result.push_back(VariantType::Double);
                 result.push_back(VariantType::FramedVector3);
                 result.push_back(VariantType::FramedPosition);
+                result.push_back(VariantType::MatrixFloat);
                 return result;
             }
         private:
@@ -130,4 +139,4 @@ namespace armarx {
     }
 }
 
-#endif // OFFSETFILTER_H
+#endif // _ARMARX_ROBOTAPI_OFFSETFILTER_H