PoseMedianFilter.cpp

Go to the documentation of this file.

#include "PoseMedianFilter.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/PoseBase.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
 
namespace armarx::filters
{
 
    PoseMedianFilter::PoseMedianFilter(int windowSize)
    {
        this->windowFilterSize = windowSize;
    }
 
 
    VariantBasePtr PoseMedianFilter::calculate(const Ice::Current& c) const
    {
        std::unique_lock lock(historyMutex);
 
        if (dataHistory.size() == 0)
        {
            return nullptr;
        }
 
        VariantTypeId type = dataHistory.begin()->second->getType();
 
        if (type == VariantType::Vector3
            or type == VariantType::FramedDirection
            or 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;
            }
 
            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<Vector3>()->toEigen()[i]);
                }
 
                std::sort(values.begin(), values.end());
                vec[i] = values.at(values.size() / 2);
            }
 
            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 nullptr;
            }
        }
        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 MedianFilter::calculate(c);
    }
 
 
    ParameterTypeList PoseMedianFilter::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;
    }
 
}