TSlidingWindow.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
         |                     Mobile Robot Programming Toolkit (MRPT)               |
         |                          http://www.mrpt.org/                             |
         |                                                                           |
         | Copyright (c) 2005-2017, 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/utils/CConfigFileBase.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/graphslam/misc/TSlidingWindow.h>
 
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::utils::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(
                mrpt::utils::CStream &out) const {
        MRPT_START;
 
        out.printf("-----------[ %s: Sliding Window Properties ]-----------\n",
                        m_name.c_str());
        out.printf("Measurements Vector: \n");
        for (std::vector<double>::const_iterator it = m_measurements_vec.begin();
                        it != m_measurements_vec.end(); ++it ) {
                out.printf("\t%.2f\n", *it);
        }
        out.printf("\n");
 
        out.printf("m_name              : %s\n"   , m_name.c_str());
        out.printf("m_mean_cached       : %.2f\n" , m_mean_cached);
        out.printf("m_median_cached     : %.2f\n" , m_median_cached);
        out.printf("m_std_dev_cached    : %.2f\n" , m_std_dev_cached);
        out.printf("m_mean_updated      : %s\n"   , m_mean_updated? "TRUE": "FALSE");
        out.printf("m_median_updated    : %s\n"   , m_median_updated? "TRUE": "FALSE");
        out.printf("m_std_dev_updated   : %s\n"   , m_std_dev_updated? "TRUE": "FALSE");
        out.printf("m_win_size          : %lu\n"  , m_win_size);
        out.printf("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());
 
}