Main MRPT website > C++ reference for MRPT 1.5.7
TSlidingWindow.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 // Implementattion file for TSlidingWindow struct
11 #include "graphslam-precomp.h" // Precompiled headers
12 
14 #include <mrpt/utils/CStream.h>
16 
17 using namespace mrpt::graphslam;
18 
20  std::string name /* = "window" */) {
21  MRPT_START;
22 
23  m_win_size = 5; // just a default value
24  m_name = name;
25 
26  m_mean_cached = 0;
27  m_median_cached = 0;
28 
29  m_is_initialized = false;
30  m_mean_updated = false;
31  m_median_updated = false;
32  m_std_dev_updated = false;
33 
34  MRPT_END;
35 }
37 
39  MRPT_START;
40 
41  double median_out = 0.0;
42  if (m_measurements_vec.empty()) {
43  return 0.0;
44  }
45 
46  if (m_median_updated) {
47  median_out = m_median_cached;
48  }
49  else {
50  // copy the current vector, sort it and return value in middle
51  std::vector<double> vec_sorted(m_measurements_vec);
52  std::sort(vec_sorted.begin(), vec_sorted.end());
53 
54  median_out = vec_sorted.at(vec_sorted.size()/2);
55 
56  m_median_cached = median_out;
57  m_median_updated = true;
58  }
59 
60  return median_out;
61 
62  MRPT_END;
63 }
65  MRPT_START;
66 
67  double mean_out = 0.0;
68 
69  if (m_mean_updated) {
70  mean_out = m_mean_cached;
71  }
72  else {
73  mean_out = std::accumulate(m_measurements_vec.begin(), m_measurements_vec.end(), 0.0);
74  mean_out /= m_measurements_vec.size();
75 
76  m_mean_cached = mean_out;
77  m_mean_updated = true;
78  }
79 
80  return mean_out;
81 
82  MRPT_END;
83 }
85  MRPT_START;
86 
87  double std_dev_out = 0.0;
88 
89  if (m_std_dev_updated) { // return the cached version?
90  std_dev_out = m_std_dev_cached;
91  }
92  else {
93  double mean = this->getMean();
94 
95  double sum_of_sq_diffs = 0;
97  it != m_measurements_vec.end(); ++it) {
98  sum_of_sq_diffs += std::pow(*it - mean, 2);
99  }
100  std_dev_out = sqrt(sum_of_sq_diffs / m_win_size);
101 
102  m_std_dev_cached = std_dev_out;
103  m_std_dev_updated = true;
104  }
105 
106  return std_dev_out;
107  MRPT_END;
108 }
109 
111  // get the boundaries for acceptance of measurements - [-3sigma, 3sigma] with
112  // regards to the mean
113  double low_lim = this->getMean() - 3*this->getStdDev();
114  double upper_lim = this->getMean() + 3*this->getStdDev();
115 
116  return measurement > low_lim && measurement < upper_lim;
117 }
119  double value) {
120  MRPT_START;
121 
122  double threshold = this->getMean();
123  return (value > threshold);
124 
125  MRPT_END;
126 }
128  double value) {
130 }
131 
132 
134  double measurement ) {
135  MRPT_START;
136 
137  m_is_initialized = true;
138 
139  // if I haven't already filled up to win_size the vector, just add it
140  if ( m_win_size > m_measurements_vec.size() ) {
141  m_measurements_vec.push_back(measurement);
142  }
143  else {
144  // remove first element - add it as last element
145  m_measurements_vec.erase(m_measurements_vec.begin());
146  m_measurements_vec.push_back(measurement);
147  }
148 
149  m_mean_updated = false;
150  m_median_updated = false;
151  m_std_dev_updated = false;
152 
153  MRPT_END;
154 }
156  size_t new_size ) {
157  MRPT_START;
158 
159  size_t curr_size = m_measurements_vec.size();
160  if ( new_size < curr_size ) {
161  // remove (curr_size - new_size) elements from the beginning of the
162  // measurements vector
164  m_measurements_vec.begin() + (curr_size - new_size));
165 
166  m_mean_updated = false;
167  m_median_updated = false;
168  }
169 
170  m_win_size = new_size;
171 
172  MRPT_END;
173 }
176  const std::string& section) {
177  MRPT_START;
178 
179  size_t sliding_win_size = source.read_int(
180  section,
181  "sliding_win_size",
182  10, false);
183  this->resizeWindow(sliding_win_size);
184 
185  MRPT_END;
186 }
188  mrpt::utils::CStream &out) const {
189  MRPT_START;
190 
191  out.printf("-----------[ %s: Sliding Window Properties ]-----------\n",
192  m_name.c_str());
193  out.printf("Measurements Vector: \n");
195  it != m_measurements_vec.end(); ++it ) {
196  out.printf("\t%.2f\n", *it);
197  }
198  out.printf("\n");
199 
200  out.printf("m_name : %s\n" , m_name.c_str());
201  out.printf("m_mean_cached : %.2f\n" , m_mean_cached);
202  out.printf("m_median_cached : %.2f\n" , m_median_cached);
203  out.printf("m_std_dev_cached : %.2f\n" , m_std_dev_cached);
204  out.printf("m_mean_updated : %s\n" , m_mean_updated? "TRUE": "FALSE");
205  out.printf("m_median_updated : %s\n" , m_median_updated? "TRUE": "FALSE");
206  out.printf("m_std_dev_updated : %s\n" , m_std_dev_updated? "TRUE": "FALSE");
207  out.printf("m_win_size : %lu\n" , m_win_size);
208  out.printf("m_is_initialized : %s\n" , m_is_initialized? "TRUE": "FALSE");
209 
210  MRPT_END;
211 }
212 
214  return m_win_size;
215 }
216 
218  return (m_win_size == m_measurements_vec.size());
219 
220 }
void resizeWindow(size_t new_size)
Resize the window.
std::vector< double > m_measurements_vec
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
This class allows loading and storing values and vectors of different types from a configuration text...
bool evaluateMeasurementInGaussian(double measurement)
Determine whether the incoming measurement is inside the [-3sigma, +3sigma] boundaries from the curre...
bool evaluateMeasurementAbove(double value)
Determine whether the incoming measurement is over the current mean value.
void addNewMeasurement(double measurement)
Update the sliding window by appending a new measurement.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
bool m_is_initialized
flag is raised the first time that TSlidingWindow::addNewMeasurement is called
SLAM methods related to graphs of pose constraints.
#define MRPT_END
double getStdDev()
Return the Standard deviation of the current measurement vector.
double m_mean_cached
Cached mean value.
double getMean()
Return the current mean value.
double m_std_dev_cached
Cached version of the standard deviation.
GLsizei const GLchar ** string
Definition: glext.h:3919
bool m_median_updated
Is the median up-to-date?
#define MRPT_START
TSlidingWindow(std::string name="window")
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
GLuint const GLchar * name
Definition: glext.h:3891
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
std::string m_name
Name of the TSlidingWindow Instance at hand.
bool evaluateMeasurementBelow(double value)
Determine whether the incoming measurement is less or equal to the current mean value.
GLsizei const GLfloat * value
Definition: glext.h:3929
size_t getWindowSize() const
Return the size of the window.
bool m_mean_updated
Is the mean up-to-date?
bool windowIsFull() const
Check if the window has reached its limit.
double m_median_cached
Cached median value.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
double getMedian()
Return the current median value.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
bool m_std_dev_updated
Is the standard deviation up-to-date?



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019