From f52fe7f01b850da8d261c10e83afd87fe2efb672 Mon Sep 17 00:00:00 2001
From: David <david.schiebener@kit.edu>
Date: Tue, 10 Nov 2015 17:12:37 +0100
Subject: [PATCH] fixed initial value bug
---
source/RobotAPI/libraries/core/CMakeLists.txt | 1 +
.../PoseMedianOffsetFilter.cpp | 110 ++++++++++++++++++
.../observerfilters/PoseMedianOffsetFilter.h | 104 +----------------
3 files changed, 117 insertions(+), 98 deletions(-)
create mode 100644 source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 27d230c5c..dce297b52 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/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
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
new file mode 100644
index 000000000..7e516f717
--- /dev/null
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
@@ -0,0 +1,110 @@
+#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);
+}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
index 749c017da..dc90a1085 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
+++ b/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);
};
--
GitLab