MRPT  1.9.9
CPose3DPDFParticles.h
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 #pragma once
10 
13 #include <mrpt/math/TPose3D.h>
14 #include <mrpt/poses/CPose3DPDF.h>
15 
16 namespace mrpt::poses
17 {
18 /** Declares a class that represents a Probability Density function (PDF) of a
19  * 3D pose
20  *
21  * This class is also the base for the implementation of Monte-Carlo
22  * Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
23  *
24  * See the application "app/pf-localization" for an example of usage.
25  *
26  * \sa CPose3D, CPose3DPDF, CPoseGaussianPDF
27  * \ingroup poses_pdf_grp
28  */
30  : public CPose3DPDF,
32  mrpt::math::TPose3D, mrpt::bayes::particle_storage_mode::VALUE>,
34  CPose3DPDFParticles,
35  mrpt::bayes::CParticleFilterData<
36  mrpt::math::TPose3D,
37  mrpt::bayes::particle_storage_mode::VALUE>::CParticleList>
38 {
40 
41  public:
42  /** Constructor
43  * \param M The number of m_particles.
44  */
45  CPose3DPDFParticles(size_t M = 1);
46 
47  /** Copy operator, translating if necesary (for example, between m_particles
48  * and gaussian representations) */
49  void copyFrom(const CPose3DPDF& o) override;
50 
51  /** Reset the PDF to a single point: All m_particles will be set exactly to
52  * the supplied pose.
53  * \param location The location to set all the m_particles.
54  * \param particlesCount If this is set to 0 the number of m_particles
55  * remains unchanged.
56  * \sa resetUniform */
57  void resetDeterministic(
58  const mrpt::math::TPose3D& location, size_t particlesCount = 0);
59 
60  /** Reset the PDF to an uniformly distributed one, inside of the defined
61  * "cube".
62  *
63  * \param particlesCount New particle count, or leave count unchanged if set
64  * to -1 (default).
65  *
66  * \note Orientations can be outside of the [-pi,pi] range if so desired,
67  * but it must hold `phi_max>=phi_min`.
68  * \sa resetDeterministic
69  * resetAroundSetOfPoses
70  */
71  void resetUniform(
72  const mrpt::math::TPose3D& corner_min,
73  const mrpt::math::TPose3D& corner_max, const int particlesCount = -1);
74 
75  /** Returns an estimate of the pose, (the mean, or mathematical expectation
76  * of the PDF), computed as a weighted average over all m_particles. \sa
77  * getCovariance */
78  void getMean(CPose3D& mean_pose) const override;
79 
80  /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and
81  * the mean, both at once. \sa getMean */
82  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
83 
84  /** Returns the pose of the i'th particle */
86 
87  /** Save PDF's m_particles to a text file. In each line it will go: "x y z"
88  */
89  bool saveToTextFile(const std::string& file) const override;
90 
91  /** Get the m_particles count (equivalent to "particlesCount") */
92  size_t size() const { return m_particles.size(); }
93  /** this = p (+) this. This can be used to convert a PDF from local
94  * coordinates to global, providing the point (newReferenceBase) from which
95  * "to project" the current pdf. Result PDF substituted the currently
96  * stored one in the object. */
97  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
98  /** Draws a single sample from the distribution (WARNING: weights are
99  * assumed to be normalized!) */
100  void drawSingleSample(CPose3D& outPart) const override;
101  /** Draws a number of samples from the distribution, and saves as a list of
102  * 1x6 vectors, where each row contains a (x,y,phi) datum. */
103  void drawManySamples(
104  size_t N,
105  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
106  /** Appends (pose-composition) a given pose "p" to each particle */
107  void operator+=(const CPose3D& Ap);
108  /** Appends (add to the list) a set of m_particles to the existing ones, and
109  * then normalize weights. */
110  void append(CPose3DPDFParticles& o);
111  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */
112  void inverse(CPose3DPDF& o) const override;
113  /** Returns the particle with the highest weight. */
115  /** Bayesian fusion */
116  void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) override;
117 
118 }; // End of class def.
119 } // namespace mrpt::poses
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s m_particles to a text file.
mrpt::math::TPose3D getMostLikelyParticle() const
Returns the particle with the highest weight.
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
void append(CPose3DPDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights...
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
void operator+=(const CPose3D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
CPose3DPDFParticles(size_t M=1)
Constructor.
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
This template class declares the array of particles and its internal data, managing some memory-relat...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
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 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void resetUniform(const mrpt::math::TPose3D &corner_min, const mrpt::math::TPose3D &corner_max, const int particlesCount=-1)
Reset the PDF to an uniformly distributed one, inside of the defined "cube".
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
mrpt::math::TPose3D getParticlePose(int i) const
Returns the pose of the i&#39;th particle.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020