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