MatrixFilters.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::
* @author     Simon Ottenhaus ( simon.ottenhaus at kit dot edu)
* @date       2015
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <RobotAPI/interface/observers/ObserverFilters.h>
 
#include <ArmarXCore/observers/filters/DatafieldFilter.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <algorithm>
 
namespace armarx::filters
{
 
    class MatrixMaxFilter :
        public MatrixMaxFilterBase,
        public DatafieldFilter
    {
    public:
        MatrixMaxFilter()
        {
            this->windowFilterSize = 1;
        }
 
        VariantBasePtr calculate(const Ice::Current&) const override
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                return new Variant(new MatrixFloat(1, 1));
            }
 
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
            MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
            return new Variant(matrix->toEigen().maxCoeff());
        }
        ParameterTypeList getSupportedTypes(const Ice::Current&) const override
        {
            ParameterTypeList result;
            result.push_back(VariantType::MatrixFloat);
            return result;
        }
    };
 
    class MatrixMinFilter :
        public MatrixMinFilterBase,
        public DatafieldFilter
    {
    public:
        MatrixMinFilter()
        {
            this->windowFilterSize = 1;
        }
 
        VariantBasePtr calculate(const Ice::Current&) const override
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                return new Variant(new MatrixFloat(1, 1));
            }
 
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
            MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
            return new Variant(matrix->toEigen().minCoeff());
        }
        ParameterTypeList getSupportedTypes(const Ice::Current&) const override
        {
            ParameterTypeList result;
            result.push_back(VariantType::MatrixFloat);
            return result;
        }
    };
 
    class MatrixAvgFilter :
        public MatrixAvgFilterBase,
        public DatafieldFilter
    {
    public:
        MatrixAvgFilter()
        {
            this->windowFilterSize = 1;
        }
 
        VariantBasePtr calculate(const Ice::Current&) const override
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                return new Variant(new MatrixFloat(1, 1));
            }
 
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
            MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
            return new Variant(matrix->toEigen().mean());
        }
        ParameterTypeList getSupportedTypes(const Ice::Current&) const override
        {
            ParameterTypeList result;
            result.push_back(VariantType::MatrixFloat);
            return result;
        }
    };
 
    class MatrixPercentileFilter :
        public MatrixPercentileFilterBase,
        public DatafieldFilter
    {
    public:
        MatrixPercentileFilter()
        {
            this->windowFilterSize = 1;
        }
        MatrixPercentileFilter(float percentile)
        {
            this->percentile = percentile;
            this->windowFilterSize = 1;
        }
 
        VariantBasePtr calculate(const Ice::Current&) const override
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                return new Variant(new MatrixFloat(1, 1));
            }
 
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
            MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
            std::vector<float> vector = matrix->toVector();
            std::sort(vector.begin(), vector.end());
            return new Variant(GetPercentile(vector, percentile));
        }
        ParameterTypeList getSupportedTypes(const Ice::Current&) const override
        {
            ParameterTypeList result;
            result.push_back(VariantType::MatrixFloat);
            return result;
        }
 
        static float GetPercentile(const std::vector<float>& sortedData, float percentile)
        {
            if (sortedData.size() == 0)
            {
                throw LocalException("GetPercentile not possible for empty vector");
            }
 
            float indexf = (sortedData.size() - 1) * percentile;
            indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
            int index = (int)indexf;
            float f = indexf - index;
 
            if (index == (int)sortedData.size() - 1)
            {
                return sortedData.at(sortedData.size() - 1);
            }
 
            return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
        }
    };
 
    class MatrixPercentilesFilter :
        public MatrixPercentilesFilterBase,
        public DatafieldFilter
    {
    public:
        MatrixPercentilesFilter()
        {
            this->windowFilterSize = 1;
            this->percentiles = 10;
        }
        MatrixPercentilesFilter(int percentiles)
        {
            this->percentiles = percentiles;
            this->windowFilterSize = 1;
        }
 
        VariantBasePtr calculate(const Ice::Current&) const override
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                ARMARX_IMPORTANT_S << "no data";
                return new Variant(new MatrixFloat(1, 1));
            }
 
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
            MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
            std::vector<float> vector = matrix->toVector();
            std::sort(vector.begin(), vector.end());
            std::vector<float> result;
            result.push_back(vector.at(0));
 
            for (int i = 1; i < percentiles; i++)
            {
                result.push_back(MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
            }
 
            result.push_back(vector.at(vector.size() - 1));
            return new Variant(new MatrixFloat(1, result.size(), result));
        }
        ParameterTypeList getSupportedTypes(const Ice::Current&) const override
        {
            ParameterTypeList result;
            result.push_back(VariantType::MatrixFloat);
            return result;
        }
    };
 
    class MatrixCumulativeFrequencyFilter :
        public MatrixCumulativeFrequencyFilterBase,
        public DatafieldFilter
    {
    public:
        MatrixCumulativeFrequencyFilter()
        {
            this->windowFilterSize = 1;
        }
        MatrixCumulativeFrequencyFilter(float min, float max, int bins)
        {
            this->min = min;
            this->max = max;
            this->bins = bins;
            this->windowFilterSize = 1;
        }
        VariantBasePtr calculate(const Ice::Current&) const override
        {
            std::unique_lock lock(historyMutex);
 
            if (dataHistory.size() == 0)
            {
                return new Variant(new MatrixFloat(1, 1));
            }
 
            VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
            MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
            std::vector<float> vector = matrix->toVector();
            std::sort(vector.begin(), vector.end());
            std::vector<int> result = Calculate(vector, min, max, bins);
            std::vector<float> resultF;
 
            for (int v : result)
            {
                resultF.push_back(v);
            }
 
            return new Variant(new MatrixFloat(1, resultF.size(), resultF));
        }
        ParameterTypeList getSupportedTypes(const Ice::Current&) const override
        {
            ParameterTypeList result;
            result.push_back(VariantType::MatrixFloat);
            return result;
        }
        static std::vector<int> Calculate(const std::vector<float>& sortedData, float min, float max, int bins)
        {
            std::vector<int> result;
            float val = min;
            int nr = 0;
            int lastCount = 0;
 
            for (size_t i = 0; i < sortedData.size(); i++)
            {
                if (sortedData.at(i) > val && nr < bins)
                {
                    result.push_back(i);
                    nr++;
                    val = min + (max - min) * nr / bins;
                    lastCount = i;
                }
            }
 
            while ((int)result.size() < bins)
            {
                result.push_back(lastCount);
            }
 
            return result;
        }
 
    };
}