Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
16 
17 #include <algorithm>
18 #include <numeric>
19 
20 using namespace mrpt::graphslam;
21 
23 {
24  MRPT_START;
25 
26  m_win_size = 5; // just a default value
27  m_name = name;
28 
29  m_mean_cached = 0;
30  m_median_cached = 0;
31 
32  m_is_initialized = false;
33  m_mean_updated = false;
34  m_median_updated = false;
35  m_std_dev_updated = false;
36 
37  MRPT_END;
38 }
41 {
42  MRPT_START;
43 
44  double median_out = 0.0;
45  if (m_measurements_vec.empty())
46  {
47  return 0.0;
48  }
49 
50  if (m_median_updated)
51  {
52  median_out = m_median_cached;
53  }
54  else
55  {
56  // copy the current vector, sort it and return value in middle
57  std::vector<double> vec_sorted(m_measurements_vec);
58  std::sort(vec_sorted.begin(), vec_sorted.end());
59 
60  median_out = vec_sorted.at(vec_sorted.size() / 2);
61 
62  m_median_cached = median_out;
63  m_median_updated = true;
64  }
65 
66  return median_out;
67 
68  MRPT_END;
69 }
71 {
72  MRPT_START;
73 
74  double mean_out = 0.0;
75 
76  if (m_mean_updated)
77  {
78  mean_out = m_mean_cached;
79  }
80  else
81  {
82  mean_out = std::accumulate(
83  m_measurements_vec.begin(), m_measurements_vec.end(), 0.0);
84  mean_out /= m_measurements_vec.size();
85 
86  m_mean_cached = mean_out;
87  m_mean_updated = true;
88  }
89 
90  return mean_out;
91 
92  MRPT_END;
93 }
95 {
96  MRPT_START;
97 
98  double std_dev_out = 0.0;
99 
100  if (m_std_dev_updated)
101  { // return the cached version?
102  std_dev_out = m_std_dev_cached;
103  }
104  else
105  {
106  double mean = this->getMean();
107 
108  double sum_of_sq_diffs = 0;
110  m_measurements_vec.begin();
111  it != m_measurements_vec.end(); ++it)
112  {
113  sum_of_sq_diffs += std::pow(*it - mean, 2);
114  }
115  std_dev_out = sqrt(sum_of_sq_diffs / m_win_size);
116 
117  m_std_dev_cached = std_dev_out;
118  m_std_dev_updated = true;
119  }
120 
121  return std_dev_out;
122  MRPT_END;
123 }
124 
126 {
127  // get the boundaries for acceptance of measurements - [-3sigma, 3sigma]
128  // with
129  // regards to the mean
130  double low_lim = this->getMean() - 3 * this->getStdDev();
131  double upper_lim = this->getMean() + 3 * this->getStdDev();
132 
133  return measurement > low_lim && measurement < upper_lim;
134 }
136 {
137  MRPT_START;
138 
139  double threshold = this->getMean();
140  return (value > threshold);
141 
142  MRPT_END;
143 }
145 {
147 }
148 
149 void TSlidingWindow::addNewMeasurement(double measurement)
150 {
151  MRPT_START;
152 
153  m_is_initialized = true;
154 
155  // if I haven't already filled up to win_size the vector, just add it
156  if (m_win_size > m_measurements_vec.size())
157  {
158  m_measurements_vec.push_back(measurement);
159  }
160  else
161  {
162  // remove first element - add it as last element
163  m_measurements_vec.erase(m_measurements_vec.begin());
164  m_measurements_vec.push_back(measurement);
165  }
166 
167  m_mean_updated = false;
168  m_median_updated = false;
169  m_std_dev_updated = false;
170 
171  MRPT_END;
172 }
173 void TSlidingWindow::resizeWindow(size_t new_size)
174 {
175  MRPT_START;
176 
177  size_t curr_size = m_measurements_vec.size();
178  if (new_size < curr_size)
179  {
180  // remove (curr_size - new_size) elements from the beginning of the
181  // measurements vector
182  m_measurements_vec.erase(
183  m_measurements_vec.begin(),
184  m_measurements_vec.begin() + (curr_size - new_size));
185 
186  m_mean_updated = false;
187  m_median_updated = false;
188  }
189 
190  m_win_size = new_size;
191 
192  MRPT_END;
193 }
195  const mrpt::config::CConfigFileBase& source, const std::string& section)
196 {
197  MRPT_START;
198 
199  size_t sliding_win_size =
200  source.read_int(section, "sliding_win_size", 10, false);
201  this->resizeWindow(sliding_win_size);
202 
203  MRPT_END;
204 }
205 void TSlidingWindow::dumpToTextStream(std::ostream& out) const
206 {
207  MRPT_START;
208 
209  out << mrpt::format(
210  "-----------[ %s: Sliding Window Properties ]-----------\n",
211  m_name.c_str());
212  out << mrpt::format("Measurements Vector: \n");
214  it != m_measurements_vec.end(); ++it)
215  {
216  out << mrpt::format("\t%.2f\n", *it);
217  }
218  out << mrpt::format("\n");
219 
220  out << mrpt::format("m_name : %s\n", m_name.c_str());
221  out << mrpt::format("m_mean_cached : %.2f\n", m_mean_cached);
222  out << mrpt::format("m_median_cached : %.2f\n", m_median_cached);
223  out << mrpt::format("m_std_dev_cached : %.2f\n", m_std_dev_cached);
224  out << mrpt::format(
225  "m_mean_updated : %s\n", m_mean_updated ? "TRUE" : "FALSE");
226  out << mrpt::format(
227  "m_median_updated : %s\n", m_median_updated ? "TRUE" : "FALSE");
228  out << mrpt::format(
229  "m_std_dev_updated : %s\n", m_std_dev_updated ? "TRUE" : "FALSE");
230  out << mrpt::format("m_win_size : %lu\n", m_win_size);
231  out << mrpt::format(
232  "m_is_initialized : %s\n", m_is_initialized ? "TRUE" : "FALSE");
233 
234  MRPT_END;
235 }
236 
237 size_t TSlidingWindow::getWindowSize() const { return m_win_size; }
239 {
240  return (m_win_size == m_measurements_vec.size());
241 }
mrpt::graphslam::TSlidingWindow::addNewMeasurement
void addNewMeasurement(double measurement)
Update the sliding window by appending a new measurement.
Definition: TSlidingWindow.cpp:149
mrpt::graphslam::TSlidingWindow::getStdDev
double getStdDev()
Return the Standard deviation of the current measurement vector.
Definition: TSlidingWindow.cpp:94
mrpt::graphslam::TSlidingWindow::m_win_size
size_t m_win_size
Definition: TSlidingWindow.h:99
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::graphslam::TSlidingWindow::m_name
std::string m_name
Name of the TSlidingWindow Instance at hand.
Definition: TSlidingWindow.h:103
mrpt::graphslam::TSlidingWindow::m_std_dev_updated
bool m_std_dev_updated
Is the standard deviation up-to-date?
Definition: TSlidingWindow.h:110
mrpt::graphslam::TSlidingWindow::m_median_cached
double m_median_cached
Cached median value.
Definition: TSlidingWindow.h:106
mrpt::graphslam::TSlidingWindow::getWindowSize
size_t getWindowSize() const
Return the size of the window.
Definition: TSlidingWindow.cpp:237
mrpt::graphslam::TSlidingWindow::m_std_dev_cached
double m_std_dev_cached
Cached version of the standard deviation.
Definition: TSlidingWindow.h:107
name
GLuint const GLchar * name
Definition: glext.h:4054
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::graphslam
SLAM methods related to graphs of pose constraints.
Definition: TUserOptionsChecker.h:33
mrpt::graphslam::TSlidingWindow::m_mean_updated
bool m_mean_updated
Is the mean up-to-date?
Definition: TSlidingWindow.h:108
CConfigFileBase.h
graphslam-precomp.h
TSlidingWindow.h
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::graphslam::TSlidingWindow::windowIsFull
bool windowIsFull() const
Check if the window has reached its limit.
Definition: TSlidingWindow.cpp:238
mrpt::graphslam::TSlidingWindow::evaluateMeasurementBelow
bool evaluateMeasurementBelow(double value)
Determine whether the incoming measurement is less or equal to the current mean value.
Definition: TSlidingWindow.cpp:144
mrpt::graphslam::TSlidingWindow::dumpToTextStream
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Definition: TSlidingWindow.cpp:205
mrpt::graphslam::TSlidingWindow::resizeWindow
void resizeWindow(size_t new_size)
Resize the window.
Definition: TSlidingWindow.cpp:173
mrpt::graphslam::TSlidingWindow::getMedian
double getMedian()
Return the current median value.
Definition: TSlidingWindow.cpp:40
mrpt::graphslam::TSlidingWindow::m_is_initialized
bool m_is_initialized
flag is raised the first time that TSlidingWindow::addNewMeasurement is called
Definition: TSlidingWindow.h:116
mrpt::graphslam::TSlidingWindow::getMean
double getMean()
Return the current mean value.
Definition: TSlidingWindow.cpp:70
mrpt::graphslam::TSlidingWindow::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: TSlidingWindow.cpp:194
mrpt::graphslam::TSlidingWindow::evaluateMeasurementInGaussian
bool evaluateMeasurementInGaussian(double measurement)
Determine whether the incoming measurement is inside the [-3sigma, +3sigma] boundaries from the curre...
Definition: TSlidingWindow.cpp:125
mrpt::graphslam::TSlidingWindow::m_median_updated
bool m_median_updated
Is the median up-to-date?
Definition: TSlidingWindow.h:109
mrpt::graphslam::TSlidingWindow::m_measurements_vec
std::vector< double > m_measurements_vec
Definition: TSlidingWindow.h:100
value
GLsizei const GLfloat * value
Definition: glext.h:4117
mean
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Definition: eigen_plugins.h:427
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::graphslam::TSlidingWindow::evaluateMeasurementAbove
bool evaluateMeasurementAbove(double value)
Determine whether the incoming measurement is over the current mean value.
Definition: TSlidingWindow.cpp:135
mrpt::graphslam::TSlidingWindow::TSlidingWindow
TSlidingWindow(std::string name="window")
Definition: TSlidingWindow.cpp:22
mrpt::graphslam::TSlidingWindow::~TSlidingWindow
~TSlidingWindow()
Definition: TSlidingWindow.cpp:39
string
GLsizei const GLchar ** string
Definition: glext.h:4101
CArchive.h
mrpt::graphslam::TSlidingWindow::m_mean_cached
double m_mean_cached
Cached mean value.
Definition: TSlidingWindow.h:105



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST