TSlidingWindow.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
// Implementattion file for TSlidingWindow struct
#include "graphslam-precomp.h"  // Precompiled headers
 
#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/graphslam/misc/TSlidingWindow.h>
 
#include <algorithm>
#include <numeric>
 
using namespace mrpt::graphslam;
 
TSlidingWindow::TSlidingWindow(std::string name /* = "window" */)
{
        MRPT_START;
 
        m_win_size = 5;  // just a default value
        m_name = name;
 
        m_mean_cached = 0;
        m_median_cached = 0;
 
        m_is_initialized = false;
        m_mean_updated = false;
        m_median_updated = false;
        m_std_dev_updated = false;
 
        MRPT_END;
}
TSlidingWindow::~TSlidingWindow() {}
double TSlidingWindow::getMedian()
{
        MRPT_START;
 
        double median_out = 0.0;
        if (m_measurements_vec.empty())
        {
                return 0.0;
        }
 
        if (m_median_updated)
        {
                median_out = m_median_cached;
        }
        else
        {
                // copy the current vector, sort it and return value in middle
                std::vector<double> vec_sorted(m_measurements_vec);
                std::sort(vec_sorted.begin(), vec_sorted.end());
 
                median_out = vec_sorted.at(vec_sorted.size() / 2);
 
                m_median_cached = median_out;
                m_median_updated = true;
        }
 
        return median_out;
 
        MRPT_END;
}
double TSlidingWindow::getMean()
{
        MRPT_START;
 
        double mean_out = 0.0;
 
        if (m_mean_updated)
        {
                mean_out = m_mean_cached;
        }
        else
        {
                mean_out = std::accumulate(
                        m_measurements_vec.begin(), m_measurements_vec.end(), 0.0);
                mean_out /= m_measurements_vec.size();
 
                m_mean_cached = mean_out;
                m_mean_updated = true;
        }
 
        return mean_out;
 
        MRPT_END;
}
double TSlidingWindow::getStdDev()
{
        MRPT_START;
 
        double std_dev_out = 0.0;
 
        if (m_std_dev_updated)
        {  // return the cached version?
                std_dev_out = m_std_dev_cached;
        }
        else
        {
                double mean = this->getMean();
 
                double sum_of_sq_diffs = 0;
                for (std::vector<double>::const_iterator it =
                                 m_measurements_vec.begin();
                         it != m_measurements_vec.end(); ++it)
                {
                        sum_of_sq_diffs += std::pow(*it - mean, 2);
                }
                std_dev_out = sqrt(sum_of_sq_diffs / m_win_size);
 
                m_std_dev_cached = std_dev_out;
                m_std_dev_updated = true;
        }
 
        return std_dev_out;
        MRPT_END;
}
 
bool TSlidingWindow::evaluateMeasurementInGaussian(double measurement)
{
        // get the boundaries for acceptance of measurements - [-3sigma, 3sigma]
        // with
        // regards to the mean
        double low_lim = this->getMean() - 3 * this->getStdDev();
        double upper_lim = this->getMean() + 3 * this->getStdDev();
 
        return measurement > low_lim && measurement < upper_lim;
}
bool TSlidingWindow::evaluateMeasurementAbove(double value)
{
        MRPT_START;
 
        double threshold = this->getMean();
        return (value > threshold);
 
        MRPT_END;
}
bool TSlidingWindow::evaluateMeasurementBelow(double value)
{
        return !evaluateMeasurementAbove(value);
}
 
void TSlidingWindow::addNewMeasurement(double measurement)
{
        MRPT_START;
 
        m_is_initialized = true;
 
        // if I haven't already filled up to win_size the vector, just add it
        if (m_win_size > m_measurements_vec.size())
        {
                m_measurements_vec.push_back(measurement);
        }
        else
        {
                // remove first element - add it as last element
                m_measurements_vec.erase(m_measurements_vec.begin());
                m_measurements_vec.push_back(measurement);
        }
 
        m_mean_updated = false;
        m_median_updated = false;
        m_std_dev_updated = false;
 
        MRPT_END;
}
void TSlidingWindow::resizeWindow(size_t new_size)
{
        MRPT_START;
 
        size_t curr_size = m_measurements_vec.size();
        if (new_size < curr_size)
        {
                // remove (curr_size - new_size) elements from the beginning of the
                // measurements vector
                m_measurements_vec.erase(
                        m_measurements_vec.begin(),
                        m_measurements_vec.begin() + (curr_size - new_size));
 
                m_mean_updated = false;
                m_median_updated = false;
        }
 
        m_win_size = new_size;
 
        MRPT_END;
}
void TSlidingWindow::loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source, const std::string& section)
{
        MRPT_START;
 
        size_t sliding_win_size =
                source.read_int(section, "sliding_win_size", 10, false);
        this->resizeWindow(sliding_win_size);
 
        MRPT_END;
}
void TSlidingWindow::dumpToTextStream(std::ostream& out) const
{
        MRPT_START;
 
        out << mrpt::format(
                "-----------[ %s: Sliding Window Properties ]-----------\n",
                m_name.c_str());
        out << mrpt::format("Measurements Vector: \n");
        for (std::vector<double>::const_iterator it = m_measurements_vec.begin();
                 it != m_measurements_vec.end(); ++it)
        {
                out << mrpt::format("\t%.2f\n", *it);
        }
        out << mrpt::format("\n");
 
        out << mrpt::format("m_name              : %s\n", m_name.c_str());
        out << mrpt::format("m_mean_cached       : %.2f\n", m_mean_cached);
        out << mrpt::format("m_median_cached     : %.2f\n", m_median_cached);
        out << mrpt::format("m_std_dev_cached    : %.2f\n", m_std_dev_cached);
        out << mrpt::format(
                "m_mean_updated      : %s\n", m_mean_updated ? "TRUE" : "FALSE");
        out << mrpt::format(
                "m_median_updated    : %s\n", m_median_updated ? "TRUE" : "FALSE");
        out << mrpt::format(
                "m_std_dev_updated   : %s\n", m_std_dev_updated ? "TRUE" : "FALSE");
        out << mrpt::format("m_win_size          : %lu\n", m_win_size);
        out << mrpt::format(
                "m_is_initialized    : %s\n", m_is_initialized ? "TRUE" : "FALSE");
 
        MRPT_END;
}
 
size_t TSlidingWindow::getWindowSize() const { return m_win_size; }
bool TSlidingWindow::windowIsFull() const
{
        return (m_win_size == m_measurements_vec.size());
}