Main MRPT website > C++ reference for MRPT 1.5.6
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-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 #include "base-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 using namespace mrpt::utils;
25 using namespace mrpt::random;
26 
27 
28 /*---------------------------------------------------------------
29  Constructor
30  ---------------------------------------------------------------*/
32  m_pdf2D(NULL),
33  m_pdf3D(NULL),
34  m_fastdraw_gauss_Z3(),
35  m_fastdraw_gauss_Z6(),
36  m_fastdraw_gauss_M_2D(),
37  m_fastdraw_gauss_M_3D()
38 {
39 }
40 
41 /*---------------------------------------------------------------
42  Destructor
43  ---------------------------------------------------------------*/
45 {
46  clear();
47 }
48 
50  m_pdf2D(NULL),
51  m_pdf3D(NULL)
52 {
53  if (o.m_pdf2D) m_pdf2D = dynamic_cast<CPosePDF*>(o.m_pdf2D->duplicate());
54  if (o.m_pdf3D) m_pdf3D = dynamic_cast<CPose3DPDF*>(o.m_pdf3D->duplicate());
59 
60 }
62 {
63  if (this == &o) return *this;
64  this->clear();
65  if (o.m_pdf2D) m_pdf2D = dynamic_cast<CPosePDF*>(o.m_pdf2D->duplicate()); else m_pdf2D = NULL;
66  if (o.m_pdf3D) m_pdf3D = dynamic_cast<CPose3DPDF*>(o.m_pdf3D->duplicate()); else m_pdf3D = NULL;
71  return *this;
72 }
73 #if MRPT_HAS_CXX11
75  m_pdf2D(nullptr),
76  m_pdf3D(nullptr)
77 {
78  if (o.m_pdf2D)
79  {
80  m_pdf2D = o.m_pdf2D;
81  o.m_pdf2D = nullptr;
82  }
83  if (o.m_pdf3D)
84  {
85  m_pdf3D = o.m_pdf3D;
86  o.m_pdf3D = nullptr;
87  }
88  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
89  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
90  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
91  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
92 }
94 {
95  if (this == &o) return *this;
96  this->clear();
97  if (o.m_pdf2D)
98  {
99  m_pdf2D = o.m_pdf2D;
100  o.m_pdf2D = nullptr;
101  }
102  if (o.m_pdf3D)
103  {
104  m_pdf3D = o.m_pdf3D;
105  o.m_pdf3D = nullptr;
106  }
107  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
108  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
109  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
110  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
111  return *this;
112 }
113 #endif
114 
115 
116 
117 /*---------------------------------------------------------------
118  clear
119  ---------------------------------------------------------------*/
121 {
124 }
125 
126 /*---------------------------------------------------------------
127  setPosePDF
128  ---------------------------------------------------------------*/
130 {
131  MRPT_START
132 
133  clear();
134  m_pdf2D = dynamic_cast<CPosePDF*>( pdf->duplicate() );
135 
136  // According to the PDF type:
138  {
139  const CPosePDFGaussian* gPdf = static_cast<const CPosePDFGaussian*>(m_pdf2D);
140  const CMatrixDouble33 &cov = gPdf->cov;
141 
142  m_fastdraw_gauss_M_2D = gPdf->mean;
143 
144  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
145  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
146  * eigenvectors and the diagonal matrix D contains the eigenvalues
147  * as diagonal elements, sorted in <i>ascending</i> order.
148  */
149  CMatrixDouble33 D;
150  cov.eigenVectors( m_fastdraw_gauss_Z3, D );
151 
152  // Scale eigenvectors with eigenvalues:
153  D = D.array().sqrt().matrix();
155  }
156  else
158  {
159  return; // Nothing to prepare.
160  }
161  else
162  {
163  THROW_EXCEPTION_FMT("Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className );
164  }
165 
166 
167  MRPT_END
168 }
169 
170 
171 /*---------------------------------------------------------------
172  setPosePDF
173  ---------------------------------------------------------------*/
175 {
176  MRPT_START
177 
178  clear();
179  m_pdf3D = dynamic_cast<CPose3DPDF*>( pdf->duplicate() );
180 
181  // According to the PDF type:
183  {
184  const CPose3DPDFGaussian* gPdf = dynamic_cast<const CPose3DPDFGaussian*>(m_pdf3D);
185  const CMatrixDouble66 &cov = gPdf->cov;
186 
187  m_fastdraw_gauss_M_3D = gPdf->mean;
188 
189  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
190  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
191  * eigenvectors and the diagonal matrix D contains the eigenvalues
192  * as diagonal elements, sorted in <i>ascending</i> order.
193  */
194  CMatrixDouble66 D;
195  cov.eigenVectors( m_fastdraw_gauss_Z6, D );
196 
197  // Scale eigenvectors with eigenvalues:
198  D = D.array().sqrt().matrix();
200  }
201  else
203  {
204  return; // Nothing to prepare.
205  }
206  else
207  {
208  THROW_EXCEPTION_FMT("Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className );
209  }
210 
211  MRPT_END
212 }
213 
214 void CPoseRandomSampler::setPosePDF( const CPose3DPDFPtr &pdf ) {
215  setPosePDF(pdf.pointer());
216 }
217 
218 void CPoseRandomSampler::setPosePDF( const CPosePDFPtr &pdf ) {
219  setPosePDF(pdf.pointer());
220 }
221 
222 /*---------------------------------------------------------------
223  drawSample
224  ---------------------------------------------------------------*/
226 {
227  MRPT_START
228 
229  if (m_pdf2D)
230  {
231  do_sample_2D(p);
232  }
233  else if (m_pdf3D)
234  {
235  CPose3D q;
236  do_sample_3D(q);
237  p.x(q.x());
238  p.y(q.y());
239  p.phi(q.yaw());
240  }
241  else THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
242 
243  return p;
244  MRPT_END
245 }
246 
247 /*---------------------------------------------------------------
248  drawSample
249  ---------------------------------------------------------------*/
251 {
252  MRPT_START
253 
254  if (m_pdf2D)
255  {
256  CPose2D q;
257  do_sample_2D(q);
258  p.setFromValues(q.x(),q.y(),0,q.phi(),0,0);
259  }
260  else if (m_pdf3D)
261  {
262  do_sample_3D(p);
263  }
264  else THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
265 
266  return p;
267  MRPT_END
268 }
269 
270 
271 /*---------------------------------------------------------------
272  do_sample_2D: Sample from a 2D PDF
273  ---------------------------------------------------------------*/
275 {
276  MRPT_START
277  ASSERT_(m_pdf2D);
278 
279  // According to the PDF type:
281  {
282  // ------------------------------
283  // A single gaussian:
284  // ------------------------------
285  CVectorDouble rndVector(3);
286  rndVector.setZero();
287  for (size_t i=0;i<3;i++)
288  {
290  for (size_t d=0;d<3;d++)
291  rndVector[d]+= ( m_fastdraw_gauss_Z3.get_unsafe(d,i)*rnd );
292  }
293 
294  p.x( m_fastdraw_gauss_M_2D.x() + rndVector[0] );
295  p.y( m_fastdraw_gauss_M_2D.y() + rndVector[1] );
296  p.phi( m_fastdraw_gauss_M_2D.phi() + rndVector[2] );
297  p.normalizePhi();
298  }
299  else
300  if ( IS_CLASS(m_pdf2D,CPosePDFSOG ) )
301  {
302  // -------------------------------------
303  // SOG
304  // -------------------------------------
305  THROW_EXCEPTION("TODO");
306  }
307  else
309  {
310  // -------------------------------------
311  // Particles: just sample as usual
312  // -------------------------------------
313  const CPosePDFParticles* pdf = dynamic_cast<const CPosePDFParticles*>(m_pdf2D);
314  pdf->drawSingleSample(p);
315  }
316  else
317  THROW_EXCEPTION_FMT("Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className );
318 
319  MRPT_END
320 }
321 
322 /*---------------------------------------------------------------
323  do_sample_3D: Sample from a 3D PDF
324  ---------------------------------------------------------------*/
326 {
327  MRPT_START
328  ASSERT_(m_pdf3D);
329 
330  // According to the PDF type:
332  {
333  // ------------------------------
334  // A single gaussian:
335  // ------------------------------
336  CVectorDouble rndVector(6);
337  rndVector.setZero();
338  for (size_t i=0;i<6;i++)
339  {
341  for (size_t d=0;d<6;d++)
342  rndVector[d]+= ( m_fastdraw_gauss_Z6.get_unsafe(d,i)*rnd );
343  }
344 
345  p.setFromValues(
346  m_fastdraw_gauss_M_3D.x() + rndVector[0],
347  m_fastdraw_gauss_M_3D.y() + rndVector[1],
348  m_fastdraw_gauss_M_3D.z() + rndVector[2],
349  m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
350  m_fastdraw_gauss_M_3D.pitch()+ rndVector[4],
351  m_fastdraw_gauss_M_3D.roll() + rndVector[5] );
352  }
353  else
355  {
356  // -------------------------------------
357  // SOG
358  // -------------------------------------
359  THROW_EXCEPTION("TODO");
360  }
361  else
363  {
364  // -------------------------------------
365  // Particles: just sample as usual
366  // -------------------------------------
367  const CPose3DPDFParticles* pdf = static_cast<const CPose3DPDFParticles*>(m_pdf3D);
368  pdf->drawSingleSample(p);
369  }
370  else
371  THROW_EXCEPTION_FMT("Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className );
372 
373  MRPT_END
374 }
375 
376 /*---------------------------------------------------------------
377  isPrepared
378  ---------------------------------------------------------------*/
380 {
381  return m_pdf2D || m_pdf3D;
382 }
383 
384 /*---------------------------------------------------------------
385  getOriginalPDFCov2D
386  ---------------------------------------------------------------*/
388 {
389  MRPT_START
390  ASSERT_(this->isPrepared())
391 
392  if (m_pdf2D)
393  {
394  m_pdf2D->getCovariance( cov3x3 );
395  }
396  else
397  {
398  ASSERT_(m_pdf3D);
399 
401  P.copyFrom(*m_pdf3D);
402  cov3x3 = P.cov;
403  }
404 
405  MRPT_END
406 }
407 
408 /*---------------------------------------------------------------
409  getOriginalPDFCov3D
410  ---------------------------------------------------------------*/
412 {
413  MRPT_START
414  ASSERT_(this->isPrepared())
415 
416  if (m_pdf2D)
417  {
419  P.copyFrom(*m_pdf2D);
420  cov6x6 = P.cov;
421  }
422  else
423  {
424  ASSERT_(m_pdf3D);
425  m_pdf3D->getCovariance( cov6x6 );
426  }
427 
428  MRPT_END
429 }
430 
431 /*---------------------------------------------------------------
432  getSamplingMean2D
433  ---------------------------------------------------------------*/
435 {
436  MRPT_START
437  ASSERT_(this->isPrepared())
438 
439  if (m_pdf2D)
440  out_mean = m_fastdraw_gauss_M_2D;
441  else out_mean = CPose2D( m_fastdraw_gauss_M_3D );
442 
443  return out_mean;
444  MRPT_END
445 }
446 
447 /*---------------------------------------------------------------
448  getSamplingMean3D
449  ---------------------------------------------------------------*/
451 {
452  MRPT_START
453  ASSERT_(this->isPrepared())
454 
455  if (m_pdf3D)
456  out_mean = m_fastdraw_gauss_M_3D;
457  else out_mean = CPose3D( m_fastdraw_gauss_M_2D );
458 
459  return out_mean;
460  MRPT_END
461 }
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
CPose2D mean
The mean value.
CPose3D mean
The mean value.
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void getOriginalPDFCov3D(mrpt::math::CMatrixDouble66 &cov6x6) const
Retrieves the 6x6 covariance of the original PDF in .
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
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:35
CPosePDF * m_pdf2D
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.
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void clear()
Clear internal pdf.
#define MRPT_END
void setPosePDF(const CPosePDF *pdf)
This method must be called to select the PDF from which to draw samples.
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:135
mrpt::math::CMatrixDouble33 m_fastdraw_gauss_Z3
void do_sample_2D(CPose2D &p) const
Used internally: sample from m_pdf2D.
void drawSingleSample(CPose3D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLfloat GLfloat p
Definition: glew.h:10113
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
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...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
CPose3DPDF * m_pdf3D
A local copy of the PDF.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define MRPT_START
virtual CObject * duplicate() const =0
Returns a copy of the object, indepently of its class.
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
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:254
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
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...
CPoseRandomSampler & operator=(const CPoseRandomSampler &o)
#define ASSERT_(f)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
void do_sample_3D(CPose3D &p) const
Used internally: sample from m_pdf3D.
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
mrpt::math::CMatrixDouble66 m_fastdraw_gauss_Z6
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
const char * className
Definition: CObject.h:48
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:287
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void drawSingleSample(CPose2D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018