Main MRPT website > C++ reference for MRPT 1.9.9
CPoseRandomSampler.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 #include "poses-precomp.h" // Precompiled headers
11 
15 #include <mrpt/poses/CPosePDFSOG.h>
19 #include <mrpt/random.h>
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::poses;
24 
25 using namespace mrpt::random;
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
30 CPoseRandomSampler::CPoseRandomSampler() {}
31 CPoseRandomSampler::CPoseRandomSampler(const CPoseRandomSampler& o)
32 {
33  if (o.m_pdf2D)
34  m_pdf2D.reset(dynamic_cast<const CPosePDF*>(o.m_pdf2D->clone()));
35  if (o.m_pdf3D)
36  m_pdf3D.reset(dynamic_cast<const CPose3DPDF*>(o.m_pdf3D->clone()));
37  m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
38  m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
39  m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
40  m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
41 }
42 
43 CPoseRandomSampler& CPoseRandomSampler::operator=(const CPoseRandomSampler& o)
44 {
45  if (&o == this) return *this;
46  m_pdf2D.reset();
47  m_pdf3D.reset();
48  if (o.m_pdf2D)
49  m_pdf2D.reset(dynamic_cast<const CPosePDF*>(o.m_pdf2D->clone()));
50  if (o.m_pdf3D)
51  m_pdf3D.reset(dynamic_cast<const CPose3DPDF*>(o.m_pdf3D->clone()));
52  m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
53  m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
54  m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
55  m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
56  return *this;
57 }
58 
59 CPoseRandomSampler::CPoseRandomSampler(CPoseRandomSampler&& o)
60  : m_pdf2D(nullptr), m_pdf3D(nullptr)
61 {
62  if (o.m_pdf2D)
63  {
64  m_pdf2D = std::move(o.m_pdf2D);
65  o.m_pdf2D = nullptr;
66  }
67  if (o.m_pdf3D)
68  {
69  m_pdf3D = std::move(o.m_pdf3D);
70  o.m_pdf3D = nullptr;
71  }
72  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
73  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
74  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
75  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
76 }
78 {
79  if (this == &o) return *this;
80  this->clear();
81  if (o.m_pdf2D)
82  {
83  m_pdf2D = std::move(o.m_pdf2D);
84  o.m_pdf2D = nullptr;
85  }
86  if (o.m_pdf3D)
87  {
88  m_pdf3D = std::move(o.m_pdf3D);
89  o.m_pdf3D = nullptr;
90  }
91  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
92  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
93  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
94  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
95  return *this;
96 }
97 
98 /*---------------------------------------------------------------
99  clear
100  ---------------------------------------------------------------*/
102 {
103  m_pdf2D.reset();
104  m_pdf3D.reset();
105 }
106 
107 /*---------------------------------------------------------------
108  setPosePDF
109  ---------------------------------------------------------------*/
111 {
112  MRPT_START
113 
114  clear();
115  m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf->clone()));
116 
117  // According to the PDF type:
118  if (IS_CLASS(m_pdf2D.get(), CPosePDFGaussian))
119  {
120  const CPosePDFGaussian* gPdf =
121  static_cast<const CPosePDFGaussian*>(pdf);
122  const CMatrixDouble33& cov = gPdf->cov;
123 
124  m_fastdraw_gauss_M_2D = gPdf->mean;
125 
126  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
127  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
128  * eigenvectors and the diagonal matrix D contains the eigenvalues
129  * as diagonal elements, sorted in <i>ascending</i> order.
130  */
131  CMatrixDouble33 D;
132  cov.eigenVectors(m_fastdraw_gauss_Z3, D);
133 
134  // Scale eigenvectors with eigenvalues:
135  D = D.array().sqrt().matrix();
137  }
138  else if (IS_CLASS(m_pdf2D.get(), CPosePDFParticles))
139  {
140  return; // Nothing to prepare.
141  }
142  else
143  {
145  "Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className);
146  }
147 
148  MRPT_END
149 }
150 
151 /*---------------------------------------------------------------
152  setPosePDF
153  ---------------------------------------------------------------*/
155 {
156  MRPT_START
157 
158  clear();
159  m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf->clone()));
160 
161  // According to the PDF type:
162  if (IS_CLASS(m_pdf3D.get(), CPose3DPDFGaussian))
163  {
164  const CPose3DPDFGaussian* gPdf =
165  static_cast<const CPose3DPDFGaussian*>(pdf);
166  const CMatrixDouble66& cov = gPdf->cov;
167 
168  m_fastdraw_gauss_M_3D = gPdf->mean;
169 
170  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
171  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
172  * eigenvectors and the diagonal matrix D contains the eigenvalues
173  * as diagonal elements, sorted in <i>ascending</i> order.
174  */
175  CMatrixDouble66 D;
176  cov.eigenVectors(m_fastdraw_gauss_Z6, D);
177 
178  // Scale eigenvectors with eigenvalues:
179  D = D.array().sqrt().matrix();
181  }
182  else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFParticles))
183  {
184  return; // Nothing to prepare.
185  }
186  else
187  {
189  "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
190  }
191 
192  MRPT_END
193 }
194 
196 {
197  setPosePDF(pdf.get());
198 }
199 
201 {
202  setPosePDF(pdf.get());
203 }
204 
205 /*---------------------------------------------------------------
206  drawSample
207  ---------------------------------------------------------------*/
209 {
210  MRPT_START
211 
212  if (m_pdf2D)
213  {
214  do_sample_2D(p);
215  }
216  else if (m_pdf3D)
217  {
218  CPose3D q;
219  do_sample_3D(q);
220  p.x(q.x());
221  p.y(q.y());
222  p.phi(q.yaw());
223  }
224  else
225  THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
226 
227  return p;
228  MRPT_END
229 }
230 
231 /*---------------------------------------------------------------
232  drawSample
233  ---------------------------------------------------------------*/
235 {
236  MRPT_START
237 
238  if (m_pdf2D)
239  {
240  CPose2D q;
241  do_sample_2D(q);
242  p.setFromValues(q.x(), q.y(), 0, q.phi(), 0, 0);
243  }
244  else if (m_pdf3D)
245  {
246  do_sample_3D(p);
247  }
248  else
249  THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
250 
251  return p;
252  MRPT_END
253 }
254 
255 /*---------------------------------------------------------------
256  do_sample_2D: Sample from a 2D PDF
257  ---------------------------------------------------------------*/
259 {
260  MRPT_START
261  ASSERT_(m_pdf2D);
262 
263  // According to the PDF type:
264  if (IS_CLASS(m_pdf2D.get(), CPosePDFGaussian))
265  {
266  // ------------------------------
267  // A single gaussian:
268  // ------------------------------
269  CVectorDouble rndVector(3);
270  rndVector.setZero();
271  for (size_t i = 0; i < 3; i++)
272  {
274  for (size_t d = 0; d < 3; d++)
275  rndVector[d] += (m_fastdraw_gauss_Z3.get_unsafe(d, i) * rnd);
276  }
277 
278  p.x(m_fastdraw_gauss_M_2D.x() + rndVector[0]);
279  p.y(m_fastdraw_gauss_M_2D.y() + rndVector[1]);
280  p.phi(m_fastdraw_gauss_M_2D.phi() + rndVector[2]);
281  p.normalizePhi();
282  }
283  else if (IS_CLASS(m_pdf2D.get(), CPosePDFSOG))
284  {
285  // -------------------------------------
286  // SOG
287  // -------------------------------------
288  THROW_EXCEPTION("TODO");
289  }
290  else if (IS_CLASS(m_pdf2D.get(), CPosePDFParticles))
291  {
292  // -------------------------------------
293  // Particles: just sample as usual
294  // -------------------------------------
295  const CPosePDFParticles* pdf =
296  static_cast<const CPosePDFParticles*>(m_pdf2D.get());
297  pdf->drawSingleSample(p);
298  }
299  else
301  "Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className);
302 
303  MRPT_END
304 }
305 
306 /*---------------------------------------------------------------
307  do_sample_3D: Sample from a 3D PDF
308  ---------------------------------------------------------------*/
310 {
311  MRPT_START
312  ASSERT_(m_pdf3D);
313 
314  // According to the PDF type:
315  if (IS_CLASS(m_pdf3D.get(), CPose3DPDFGaussian))
316  {
317  // ------------------------------
318  // A single gaussian:
319  // ------------------------------
320  CVectorDouble rndVector(6);
321  rndVector.setZero();
322  for (size_t i = 0; i < 6; i++)
323  {
325  for (size_t d = 0; d < 6; d++)
326  rndVector[d] += (m_fastdraw_gauss_Z6.get_unsafe(d, i) * rnd);
327  }
328 
329  p.setFromValues(
330  m_fastdraw_gauss_M_3D.x() + rndVector[0],
331  m_fastdraw_gauss_M_3D.y() + rndVector[1],
332  m_fastdraw_gauss_M_3D.z() + rndVector[2],
333  m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
334  m_fastdraw_gauss_M_3D.pitch() + rndVector[4],
335  m_fastdraw_gauss_M_3D.roll() + rndVector[5]);
336  }
337  else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFSOG))
338  {
339  // -------------------------------------
340  // SOG
341  // -------------------------------------
342  THROW_EXCEPTION("TODO");
343  }
344  else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFParticles))
345  {
346  // -------------------------------------
347  // Particles: just sample as usual
348  // -------------------------------------
349  const CPose3DPDFParticles* pdf =
350  static_cast<const CPose3DPDFParticles*>(m_pdf3D.get());
351  pdf->drawSingleSample(p);
352  }
353  else
355  "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
356 
357  MRPT_END
358 }
359 
360 /*---------------------------------------------------------------
361  isPrepared
362  ---------------------------------------------------------------*/
363 bool CPoseRandomSampler::isPrepared() const { return m_pdf2D || m_pdf3D; }
364 /*---------------------------------------------------------------
365  getOriginalPDFCov2D
366  ---------------------------------------------------------------*/
368 {
369  MRPT_START
370  ASSERT_(this->isPrepared());
371 
372  if (m_pdf2D)
373  {
374  m_pdf2D->getCovariance(cov3x3);
375  }
376  else
377  {
378  ASSERT_(m_pdf3D);
379 
381  P.copyFrom(*m_pdf3D);
382  cov3x3 = P.cov;
383  }
384 
385  MRPT_END
386 }
387 
388 /*---------------------------------------------------------------
389  getOriginalPDFCov3D
390  ---------------------------------------------------------------*/
392 {
393  MRPT_START
394  ASSERT_(this->isPrepared());
395 
396  if (m_pdf2D)
397  {
399  P.copyFrom(*m_pdf2D);
400  cov6x6 = P.cov;
401  }
402  else
403  {
404  ASSERT_(m_pdf3D);
405  m_pdf3D->getCovariance(cov6x6);
406  }
407 
408  MRPT_END
409 }
410 
411 /*---------------------------------------------------------------
412  getSamplingMean2D
413  ---------------------------------------------------------------*/
415 {
416  MRPT_START
417  ASSERT_(this->isPrepared());
418 
419  if (m_pdf2D)
420  out_mean = m_fastdraw_gauss_M_2D;
421  else
422  out_mean = CPose2D(m_fastdraw_gauss_M_3D);
423 
424  return out_mean;
425  MRPT_END
426 }
427 
428 /*---------------------------------------------------------------
429  getSamplingMean3D
430  ---------------------------------------------------------------*/
432 {
433  MRPT_START
434  ASSERT_(this->isPrepared());
435 
436  if (m_pdf3D)
437  out_mean = m_fastdraw_gauss_M_3D;
438  else
439  out_mean = CPose3D(m_fastdraw_gauss_M_2D);
440 
441  return out_mean;
442  MRPT_END
443 }
A namespace of pseudo-random numbers generators of diferent distributions.
CPose2D & getSamplingMean2D(CPose2D &out_mean) const
If the object has been loaded with setPosePDF this method returns the 2D pose mean samples will be dr...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define MRPT_START
Definition: exceptions.h:262
CPose2D mean
The mean value.
CPose3D mean
The mean value.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:534
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
std::unique_ptr< const CPose3DPDF > m_pdf3D
A local copy of the PDF.
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void getOriginalPDFCov3D(mrpt::math::CMatrixDouble66 &cov6x6) const
Retrieves the 6x6 covariance of the original PDF in .
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
CPose3D & getSamplingMean3D(CPose3D &out_mean) const
If the object has been loaded with setPosePDF this method returns the 3D pose mean samples will be dr...
void do_sample_3D(CPose3D &p) const
Used internally: sample from m_pdf3D.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void clear()
Clear internal pdf.
void setPosePDF(const CPosePDF *pdf)
This method must be called to select the PDF from which to draw samples.
virtual CObject * clone() const =0
Returns a deep copy (clone) of the object, indepently of its class.
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
void do_sample_2D(CPose2D &p) const
Used internally: sample from m_pdf2D.
mrpt::math::CMatrixDouble33 m_fastdraw_gauss_Z3
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:540
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
#define MRPT_END
Definition: exceptions.h:266
CPoseRandomSampler & operator=(const CPoseRandomSampler &o)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
std::unique_ptr< const CPosePDF > m_pdf2D
A local copy of the PDF.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
mrpt::math::CMatrixDouble66 m_fastdraw_gauss_Z6
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLfloat GLfloat p
Definition: glext.h:6305
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:42
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019