MedianFilter.cpp

Go to the documentation of this file.

#include "robodyn_utilities/MedianFilter.h"

MedianFilter::MedianFilter():
    size(1)
{
}

MedianFilter::~MedianFilter()
{
}

void MedianFilter::setSize(unsigned int size)
{
    if (size > 0)
    {
        this->size = size;
    }
    else
    {
        this->size = 1;
    }
}

void MedianFilter::addValue(double newValue)
{
    while (values.size() >= size)
    {
        values.pop_front();
    }

    values.push_back(newValue);
}

double MedianFilter::getMedian()
{
    std::deque<double> temp = values;

    std::sort(temp.begin(), temp.end());

    if (temp.size() % 2 == 0)
    {
        median = (temp[temp.size() / 2 - 1] + temp[temp.size() / 2]) / 2;
    }
    else
    {
        median = temp[temp.size() / 2];
    }

    return median;
}

double MedianFilter::getMedian(double newValue)
{
    addValue(newValue);

    return getMedian();
}