SlidingWindowVectorMedian.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::Core
* @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
* @date       2015
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include "StatUtils.h"
 
#include <math.h>
#include <vector>
 
#include <ArmarXCore/core/exceptions/Exception.h>
 
namespace armarx::math
{
    class SlidingWindowVectorMedian;
    using SlidingWindowVectorMedianPtr = std::shared_ptr<SlidingWindowVectorMedian>;
 
    class SlidingWindowVectorMedian
    {
    private:
        size_t windowSize;
        size_t vectorSize;
        std::vector<float> data;
        size_t currentIndex;
        bool fullCycle;
 
    public:
        SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize)
            : windowSize(windowSize),
              vectorSize(vectorSize),
              data(vectorSize * windowSize, 0), // initialize all data to 0
              currentIndex(0),
              fullCycle(false)
        {
        }
 
        void addEntry(const std::vector<float>& entry)
        {
            if (entry.size() != vectorSize)
            {
                throw LocalException("Vector of wrong size added. Execting: ") << vectorSize << "; Actual: " << entry.size();
            }
 
            for (size_t i = 0; i < entry.size(); i++)
            {
                data.at(i + currentIndex * vectorSize) = entry.at(i);
            }
 
            currentIndex = (currentIndex + 1) % windowSize;
            fullCycle = fullCycle || currentIndex == 0;
        }
 
        std::vector<float> getMedian()
        {
            std::vector<float> median;
 
            for (size_t i = 0; i < vectorSize; i++)
            {
                std::vector<float> samples;
 
                for (size_t n = 0; n < windowSize; n++)
                {
                    samples.push_back(data.at(i + n * vectorSize));
                }
 
                std::sort(samples.begin(), samples.end());
                median.push_back(StatUtils::GetMedian(samples));
            }
 
            return median;
        }
 
    };
}