fixed initial value bug

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -27,6 +27,7 @@ set(LIB_FILES
     LinkedPose.cpp
     RobotStatechartContext.cpp
     checks/ConditionCheckMagnitudeChecks.cpp
+    observerfilters/PoseMedianOffsetFilter.cpp
     RobotAPIObjectFactories.cpp
     remoterobot/RobotStateObserver.cpp
     remoterobot/RemoteRobot.cpp

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

+#include "PoseMedianOffsetFilter.h"
+
+using namespace armarx;
+using namespace filters;
+
+armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize)
+    : MedianFilter(windowSize)
+{
+    this->windowFilterSize = windowSize;
+    this->dataIndex = -windowSize;
+    this->offset = Eigen::Vector3f::Zero();
+    this->currentValue = Eigen::Vector3f::Zero();
+}
+
+armarx::VariantBasePtr armarx::filters::PoseMedianOffsetFilter::calculate(const Ice::Current& c) const
+{
+    if (dataHistory.size() == 0)
+    {
+        return NULL;
+    }
+
+    VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+    VariantTypeId type = var->getType();
+
+    if (type == VariantType::Vector3)
+    {
+        Vector3Ptr vecVar = new Vector3(currentValue);
+        return new Variant(vecVar);
+    }
+    else if (type == VariantType::FramedDirection)
+    {
+        FramedDirectionPtr p = var->get<FramedDirection>();
+        FramedDirectionPtr vecVar = new FramedDirection(currentValue, p->frame, p->agent);
+        return new Variant(vecVar);
+    }
+    else if (type == VariantType::FramedPosition)
+    {
+        FramedPositionPtr p = var->get<FramedPosition>();
+        FramedPositionPtr vecVar = new FramedPosition(currentValue, p->frame, p->agent);
+        return new Variant(vecVar);
+    }
+    else
+    {
+        ARMARX_WARNING_S << "Unsupported Variant Type: " << var->getTypeName();
+        return NULL;
+    }
+
+}
+
+armarx::ParameterTypeList armarx::filters::PoseMedianOffsetFilter::getSupportedTypes(const Ice::Current& c) const
+{
+    ParameterTypeList result = MedianFilter::getSupportedTypes(c);
+    result.push_back(VariantType::Vector3);
+    result.push_back(VariantType::FramedDirection);
+    result.push_back(VariantType::FramedPosition);
+    return result;
+}
+
+float armarx::filters::PoseMedianOffsetFilter::median(std::vector<float>& values)
+{
+    std::sort(values.begin(), values.end());
+    return values.size() % 2 == 0 ? (values.at(values.size() / 2 - 1) + values.at(values.size() / 2)) / 2 : values.at(values.size() / 2);
+}
+
+Eigen::Vector3f armarx::filters::PoseMedianOffsetFilter::calculateMedian()
+{
+    Eigen::Vector3f result;
+    for (int i = 0; i < 3; ++i)
+    {
+        std::vector<float> values;
+        values.reserve(data.size());
+
+        for (const Eigen::Vector3f& v : data)
+        {
+            values.push_back(v(i));
+        }
+        result(i) = median(values);
+    }
+    return result;
+}
+
+void armarx::filters::PoseMedianOffsetFilter::update(Ice::Long timestamp, const armarx::VariantBasePtr& value, const Ice::Current& c)
+{
+    VariantTypeId type = value->getType();
+    if (type == VariantType::Vector3 || type == VariantType::FramedDirection || type == VariantType::FramedPosition)
+    {
+        Eigen::Vector3f currentValue = VariantPtr::dynamicCast(value)->get<Vector3>()->toEigen();
+        if (dataIndex < 0)
+        {
+            data.push_back(currentValue);
+            this->currentValue = Eigen::Vector3f::Zero();
+            dataIndex++;
+            if (dataIndex == 0)
+            {
+                offset = calculateMedian();
+            }
+        }
+        else
+        {
+            data.at(dataIndex) = currentValue;
+            dataIndex = (dataIndex + 1) % windowFilterSize;
+            this->currentValue = calculateMedian() - offset;
+        }
+    }
+    else
+    {
+        ARMARX_WARNING_S << "Unsupported Variant Type: " << value->getTypeName();
+    }
+    DatafieldFilter::update(timestamp, value, c);
+}

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

@@ -22,61 +22,17 @@ namespace armarx
             public MedianFilter
         {
         public:
-            PoseMedianOffsetFilter(int windowSize = 11)
-            {
-                this->windowFilterSize = windowSize;
-                this->dataIndex = -windowSize;
-            }
+            PoseMedianOffsetFilter(int windowSize = 11);
 
             // DatafieldFilterBase interface
         public:
-            VariantBasePtr calculate(const Ice::Current& c) const
-            {
-                if (dataHistory.size() == 0)
-                {
-                    return NULL;
-                }
-
-                VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
-                VariantTypeId type = var->getType();
-
-                if (type == VariantType::Vector3)
-                {
-                    Vector3Ptr vecVar = new Vector3(currentValue);
-                    return new Variant(vecVar);
-                }
-                else if (type == VariantType::FramedDirection)
-                {
-                    FramedDirectionPtr p = var->get<FramedDirection>();
-                    FramedDirectionPtr vecVar = new FramedDirection(currentValue, p->frame, p->agent);
-                    return new Variant(vecVar);
-                }
-                else if (type == VariantType::FramedPosition)
-                {
-                    FramedPositionPtr p = var->get<FramedPosition>();
-                    FramedPositionPtr vecVar = new FramedPosition(currentValue, p->frame, p->agent);
-                    return new Variant(vecVar);
-                }
-                else
-                {
-                    ARMARX_WARNING_S << "Unsupported Variane Type: " << var->getTypeName();
-                    return NULL;
-                }
-
-            }
+            VariantBasePtr calculate(const Ice::Current& c) const;
 
             /**
              * @brief This filter supports: Vector3, FramedDirection, FramedPosition
              * @return List of VariantTypes
              */
-            ParameterTypeList getSupportedTypes(const Ice::Current& c) const
-            {
-                ParameterTypeList result = MedianFilter::getSupportedTypes(c);
-                result.push_back(VariantType::Vector3);
-                result.push_back(VariantType::FramedDirection);
-                result.push_back(VariantType::FramedPosition);
-                return result;
-            }
+            ParameterTypeList getSupportedTypes(const Ice::Current& c) const;
 
         private:
             Eigen::Vector3f offset;
@@ -84,59 +40,11 @@ namespace armarx
             std::vector<Eigen::Vector3f> data;
             int dataIndex;
 
-            float median(std::vector<float>& values)
-            {
-                std::sort(values.begin(), values.end());
-                return values.size() % 2 == 0 ? (values.at(values.size() / 2 - 1) + values.at(values.size() / 2)) / 2 : values.at(values.size() / 2);
-            }
-
-            Eigen::Vector3f calculateMedian()
-            {
-                Eigen::Vector3f result;
-                for (int i = 0; i < 3; ++i)
-                {
-                    std::vector<float> values;
-                    values.reserve(data.size());
-
-                    for (const Eigen::Vector3f& v : data)
-                    {
-                        values.push_back(v(i));
-                    }
-                    result(i) = median(values);
-                }
-                return result;
-            }
+            float median(std::vector<float>& values);
+            Eigen::Vector3f calculateMedian();
 
         public:
-            void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c)
-            {
-                DatafieldFilter::update(timestamp, value, c);
-                VariantTypeId type = value->getType();
-                if (type == VariantType::Vector3 || type == VariantType::FramedDirection || type == VariantType::FramedPosition)
-                {
-                    Eigen::Vector3f currentValue = VariantPtr::dynamicCast(value)->get<Vector3>()->toEigen();
-                    if (dataIndex < 0)
-                    {
-                        data.push_back(currentValue);
-                        this->currentValue == Eigen::Vector3f::Zero();
-                        dataIndex++;
-                        if (dataIndex == 0)
-                        {
-                            offset = calculateMedian();
-                        }
-                    }
-                    else
-                    {
-                        data.at(dataIndex) = currentValue;
-                        dataIndex = (dataIndex + 1) % windowFilterSize;
-                        this->currentValue = calculateMedian() - offset;
-                    }
-                }
-                else
-                {
-                    ARMARX_WARNING_S << "Unsupported Variane Type: " << value->getTypeName();
-                }
-            }
+            void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c);
 
         };