MRPT  2.0.1
TSlidingWindow.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 
22 TSlidingWindow::TSlidingWindow(const std::string& name /* = "window" */)
23  : m_name(name)
24 {
25 }
26 
28 {
30 
31  double median_out = 0.0;
32  if (m_measurements_vec.empty())
33  {
34  return 0.0;
35  }
36 
37  if (m_median_updated)
38  {
39  median_out = m_median_cached;
40  }
41  else
42  {
43  // copy the current vector, sort it and return value in middle
44  std::vector<double> vec_sorted(m_measurements_vec);
45  std::sort(vec_sorted.begin(), vec_sorted.end());
46 
47  median_out = vec_sorted.at(vec_sorted.size() / 2);
48 
49  m_median_cached = median_out;
50  m_median_updated = true;
51  }
52 
53  return median_out;
54 
55  MRPT_END
56 }
58 {
60 
61  double mean_out = 0.0;
62 
63  if (m_mean_updated)
64  {
65  mean_out = m_mean_cached;
66  }
67  else
68  {
69  mean_out = std::accumulate(
70  m_measurements_vec.begin(), m_measurements_vec.end(), 0.0);
71  mean_out /= m_measurements_vec.size();
72 
73  m_mean_cached = mean_out;
74  m_mean_updated = true;
75  }
76 
77  return mean_out;
78 
79  MRPT_END
80 }
82 {
84 
85  double std_dev_out = 0.0;
86 
88  { // return the cached version?
89  std_dev_out = m_std_dev_cached;
90  }
91  else
92  {
93  double mean = this->getMean();
94 
95  double sum_of_sq_diffs = 0;
96  for (auto it = m_measurements_vec.begin();
97  it != m_measurements_vec.end(); ++it)
98  {
99  sum_of_sq_diffs += std::pow(*it - mean, 2);
100  }
101  std_dev_out = sqrt(sum_of_sq_diffs / m_win_size);
102 
103  m_std_dev_cached = std_dev_out;
104  m_std_dev_updated = true;
105  }
106 
107  return std_dev_out;
108  MRPT_END
109 }
110 
112 {
113  // get the boundaries for acceptance of measurements - [-3sigma, 3sigma]
114  // with
115  // regards to the mean
116  double low_lim = this->getMean() - 3 * this->getStdDev();
117  double upper_lim = this->getMean() + 3 * this->getStdDev();
118 
119  return measurement > low_lim && measurement < upper_lim;
120 }
122 {
123  MRPT_START
124 
125  double threshold = this->getMean();
126  return (value > threshold);
127 
128  MRPT_END
129 }
131 {
132  return !evaluateMeasurementAbove(value);
133 }
134 
135 void TSlidingWindow::addNewMeasurement(double measurement)
136 {
137  MRPT_START
138 
139  m_is_initialized = true;
140 
141  // if I haven't already filled up to win_size the vector, just add it
142  if (m_win_size > m_measurements_vec.size())
143  {
144  m_measurements_vec.push_back(measurement);
145  }
146  else
147  {
148  // remove first element - add it as last element
149  m_measurements_vec.erase(m_measurements_vec.begin());
150  m_measurements_vec.push_back(measurement);
151  }
152 
153  m_mean_updated = false;
154  m_median_updated = false;
155  m_std_dev_updated = false;
156 
157  MRPT_END
158 }
159 void TSlidingWindow::resizeWindow(size_t new_size)
160 {
161  MRPT_START
162 
163  size_t curr_size = m_measurements_vec.size();
164  if (new_size < curr_size)
165  {
166  // remove (curr_size - new_size) elements from the beginning of the
167  // measurements vector
168  m_measurements_vec.erase(
169  m_measurements_vec.begin(),
170  m_measurements_vec.begin() + (curr_size - new_size));
171 
172  m_mean_updated = false;
173  m_median_updated = false;
174  }
175 
176  m_win_size = new_size;
177 
178  MRPT_END
179 }
181  const mrpt::config::CConfigFileBase& source, const std::string& section)
182 {
183  MRPT_START
184 
185  size_t sliding_win_size =
186  source.read_int(section, "sliding_win_size", 10, false);
187  this->resizeWindow(sliding_win_size);
188 
189  MRPT_END
190 }
191 void TSlidingWindow::dumpToTextStream(std::ostream& out) const
192 {
193  MRPT_START
194 
195  out << mrpt::format(
196  "-----------[ %s: Sliding Window Properties ]-----------\n",
197  m_name.c_str());
198  out << "Measurements Vector: \n";
199  for (double it : m_measurements_vec)
200  {
201  out << mrpt::format("\t%.2f\n", it);
202  }
203  out << "\n";
204 
205  out << mrpt::format("m_name : %s\n", m_name.c_str());
206  out << mrpt::format("m_mean_cached : %.2f\n", m_mean_cached);
207  out << mrpt::format("m_median_cached : %.2f\n", m_median_cached);
208  out << mrpt::format("m_std_dev_cached : %.2f\n", m_std_dev_cached);
209  out << mrpt::format(
210  "m_mean_updated : %s\n", m_mean_updated ? "TRUE" : "FALSE");
211  out << mrpt::format(
212  "m_median_updated : %s\n", m_median_updated ? "TRUE" : "FALSE");
213  out << mrpt::format(
214  "m_std_dev_updated : %s\n", m_std_dev_updated ? "TRUE" : "FALSE");
215  out << mrpt::format("m_win_size : %lu\n", m_win_size);
216  out << mrpt::format(
217  "m_is_initialized : %s\n", m_is_initialized ? "TRUE" : "FALSE");
218 
219  MRPT_END
220 }
221 
222 size_t TSlidingWindow::getWindowSize() const { return m_win_size; }
224 {
225  return (m_win_size == m_measurements_vec.size());
226 }
#define MRPT_START
Definition: exceptions.h:241
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void resizeWindow(size_t new_size)
Resize the window.
std::vector< double > m_measurements_vec
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool m_is_initialized
flag is raised the first time that TSlidingWindow::addNewMeasurement is called
SLAM methods related to graphs of pose constraints.
This class allows loading and storing values and vectors of different types from a configuration text...
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.
bool m_median_updated
Is the median up-to-date?
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
double mean(const CONTAINER &v)
Computes the mean value of a vector.
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.
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.
TSlidingWindow(const std::string &name="window")
double getMedian()
Return the current median value.
bool m_std_dev_updated
Is the standard deviation up-to-date?



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020