From ab81b3f97c80ea8cc37382ebf15472dacb13635e Mon Sep 17 00:00:00 2001
From: Sarah Puch <sarah.puch@domain.de>
Date: Tue, 27 Jan 2015 18:23:59 +0100
Subject: [PATCH] PoseMedianFilter now works for Vector3, FramedVector3, and
FramedPosition
---
.../core/observerfilters/PoseMedianFilter.h | 46 ++++++++++++++++---
1 file changed, 40 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 8bfd2fde7..2744da772 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -33,25 +33,59 @@ namespace armarx {
VariantTypeId type = dataHistory.begin()->second->getType();
- if(type == VariantType::FramedVector3)
+ if ((type == VariantType::Vector3) || (type == VariantType::FramedVector3) || (type == VariantType::FramedPosition))
{
Eigen::Vector3f vec;
vec.setZero();
+ std::string frame = "";
+ std::string agent = "";
+ VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+ if (type == VariantType::FramedVector3)
+ {
+ FramedVector3Ptr p = var->get<FramedVector3>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr p = var->get<FramedPosition>();
+ frame = p->frame;
+ agent = p->agent;
+ }
for (int i = 0; i < 3; ++i) {
std::vector<double> values;
for(auto v : dataHistory)
{
VariantPtr v2 = VariantPtr::dynamicCast(v.second);
- values.push_back(v2->get<FramedVector3>()->toEigen()[i]);
+ values.push_back(v2->get<Vector3>()->toEigen()[i]);
}
std::sort(values.begin(), values.end());
vec[i] = values.at(values.size()/2);
}
- Vector3Ptr vecVar = new Vector3(vec);
- return new Variant(vecVar);
+ if (type == VariantType::Vector3)
+ {
+ Vector3Ptr vecVar = new Vector3(vec);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedVector3)
+ {
+
+ FramedVector3Ptr vecVar = new FramedVector3(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)
{
@@ -73,9 +107,9 @@ namespace armarx {
ParameterTypeList getSupportedTypes(const Ice::Current &c) const
{
ParameterTypeList result = MedianFilter::getSupportedTypes(c);
-// result.push_back(VariantType::Vector3);
+ result.push_back(VariantType::Vector3);
result.push_back(VariantType::FramedVector3);
-// result.push_back(VariantType::FramedPosition);
+ result.push_back(VariantType::FramedPosition);
return result;
}
--
GitLab