diff --git a/source/RobotAPI/interface/core/FramedPoseBase.ice b/source/RobotAPI/interface/core/FramedPoseBase.ice
index 59d50d4c4905ad10da3b0160579a556ee21043a7..8766322dcdf4fdff9d3e6b263e29642c030b2b50 100644
--- a/source/RobotAPI/interface/core/FramedPoseBase.ice
+++ b/source/RobotAPI/interface/core/FramedPoseBase.ice
@@ -74,7 +74,7 @@ module armarx
* @param frame Name of frame.
* @param frame Name of agent.
*/
- ["cpp:virtual"]
+ ["cpp:virtual"]
class FramedOrientationBase extends QuaternionBase
{
string frame;
@@ -100,6 +100,16 @@ module armarx
};
+ /**
+ * PoseMedianOffsetFilterBase filters poses with median filter and subsequent offset filter.
+ */
+ ["cpp:virtual"]
+ class PoseMedianOffsetFilterBase extends PoseMedianFilterBase
+ {
+
+ };
+
+
};
#endif
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 18f0c9ed102a2393d6a5d5f82aed057b39c5eb08..27d230c5ce79767bddb3c7a52771939ed3cc0f81 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -41,6 +41,7 @@ set(LIB_HEADERS
LinkedPose.h
RobotStatechartContext.h
observerfilters/PoseMedianFilter.h
+ observerfilters/PoseMedianOffsetFilter.h
observerfilters/OffsetFilter.h
observerfilters/MatrixFilters.h
checks/ConditionCheckEqualsPose.h
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
index c37a9328d1a9520e9b7d1fc1346b79b619902c86..cfcbe2d9e761e3d0816a99e66fb6a449ef172f76 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
@@ -32,6 +32,7 @@
#include <RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
+#include <RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h>
namespace armarx
{
@@ -169,6 +170,7 @@ namespace armarx
map.insert(std::make_pair(armarx::LinkedPoseBase::ice_staticId(), new LinkedPoseObjectFactory));
add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
+ add<armarx::PoseMedianOffsetFilterBase, armarx::filters::PoseMedianOffsetFilter>(map);
add<armarx::OffsetFilterBase, armarx::filters::OffsetFilter>(map);
add<armarx::MatrixMaxFilterBase, armarx::filters::MatrixMaxFilter>(map);
add<armarx::MatrixMinFilterBase, armarx::filters::MatrixMinFilter>(map);
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 64e558129a5b284b3a8cd157b7cd5ac953e936ae..74d860defc9f43cdfb9ea21091b56439c6284dba 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -1,5 +1,5 @@
-#ifndef _ARMARX_ROBOTAPI_MEDIANFILTER_H
-#define _ARMARX_ROBOTAPI_MEDIANFILTER_H
+#ifndef _ARMARX_ROBOTAPI_POSEMEDIANFILTER_H
+#define _ARMARX_ROBOTAPI_POSEMEDIANFILTER_H
#include <ArmarXCore/observers/filters/MedianFilter.h>
#include <RobotAPI/libraries/core/FramedPose.h>
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..749c017dab75f623ae199d33fbf132fc382c1e8a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
@@ -0,0 +1,146 @@
+#ifndef _ARMARX_ROBOTAPI_PoseMedianOffsetFilter_H
+#define _ARMARX_ROBOTAPI_PoseMedianOffsetFilter_H
+
+#include <RobotAPI/libraries/core/EigenStl.h>
+#include <ArmarXCore/observers/filters/MedianFilter.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+
+namespace armarx
+{
+ namespace filters
+ {
+
+ /**
+ * @class PoseMedianOffsetFilter
+ * @ingroup ObserverFilters
+ * @brief The MedianFilter class provides an implementation
+ * for a median for datafields of type float, int and double.
+ */
+ class PoseMedianOffsetFilter :
+ public ::armarx::PoseMedianOffsetFilterBase,
+ public MedianFilter
+ {
+ public:
+ PoseMedianOffsetFilter(int windowSize = 11)
+ {
+ this->windowFilterSize = windowSize;
+ this->dataIndex = -windowSize;
+ }
+
+ // 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;
+ }
+
+ }
+
+ /**
+ * @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;
+ }
+
+ private:
+ Eigen::Vector3f offset;
+ Eigen::Vector3f currentValue;
+ 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;
+ }
+
+ 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();
+ }
+ }
+
+ };
+
+ } // namespace Filters
+}
+
+#endif