added pose average filter

source/RobotAPI/interface/core/FramedPoseBase.ice

@@ -116,5 +116,14 @@ module armarx
     };
 
 
+    /**
+    * PoseAverageFilterBase filters poses with an average filter.
+    */
+    ["cpp:virtual"]
+    class PoseAverageFilterBase extends AverageFilterBase
+    {
+
+    };
+
 };
 

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -31,6 +31,7 @@ set(LIB_FILES
     FramedOrientedPoint.cpp
     RobotStatechartContext.cpp
     checks/ConditionCheckMagnitudeChecks.cpp
+    observerfilters/PoseAverageFilter.cpp
     observerfilters/PoseMedianOffsetFilter.cpp
     observerfilters/MedianDerivativeFilterV3.cpp
     RobotAPIObjectFactories.cpp
@@ -59,6 +60,7 @@ set(LIB_HEADERS
     FramedOrientedPoint.h
     RobotStatechartContext.h
     observerfilters/PoseMedianFilter.h
+    observerfilters/PoseAverageFilter.h
     observerfilters/PoseMedianOffsetFilter.h
     observerfilters/OffsetFilter.h
     observerfilters/MatrixFilters.h

source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp

@@ -32,6 +32,7 @@
 #include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
 #include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
 #include <RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h>
+#include <RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h>
 #include <RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h>
 #include <RobotAPI/libraries/core/OrientedPoint.h>
 #include <RobotAPI/libraries/core/FramedOrientedPoint.h>
@@ -59,6 +60,7 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
     add<armarx::FramedOrientedPointBase, armarx::FramedOrientedPoint>(map);
 
     add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
+    add<armarx::PoseAverageFilterBase, armarx::filters::PoseAverageFilter>(map);
     add<armarx::PoseMedianOffsetFilterBase, armarx::filters::PoseMedianOffsetFilter>(map);
     add<armarx::MedianDerivativeFilterV3Base, armarx::filters::MedianDerivativeFilterV3>(map);
     add<armarx::OffsetFilterBase, armarx::filters::OffsetFilter>(map);
@@ -69,5 +71,6 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
     add<armarx::MatrixPercentilesFilterBase, armarx::filters::MatrixPercentilesFilter>(map);
     add<armarx::MatrixCumulativeFrequencyFilterBase, armarx::filters::MatrixCumulativeFrequencyFilter>(map);
     add<armarx::TrajectoryBase, armarx::Trajectory>(map);
+
     return map;
 }

source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#include "PoseAverageFilter.h"
+
+namespace armarx
+{
+    namespace filters
+    {
+
+        VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
+        {
+            ScopedLock lock(historyMutex);
+
+            if (dataHistory.size() == 0)
+            {
+                return NULL;
+            }
+
+            VariantTypeId type = dataHistory.begin()->second->getType();
+
+            if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
+            {
+
+                Eigen::Vector3f vec;
+                vec.setZero();
+                std::string frame = "";
+                std::string agent = "";
+                VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+
+                if (type == VariantType::FramedDirection)
+                {
+                    FramedDirectionPtr p = var->get<FramedDirection>();
+                    frame = p->frame;
+                    agent = p->agent;
+                }
+                else if (type == VariantType::FramedPosition)
+                {
+                    FramedPositionPtr p = var->get<FramedPosition>();
+                    frame = p->frame;
+                    agent = p->agent;
+                }
+                Eigen::MatrixXf keypointPositions(dataHistory.size(), 3);
+                int i = 0;
+                for (auto& v : dataHistory)
+                {
+                    VariantPtr v2 = VariantPtr::dynamicCast(v.second);
+                    Eigen::Vector3f value = v2->get<Vector3>()->toEigen();
+                    keypointPositions(i, 0) = value(0);
+                    keypointPositions(i, 1) = value(1);
+                    keypointPositions(i, 2) = value(2);
+                    i++;
+                }
+                vec = keypointPositions.colwise().mean();
+
+
+                if (type == VariantType::Vector3)
+                {
+                    Vector3Ptr vecVar = new Vector3(vec);
+                    return new Variant(vecVar);
+                }
+                else if (type == VariantType::FramedDirection)
+                {
+
+                    FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
+                    return new Variant(vecVar);
+                }
+                else if (type == VariantType::FramedPosition)
+                {
+                    FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
+                    return new Variant(vecVar);
+                }
+                else
+                {
+                    ARMARX_WARNING_S << "Implementation missing here";
+                    return NULL;
+                }
+            }
+            else if (type == VariantType::Double)
+            {
+                //                    auto values = SortVariants<double>(dataHistory);
+                //                    return new Variant(values.at(values.size()/2));
+            }
+            else if (type == VariantType::Int)
+            {
+                //                    auto values = SortVariants<int>(dataHistory);
+                //                    return new Variant(values.at(values.size()/2));
+            }
+
+            return AverageFilter::calculate(c);
+        }
+
+
+
+    } // namespace filters
+} // namespace armarx

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

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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 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     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+
+namespace armarx
+{
+    namespace filters
+    {
+
+        class PoseAverageFilter :
+            public ::armarx::PoseAverageFilterBase,
+            public AverageFilter
+        {
+        public:
+            PoseAverageFilter(int windowSize = 11)
+            {
+                this->windowFilterSize = windowSize;
+            }
+
+            // DatafieldFilterBase interface
+        public:
+            VariantBasePtr calculate(const Ice::Current& c) const override;
+
+            /**
+             * @brief This filter supports: Vector3, FramedDirection, FramedPosition
+             * @return List of VariantTypes
+             */
+            ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
+            {
+                ParameterTypeList result = AverageFilter::getSupportedTypes(c);
+                result.push_back(VariantType::Vector3);
+                result.push_back(VariantType::FramedDirection);
+                result.push_back(VariantType::FramedPosition);
+                return result;
+            }
+        };
+
+    } // namespace
+} // namespace
+
+