MRPT  1.9.9
CPose3DPDFSOG.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 #include "poses-precomp.h" // Precompiled headers
11 
16 #include <mrpt/system/os.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::math;
21 using namespace mrpt::system;
22 using namespace std;
23 
25 
26 /*---------------------------------------------------------------
27  Constructor
28  ---------------------------------------------------------------*/
29 CPose3DPDFSOG::CPose3DPDFSOG(size_t nModes) : m_modes(nModes) {}
30 /*---------------------------------------------------------------
31  clear
32  ---------------------------------------------------------------*/
33 void CPose3DPDFSOG::clear() { m_modes.clear(); }
34 /*---------------------------------------------------------------
35  Resize
36  ---------------------------------------------------------------*/
37 void CPose3DPDFSOG::resize(const size_t N) { m_modes.resize(N); }
38 /*---------------------------------------------------------------
39  getMean
40  Returns an estimate of the pose, (the mean, or mathematical expectation of the
41  PDF)
42  ---------------------------------------------------------------*/
44 {
45  if (!m_modes.empty())
46  {
47  // Calc average on SE(3)
48  mrpt::poses::SE_average<3> se_averager;
49  for (const auto& m : m_modes)
50  {
51  const double w = exp(m.log_w);
52  se_averager.append(m.val.mean, w);
53  }
54  se_averager.get_average(p);
55  }
56  else
57  {
58  p.setFromValues(0, 0, 0, 0, 0, 0);
59  }
60 }
61 
62 std::tuple<mrpt::math::CMatrixDouble66, CPose3D>
64 {
65  const size_t N = m_modes.size();
66 
67  // Get mean:
68  CPose3D mean;
69  getMean(mean);
70 
71  // Get cov:
73  if (N)
74  {
75  double sumW = 0;
76 
79  for (const auto& m : m_modes)
80  {
81  double w;
82  sumW += w = exp(m.log_w);
83  estMean_i = mrpt::math::CMatrixDouble61(m.val.mean);
84  MMt.matProductOf_AAt(estMean_i);
85  MMt += m.val.cov;
86  MMt *= w;
87  estCov += MMt; // w * ( (it)->val.cov +
88  // ((estMean_i-estMean)*(~(estMean_i-estMean))) );
89  }
90 
91  if (sumW != 0) estCov *= (1.0 / sumW);
92  }
93 
94  return {estCov, mean};
95 }
96 
97 uint8_t CPose3DPDFSOG::serializeGetVersion() const { return 2; }
99 {
100  uint32_t N = m_modes.size();
101  out << N;
102  for (const auto& m : m_modes) out << m.log_w << m.val.mean << m.val.cov;
103 }
105  mrpt::serialization::CArchive& in, uint8_t version)
106 {
107  switch (version)
108  {
109  case 0:
110  case 1:
111  case 2:
112  {
113  uint32_t N;
114  in >> N;
115  this->resize(N);
116 
117  for (auto& m : m_modes)
118  {
119  in >> m.log_w;
120 
121  // In version 0, weights were linear!!
122  if (version == 0) m.log_w = log(std::max(1e-300, m.log_w));
123 
124  in >> m.val.mean;
125 
126  if (version == 1) // were floats
127  {
128  THROW_EXCEPTION("Unsupported serialized version: too old");
129  }
130  else
131  in >> m.val.cov;
132  }
133  }
134  break;
135  default:
137  };
138 }
139 
141 {
142  MRPT_START
143 
144  if (this == &o) return; // It may be used sometimes
145 
147  {
148  *this = dynamic_cast<const CPose3DPDFSOG&>(o);
149  }
150  else
151  {
152  this->resize(1);
153  m_modes[0].log_w = 0;
155  o.getCovarianceAndMean(C, m_modes[0].val.mean);
156  m_modes[0].val.cov = C;
157  }
158 
159  MRPT_END
160 }
161 
162 /*---------------------------------------------------------------
163  saveToTextFile
164  ---------------------------------------------------------------*/
165 bool CPose3DPDFSOG::saveToTextFile(const std::string& file) const
166 {
167  FILE* f = os::fopen(file.c_str(), "wt");
168  if (!f) return false;
169 
170  for (const auto& m : m_modes)
171  os::fprintf(
172  f, "%e %e %e %e %e %e %e %e %e %e\n", exp(m.log_w), m.val.mean.x(),
173  m.val.mean.y(), m.val.mean.z(), m.val.cov(0, 0), m.val.cov(1, 1),
174  m.val.cov(2, 2), m.val.cov(0, 1), m.val.cov(0, 2), m.val.cov(1, 2));
175  os::fclose(f);
176  return true;
177 }
178 
179 /*---------------------------------------------------------------
180  changeCoordinatesReference
181  ---------------------------------------------------------------*/
183 {
184  for (auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
185 }
186 
187 /*---------------------------------------------------------------
188  bayesianFusion
189  ---------------------------------------------------------------*/
191 {
192  MRPT_START
193 
194  // p1: CPose3DPDFSOG, p2: CPosePDFGaussian:
195 
198 
199  THROW_EXCEPTION("TODO!!!");
200 
201  MRPT_END
202 }
203 
204 /*---------------------------------------------------------------
205  enforceCovSymmetry
206  ---------------------------------------------------------------*/
208 {
209  MRPT_START
210  // Differences, when they exist, appear in the ~15'th significant
211  // digit, so... just take one of them arbitrarily!
212  for (auto& m : m_modes)
213  {
214  for (size_t i = 0; i < 6; i++)
215  for (size_t j = i + 1; j < 6; j++)
216  m.val.cov(i, j) = m.val.cov(j, i);
217  }
218 
219  MRPT_END
220 }
221 
222 /*---------------------------------------------------------------
223  normalizeWeights
224  ---------------------------------------------------------------*/
226 {
227  MRPT_START
228  if (m_modes.empty()) return;
229  double maxW = m_modes[0].log_w;
230  for (auto& m : m_modes) maxW = max(maxW, m.log_w);
231  for (auto& m : m_modes) m.log_w -= maxW;
232  MRPT_END
233 }
234 
235 /*---------------------------------------------------------------
236  drawSingleSample
237  ---------------------------------------------------------------*/
238 void CPose3DPDFSOG::drawSingleSample([[maybe_unused]] CPose3D& outPart) const
239 {
240  THROW_EXCEPTION("TO DO!");
241 }
242 
243 /*---------------------------------------------------------------
244  drawManySamples
245  ---------------------------------------------------------------*/
247  [[maybe_unused]] size_t N,
248  [[maybe_unused]] std::vector<CVectorDouble>& outSamples) const
249 {
250  THROW_EXCEPTION("TO DO!");
251 }
252 
253 /*---------------------------------------------------------------
254  inverse
255  ---------------------------------------------------------------*/
257 {
258  MRPT_START
260  auto* out = dynamic_cast<CPose3DPDFSOG*>(&o);
261  ASSERT_(out != nullptr);
262 
263  // Prepare the output SOG:
264  out->resize(m_modes.size());
265 
266  const_iterator it;
267  iterator outIt;
268 
269  for (it = m_modes.begin(), outIt = out->m_modes.begin();
270  it != m_modes.end(); it++, outIt++)
271  {
272  (it)->val.inverse((outIt)->val);
273  (outIt)->log_w = (it)->log_w;
274  }
275 
276  MRPT_END
277 }
278 
279 /*---------------------------------------------------------------
280  appendFrom
281  ---------------------------------------------------------------*/
283 {
284  MRPT_START
285 
286  ASSERT_(&o != this); // Don't be bad...
287  if (o.m_modes.empty()) return;
288 
289  // Make copies:
290  for (const auto& m_mode : o.m_modes) m_modes.push_back(m_mode);
291 
292  normalizeWeights();
293  MRPT_END
294 }
295 
296 /*---------------------------------------------------------------
297  getMostLikelyMode
298  ---------------------------------------------------------------*/
300 {
301  if (this->empty())
302  {
303  outVal = CPose3DPDFGaussian();
304  }
305  else
306  {
307  auto it_best = m_modes.end();
308  for (auto it = m_modes.begin(); it != m_modes.end(); ++it)
309  if (it_best == m_modes.end() || it->log_w > it_best->log_w)
310  it_best = it;
311 
312  outVal = it_best->val;
313  }
314 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
#define MRPT_START
Definition: exceptions.h:241
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
void getMostLikelyMode(CPose3DPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
void appendFrom(const CPose3DPDFSOG &o)
Append the Gaussian modes from "o" to the current set of modes of "this" density. ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
void getMean(CPose3D &mean_pose) const override
STL namespace.
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.
TModesList m_modes
Access directly to this array for modify the modes as desired.
Definition: CPose3DPDFSOG.h:64
bool saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
CMatrixFixed< double, 6, 1 > CMatrixDouble61
Definition: CMatrixFixed.h:377
int val
Definition: mrpt_jpeglib.h:957
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:51
bool empty() const
Definition: ts_hash_map.h:191
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Computes weighted and un-weighted averages of SE(3) poses.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
virtual std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void clear()
Clear all the gaussian modes.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:52
This file implements matrix/vector text and binary serialization.
#define MRPT_END
Definition: exceptions.h:245
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
double mean(const CONTAINER &v)
Computes the mean value of a vector.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
images resize(NUM_IMGS)
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void resize(const size_t N)
Set the number of SOG modes.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020