MRPT  1.9.9
CPoseRandomSampler.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-2019, 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 
17 #include <mrpt/poses/CPosePDFSOG.h>
19 #include <mrpt/random.h>
20 #include <Eigen/Dense>
21 
22 using namespace mrpt;
23 using namespace mrpt::math;
24 using namespace mrpt::poses;
25 
26 using namespace mrpt::random;
27 
28 /*---------------------------------------------------------------
29  Constructor
30  ---------------------------------------------------------------*/
31 CPoseRandomSampler::CPoseRandomSampler() = default;
32 CPoseRandomSampler::CPoseRandomSampler(const CPoseRandomSampler& o)
33 {
34  *this = o;
35 }
36 
37 CPoseRandomSampler& CPoseRandomSampler::operator=(const CPoseRandomSampler& o)
38 {
39  if (&o == this) return *this;
40  m_pdf2D.reset();
41  m_pdf3D.reset();
42  if (o.m_pdf2D) m_pdf2D.reset(dynamic_cast<CPosePDF*>(o.m_pdf2D->clone()));
43  if (o.m_pdf3D) m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(o.m_pdf3D->clone()));
44  m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
45  m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
46  m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
47  m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
48  return *this;
49 }
50 
51 CPoseRandomSampler::CPoseRandomSampler(CPoseRandomSampler&& o)
52  : m_pdf2D(nullptr), m_pdf3D(nullptr)
53 {
54  if (o.m_pdf2D)
55  {
56  m_pdf2D = std::move(o.m_pdf2D);
57  o.m_pdf2D = nullptr;
58  }
59  if (o.m_pdf3D)
60  {
61  m_pdf3D = std::move(o.m_pdf3D);
62  o.m_pdf3D = nullptr;
63  }
64  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
65  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
66  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
67  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
68 }
70 {
71  if (this == &o) return *this;
72  this->clear();
73  if (o.m_pdf2D)
74  {
75  m_pdf2D = std::move(o.m_pdf2D);
76  o.m_pdf2D = nullptr;
77  }
78  if (o.m_pdf3D)
79  {
80  m_pdf3D = std::move(o.m_pdf3D);
81  o.m_pdf3D = nullptr;
82  }
83  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
84  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
85  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
86  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
87  return *this;
88 }
89 
90 /*---------------------------------------------------------------
91  clear
92  ---------------------------------------------------------------*/
94 {
95  m_pdf2D.reset();
96  m_pdf3D.reset();
97 }
98 
99 /*---------------------------------------------------------------
100  setPosePDF
101  ---------------------------------------------------------------*/
103 {
104  MRPT_START
105 
106  clear();
107  m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf.clone()));
108 
109  // According to the PDF type:
110  if (IS_CLASS(pdf, CPosePDFGaussian))
111  {
112  const auto& gPdf = dynamic_cast<const CPosePDFGaussian&>(pdf);
113  const CMatrixDouble33& cov = gPdf.cov;
114 
115  m_fastdraw_gauss_M_2D = gPdf.mean;
116 
117  std::vector<double> eigVals;
119 
120  // Scale eigenvectors with eigenvalues:
122  D.setDiagonal(eigVals);
123  D = D.asEigen().array().sqrt().matrix();
125  }
126  else if (IS_CLASS(pdf, CPosePDFParticles))
127  {
128  return; // Nothing to prepare.
129  }
130  else
131  {
133  "Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className);
134  }
135 
136  MRPT_END
137 }
138 
139 /*---------------------------------------------------------------
140  setPosePDF
141  ---------------------------------------------------------------*/
143 {
144  MRPT_START
145 
146  clear();
147  m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf.clone()));
148 
149  // According to the PDF type:
150  if (IS_CLASS(pdf, CPose3DPDFGaussian))
151  {
152  const auto& gPdf = dynamic_cast<const CPose3DPDFGaussian&>(pdf);
153  const CMatrixDouble66& cov = gPdf.cov;
154 
155  m_fastdraw_gauss_M_3D = gPdf.mean;
156 
157  std::vector<double> eigVals;
159 
160  // Scale eigenvectors with eigenvalues:
162  D.setDiagonal(eigVals);
163 
164  // Scale eigenvectors with eigenvalues:
165  D = D.asEigen().array().sqrt().matrix();
167  }
168  else if (IS_CLASS(pdf, CPose3DPDFParticles))
169  {
170  return; // Nothing to prepare.
171  }
172  else
173  {
175  "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
176  }
177 
178  MRPT_END
179 }
180 
181 /*---------------------------------------------------------------
182  drawSample
183  ---------------------------------------------------------------*/
185 {
186  MRPT_START
187 
188  if (m_pdf2D)
189  {
190  do_sample_2D(p);
191  }
192  else if (m_pdf3D)
193  {
194  CPose3D q;
195  do_sample_3D(q);
196  p.x(q.x());
197  p.y(q.y());
198  p.phi(q.yaw());
199  }
200  else
201  THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
202 
203  return p;
204  MRPT_END
205 }
206 
207 /*---------------------------------------------------------------
208  drawSample
209  ---------------------------------------------------------------*/
211 {
212  MRPT_START
213 
214  if (m_pdf2D)
215  {
216  CPose2D q;
217  do_sample_2D(q);
218  p.setFromValues(q.x(), q.y(), 0, q.phi(), 0, 0);
219  }
220  else if (m_pdf3D)
221  {
222  do_sample_3D(p);
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  do_sample_2D: Sample from a 2D PDF
233  ---------------------------------------------------------------*/
235 {
236  MRPT_START
237  ASSERT_(m_pdf2D);
238 
239  // According to the PDF type:
241  {
242  // ------------------------------
243  // A single gaussian:
244  // ------------------------------
245  CVectorDouble rndVector(3);
246  rndVector.setZero();
247  for (size_t i = 0; i < 3; i++)
248  {
250  for (size_t d = 0; d < 3; d++)
251  rndVector[d] += (m_fastdraw_gauss_Z3(d, i) * rnd);
252  }
253 
254  p.x(m_fastdraw_gauss_M_2D.x() + rndVector[0]);
255  p.y(m_fastdraw_gauss_M_2D.y() + rndVector[1]);
256  p.phi(m_fastdraw_gauss_M_2D.phi() + rndVector[2]);
257  p.normalizePhi();
258  }
259  else if (IS_CLASS(*m_pdf2D, CPosePDFSOG))
260  {
261  // -------------------------------------
262  // SOG
263  // -------------------------------------
264  THROW_EXCEPTION("TODO");
265  }
266  else if (IS_CLASS(*m_pdf2D, CPosePDFParticles))
267  {
268  // -------------------------------------
269  // Particles: just sample as usual
270  // -------------------------------------
271  const auto& pdf = dynamic_cast<const CPosePDFParticles&>(*m_pdf2D);
272  pdf.drawSingleSample(p);
273  }
274  else
276  "Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className);
277 
278  MRPT_END
279 }
280 
281 /*---------------------------------------------------------------
282  do_sample_3D: Sample from a 3D PDF
283  ---------------------------------------------------------------*/
285 {
286  MRPT_START
287  ASSERT_(m_pdf3D);
288 
289  // According to the PDF type:
291  {
292  // ------------------------------
293  // A single gaussian:
294  // ------------------------------
295  CVectorDouble rndVector(6);
296  rndVector.setZero();
297  for (size_t i = 0; i < 6; i++)
298  {
300  for (size_t d = 0; d < 6; d++)
301  rndVector[d] += (m_fastdraw_gauss_Z6(d, i) * rnd);
302  }
303 
304  p.setFromValues(
305  m_fastdraw_gauss_M_3D.x() + rndVector[0],
306  m_fastdraw_gauss_M_3D.y() + rndVector[1],
307  m_fastdraw_gauss_M_3D.z() + rndVector[2],
308  m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
309  m_fastdraw_gauss_M_3D.pitch() + rndVector[4],
310  m_fastdraw_gauss_M_3D.roll() + rndVector[5]);
311  }
312  else if (IS_CLASS(*m_pdf3D, CPose3DPDFSOG))
313  {
314  // -------------------------------------
315  // SOG
316  // -------------------------------------
317  THROW_EXCEPTION("TODO");
318  }
320  {
321  // -------------------------------------
322  // Particles: just sample as usual
323  // -------------------------------------
324  const auto& pdf = dynamic_cast<const CPose3DPDFParticles&>(*m_pdf3D);
325  pdf.drawSingleSample(p);
326  }
327  else
329  "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
330 
331  MRPT_END
332 }
333 
334 /*---------------------------------------------------------------
335  isPrepared
336  ---------------------------------------------------------------*/
337 bool CPoseRandomSampler::isPrepared() const { return m_pdf2D || m_pdf3D; }
338 /*---------------------------------------------------------------
339  getOriginalPDFCov2D
340  ---------------------------------------------------------------*/
342 {
343  MRPT_START
344  ASSERT_(this->isPrepared());
345 
346  if (m_pdf2D)
347  {
348  m_pdf2D->getCovariance(cov3x3);
349  }
350  else
351  {
352  ASSERT_(m_pdf3D);
353 
355  P.copyFrom(*m_pdf3D);
356  cov3x3 = P.cov;
357  }
358 
359  MRPT_END
360 }
361 
362 /*---------------------------------------------------------------
363  getOriginalPDFCov3D
364  ---------------------------------------------------------------*/
366 {
367  MRPT_START
368  ASSERT_(this->isPrepared());
369 
370  if (m_pdf2D)
371  {
373  P.copyFrom(*m_pdf2D);
374  cov6x6 = P.cov;
375  }
376  else
377  {
378  ASSERT_(m_pdf3D);
379  m_pdf3D->getCovariance(cov6x6);
380  }
381 
382  MRPT_END
383 }
384 
385 /*---------------------------------------------------------------
386  getSamplingMean2D
387  ---------------------------------------------------------------*/
389 {
390  MRPT_START
391  ASSERT_(this->isPrepared());
392 
393  if (m_pdf2D)
394  out_mean = m_fastdraw_gauss_M_2D;
395  else
396  out_mean = CPose2D(m_fastdraw_gauss_M_3D);
397 
398  return out_mean;
399  MRPT_END
400 }
401 
402 /*---------------------------------------------------------------
403  getSamplingMean3D
404  ---------------------------------------------------------------*/
406 {
407  MRPT_START
408  ASSERT_(this->isPrepared());
409 
410  if (m_pdf3D)
411  out_mean = m_fastdraw_gauss_M_3D;
412  else
413  out_mean = CPose3D(m_fastdraw_gauss_M_2D);
414 
415  return out_mean;
416  MRPT_END
417 }
418 
420  mrpt::math::CMatrixDouble& cov3x3) const
421 {
423  this->getOriginalPDFCov2D(M);
424  cov3x3 = mrpt::math::CMatrixDouble(M);
425 }
426 
428  mrpt::math::CMatrixDouble& cov6x6) const
429 {
431  this->getOriginalPDFCov3D(M);
432  cov6x6 = mrpt::math::CMatrixDouble(M);
433 }
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...
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:34
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:3727
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
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:120
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.
virtual CObject * clone() const =0
Returns a deep copy (clone) of the object, indepently of its class.
void setPosePDF(const CPosePDF &pdf)
This method must be called to select the PDF from which to draw samples.
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.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:133
mrpt::math::CMatrixDouble33 m_fastdraw_gauss_Z3
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
void matProductOf_AB(const Derived &A, const Derived &B)
this = A*B, with A & B of the same type of this.
CMatrixDouble 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
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
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:554
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
#define MRPT_END
Definition: exceptions.h:245
CPoseRandomSampler & operator=(const CPoseRandomSampler &o)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
CPosePDF::Ptr m_pdf2D
A local copy of the PDF.
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:69
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLfloat GLfloat p
Definition: glext.h:6398
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
void setDiagonal(const std::size_t N, const Scalar value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value ...
Definition: MatrixBase.h:34
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.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
CPose3DPDF::Ptr m_pdf3D
A local copy of the PDF.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019