RtMedianFilter.cpp

Go to the documentation of this file.

#include "RtMedianFilter.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
 
namespace armarx::control::rt_filters
{
    RtMedianFilter::RtMedianFilter(size_t windowSize)
    {
        dataHistory.set_capacity(windowSize);
        medianBuffer.resize(windowSize);
    }
 
    float
    RtMedianFilter::update(float value)
    {
        dataHistory.push_back(value);
        std::copy(dataHistory.begin(), dataHistory.end(), medianBuffer.begin());
        const auto n = dataHistory.size();
        std::sort(medianBuffer.begin(), medianBuffer.begin() + static_cast<long>(n));
        float result = n % 2 == 0 ? (medianBuffer.at(n / 2 - 1) + medianBuffer.at(n / 2)) / 2
                                  : medianBuffer.at(n / 2);
        return result;
    }
 
    std::size_t
    RtMedianFilter::getWindowSize() const
    {
        return medianBuffer.size();
    }
} // namespace armarx::control::rt_filters