MRPT  2.0.1
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-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 
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 using namespace mrpt::random;
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
30 CPoseRandomSampler::CPoseRandomSampler() = default;
31 CPoseRandomSampler::CPoseRandomSampler(const CPoseRandomSampler& o)
32 {
33  *this = o;
34 }
35 
36 CPoseRandomSampler& CPoseRandomSampler::operator=(const CPoseRandomSampler& o)
37 {
38  if (&o == this) return *this;
39  m_pdf2D.reset();
40  m_pdf3D.reset();
41  if (o.m_pdf2D) m_pdf2D.reset(dynamic_cast<CPosePDF*>(o.m_pdf2D->clone()));
42  if (o.m_pdf3D) m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(o.m_pdf3D->clone()));
43  m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
44  m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
45  m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
46  m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
47  return *this;
48 }
49 
50 CPoseRandomSampler::CPoseRandomSampler(CPoseRandomSampler&& o)
51  : m_pdf2D(nullptr), m_pdf3D(nullptr)
52 {
53  if (o.m_pdf2D)
54  {
55  m_pdf2D = std::move(o.m_pdf2D);
56  o.m_pdf2D = nullptr;
57  }
58  if (o.m_pdf3D)
59  {
60  m_pdf3D = std::move(o.m_pdf3D);
61  o.m_pdf3D = nullptr;
62  }
63  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
64  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
65  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
66  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
67 }
69 {
70  if (this == &o) return *this;
71  this->clear();
72  if (o.m_pdf2D)
73  {
74  m_pdf2D = std::move(o.m_pdf2D);
75  o.m_pdf2D = nullptr;
76  }
77  if (o.m_pdf3D)
78  {
79  m_pdf3D = std::move(o.m_pdf3D);
80  o.m_pdf3D = nullptr;
81  }
82  m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
83  m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
84  m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
85  m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
86  return *this;
87 }
88 
89 /*---------------------------------------------------------------
90  clear
91  ---------------------------------------------------------------*/
93 {
94  m_pdf2D.reset();
95  m_pdf3D.reset();
96 }
97 
98 /*---------------------------------------------------------------
99  setPosePDF
100  ---------------------------------------------------------------*/
102 {
103  MRPT_START
104 
105  clear();
106  m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf.clone()));
107 
108  // According to the PDF type:
109  if (IS_CLASS(pdf, CPosePDFGaussian))
110  {
111  const auto& gPdf = dynamic_cast<const CPosePDFGaussian&>(pdf);
112  const CMatrixDouble33& cov = gPdf.cov;
113 
114  m_fastdraw_gauss_M_2D = gPdf.mean;
115 
116  std::vector<double> eigVals;
118 
119  // Scale eigenvectors with eigenvalues:
121  D.setDiagonal(eigVals);
122  D = D.asEigen().array().sqrt().matrix();
124  }
125  else if (IS_CLASS(pdf, CPosePDFParticles))
126  {
127  return; // Nothing to prepare.
128  }
129  else
130  {
132  "Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className);
133  }
134 
135  MRPT_END
136 }
137 
138 /*---------------------------------------------------------------
139  setPosePDF
140  ---------------------------------------------------------------*/
142 {
143  MRPT_START
144 
145  clear();
146  m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf.clone()));
147 
148  // According to the PDF type:
149  if (IS_CLASS(pdf, CPose3DPDFGaussian))
150  {
151  const auto& gPdf = dynamic_cast<const CPose3DPDFGaussian&>(pdf);
152  const CMatrixDouble66& cov = gPdf.cov;
153 
154  m_fastdraw_gauss_M_3D = gPdf.mean;
155 
156  std::vector<double> eigVals;
158 
159  // Scale eigenvectors with eigenvalues:
161  D.setDiagonal(eigVals);
162 
163  // Scale eigenvectors with eigenvalues:
164  D = D.asEigen().array().sqrt().matrix();
166  }
167  else if (IS_CLASS(pdf, CPose3DPDFParticles))
168  {
169  return; // Nothing to prepare.
170  }
171  else
172  {
174  "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
175  }
176 
177  MRPT_END
178 }
179 
180 /*---------------------------------------------------------------
181  drawSample
182  ---------------------------------------------------------------*/
184 {
185  MRPT_START
186 
187  if (m_pdf2D)
188  {
189  do_sample_2D(p);
190  }
191  else if (m_pdf3D)
192  {
193  CPose3D q;
194  do_sample_3D(q);
195  p.x(q.x());
196  p.y(q.y());
197  p.phi(q.yaw());
198  }
199  else
200  THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
201 
202  return p;
203  MRPT_END
204 }
205 
206 /*---------------------------------------------------------------
207  drawSample
208  ---------------------------------------------------------------*/
210 {
211  MRPT_START
212 
213  if (m_pdf2D)
214  {
215  CPose2D q;
216  do_sample_2D(q);
217  p.setFromValues(q.x(), q.y(), 0, q.phi(), 0, 0);
218  }
219  else if (m_pdf3D)
220  {
221  do_sample_3D(p);
222  }
223  else
224  THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");
225 
226  return p;
227  MRPT_END
228 }
229 
230 /*---------------------------------------------------------------
231  do_sample_2D: Sample from a 2D PDF
232  ---------------------------------------------------------------*/
234 {
235  MRPT_START
236  ASSERT_(m_pdf2D);
237 
238  // According to the PDF type:
240  {
241  // ------------------------------
242  // A single gaussian:
243  // ------------------------------
244  CVectorDouble rndVector(3);
245  rndVector.setZero();
246  for (size_t i = 0; i < 3; i++)
247  {
249  for (size_t d = 0; d < 3; d++)
250  rndVector[d] += (m_fastdraw_gauss_Z3(d, i) * rnd);
251  }
252 
253  p.x(m_fastdraw_gauss_M_2D.x() + rndVector[0]);
254  p.y(m_fastdraw_gauss_M_2D.y() + rndVector[1]);
255  p.phi(m_fastdraw_gauss_M_2D.phi() + rndVector[2]);
256  p.normalizePhi();
257  }
258  else if (IS_CLASS(*m_pdf2D, CPosePDFSOG))
259  {
260  // -------------------------------------
261  // SOG
262  // -------------------------------------
263  THROW_EXCEPTION("TODO");
264  }
265  else if (IS_CLASS(*m_pdf2D, CPosePDFParticles))
266  {
267  // -------------------------------------
268  // Particles: just sample as usual
269  // -------------------------------------
270  const auto& pdf = dynamic_cast<const CPosePDFParticles&>(*m_pdf2D);
271  pdf.drawSingleSample(p);
272  }
273  else
275  "Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className);
276 
277  MRPT_END
278 }
279 
280 /*---------------------------------------------------------------
281  do_sample_3D: Sample from a 3D PDF
282  ---------------------------------------------------------------*/
284 {
285  MRPT_START
286  ASSERT_(m_pdf3D);
287 
288  // According to the PDF type:
290  {
291  // ------------------------------
292  // A single gaussian:
293  // ------------------------------
294  CVectorDouble rndVector(6);
295  rndVector.setZero();
296  for (size_t i = 0; i < 6; i++)
297  {
299  for (size_t d = 0; d < 6; d++)
300  rndVector[d] += (m_fastdraw_gauss_Z6(d, i) * rnd);
301  }
302 
303  p.setFromValues(
304  m_fastdraw_gauss_M_3D.x() + rndVector[0],
305  m_fastdraw_gauss_M_3D.y() + rndVector[1],
306  m_fastdraw_gauss_M_3D.z() + rndVector[2],
307  m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
308  m_fastdraw_gauss_M_3D.pitch() + rndVector[4],
309  m_fastdraw_gauss_M_3D.roll() + rndVector[5]);
310  }
311  else if (IS_CLASS(*m_pdf3D, CPose3DPDFSOG))
312  {
313  // -------------------------------------
314  // SOG
315  // -------------------------------------
316  THROW_EXCEPTION("TODO");
317  }
319  {
320  // -------------------------------------
321  // Particles: just sample as usual
322  // -------------------------------------
323  const auto& pdf = dynamic_cast<const CPose3DPDFParticles&>(*m_pdf3D);
324  pdf.drawSingleSample(p);
325  }
326  else
328  "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
329 
330  MRPT_END
331 }
332 
333 /*---------------------------------------------------------------
334  isPrepared
335  ---------------------------------------------------------------*/
336 bool CPoseRandomSampler::isPrepared() const { return m_pdf2D || m_pdf3D; }
337 /*---------------------------------------------------------------
338  getOriginalPDFCov2D
339  ---------------------------------------------------------------*/
341 {
342  MRPT_START
343  ASSERT_(this->isPrepared());
344 
345  if (m_pdf2D)
346  {
347  m_pdf2D->getCovariance(cov3x3);
348  }
349  else
350  {
351  ASSERT_(m_pdf3D);
352 
354  P.copyFrom(*m_pdf3D);
355  cov3x3 = P.cov;
356  }
357 
358  MRPT_END
359 }
360 
361 /*---------------------------------------------------------------
362  getOriginalPDFCov3D
363  ---------------------------------------------------------------*/
365 {
366  MRPT_START
367  ASSERT_(this->isPrepared());
368 
369  if (m_pdf2D)
370  {
372  P.copyFrom(*m_pdf2D);
373  cov6x6 = P.cov;
374  }
375  else
376  {
377  ASSERT_(m_pdf3D);
378  m_pdf3D->getCovariance(cov6x6);
379  }
380 
381  MRPT_END
382 }
383 
384 /*---------------------------------------------------------------
385  getSamplingMean2D
386  ---------------------------------------------------------------*/
388 {
389  MRPT_START
390  ASSERT_(this->isPrepared());
391 
392  if (m_pdf2D)
393  out_mean = m_fastdraw_gauss_M_2D;
394  else
395  out_mean = CPose2D(m_fastdraw_gauss_M_3D);
396 
397  return out_mean;
398  MRPT_END
399 }
400 
401 /*---------------------------------------------------------------
402  getSamplingMean3D
403  ---------------------------------------------------------------*/
405 {
406  MRPT_START
407  ASSERT_(this->isPrepared());
408 
409  if (m_pdf3D)
410  out_mean = m_fastdraw_gauss_M_3D;
411  else
412  out_mean = CPose3D(m_fastdraw_gauss_M_2D);
413 
414  return out_mean;
415  MRPT_END
416 }
417 
419  mrpt::math::CMatrixDouble& cov3x3) const
420 {
422  this->getOriginalPDFCov2D(M);
423  cov3x3 = mrpt::math::CMatrixDouble(M);
424 }
425 
427  mrpt::math::CMatrixDouble& cov6x6) const
428 {
430  this->getOriginalPDFCov3D(M);
431  cov6x6 = mrpt::math::CMatrixDouble(M);
432 }
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) ...
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:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
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.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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:146
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:149
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:558
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:85
#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
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.
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
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:333
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020