Main MRPT website > C++ reference for MRPT 1.9.9
CPointPDFParticles.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-2018, 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 "poses-precomp.h" // Precompiled headers
11 
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/math/ops_containers.h> // maximum()
17 #include <mrpt/system/os.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 using namespace mrpt::system;
23 
25 
27 {
28  setSize(numParticles);
29 }
30 
31 /** Clear all the particles (free memory) */
33 /*---------------------------------------------------------------
34  setSize
35  ---------------------------------------------------------------*/
37  size_t numberParticles, const mrpt::math::TPoint3Df& defaultValue)
38 {
39  // Free old particles: automatic via smart ptr
40  m_particles.resize(numberParticles);
41  for (auto& it : m_particles)
42  {
43  it.log_w = 0;
44  it.d.reset(new TPoint3Df(defaultValue));
45  }
46 }
47 
48 /*---------------------------------------------------------------
49  getMean
50  Returns an estimate of the pose, (the mean, or mathematical expectation of the
51  PDF)
52  ---------------------------------------------------------------*/
54 {
56  if (m_particles.empty())
57  THROW_EXCEPTION("Cannot compute mean since there are zero particles.");
58 
60  double sumW = 0;
61  double x = 0, y = 0, z = 0;
62  for (it = m_particles.begin(); it != m_particles.end(); it++)
63  {
64  const double w = exp(it->log_w);
65  x += it->d->x * w;
66  y += it->d->y * w;
67  z += it->d->z * w;
68  sumW += w;
69  }
70 
71  ASSERT_(sumW != 0);
72 
73  sumW = 1.0 / sumW;
74 
75  p.x(x * sumW);
76  p.y(y * sumW);
77  p.z(z * sumW);
78 
79  MRPT_END
80 }
81 
82 /*---------------------------------------------------------------
83  getEstimatedCovariance
84  ---------------------------------------------------------------*/
87 {
89 
90  getMean(mean);
91  cov.zeros();
92 
93  size_t i, n = m_particles.size();
94  double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
95 
96  double lin_w_sum = 0;
97 
98  for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w);
99  if (lin_w_sum == 0) lin_w_sum = 1;
100 
101  for (i = 0; i < n; i++)
102  {
103  double w = exp(m_particles[i].log_w) / lin_w_sum;
104 
105  double err_x = m_particles[i].d->x - mean.x();
106  double err_y = m_particles[i].d->y - mean.y();
107  double err_phi = m_particles[i].d->z - mean.z();
108 
109  var_x += square(err_x) * w;
110  var_y += square(err_y) * w;
111  var_p += square(err_phi) * w;
112  var_xy += err_x * err_y * w;
113  var_xp += err_x * err_phi * w;
114  var_yp += err_y * err_phi * w;
115  }
116 
117  if (n >= 2)
118  {
119  // Unbiased estimation of variance:
120  cov(0, 0) = var_x;
121  cov(1, 1) = var_y;
122  cov(2, 2) = var_p;
123 
124  cov(1, 0) = cov(0, 1) = var_xy;
125  cov(2, 0) = cov(0, 2) = var_xp;
126  cov(1, 2) = cov(2, 1) = var_yp;
127  }
128 
129  MRPT_END
130 }
131 
134 {
135  uint32_t N = size();
136  out << N;
137 
138  for (CParticleList::const_iterator it = m_particles.begin();
139  it != m_particles.end(); ++it)
140  out << it->log_w << it->d->x << it->d->y << it->d->z;
141 }
142 
145 {
146  switch (version)
147  {
148  case 0:
149  {
150  uint32_t N;
151  in >> N;
152  setSize(N);
153 
154  for (CParticleList::iterator it = m_particles.begin();
155  it != m_particles.end(); ++it)
156  in >> it->log_w >> it->d->x >> it->d->y >> it->d->z;
157  }
158  break;
159  default:
161  };
162 }
163 
165 {
166  if (this == &o) return; // It may be used sometimes
167 
168  // Convert to samples:
169  THROW_EXCEPTION("NO");
170 }
171 
172 /*---------------------------------------------------------------
173 
174  ---------------------------------------------------------------*/
176 {
177  MRPT_START
178 
179  FILE* f = os::fopen(file.c_str(), "wt");
180  if (!f) return false;
181 
182  size_t i, N = m_particles.size();
183  for (i = 0; i < N; i++)
184  os::fprintf(
185  f, "%f %f %f %e\n", m_particles[i].d->x, m_particles[i].d->y,
186  m_particles[i].d->z, m_particles[i].log_w);
187 
188  os::fclose(f);
189  return true;
190  MRPT_END
191 }
192 
193 /*---------------------------------------------------------------
194  changeCoordinatesReference
195  ---------------------------------------------------------------*/
197  const CPose3D& newReferenceBase)
198 {
199  TPoint3D pt;
200  for (CParticleList::iterator it = m_particles.begin();
201  it != m_particles.end(); ++it)
202  {
203  newReferenceBase.composePoint(
204  it->d->x, it->d->y, it->d->z, // In
205  pt.x, pt.y, pt.z // Out
206  );
207  it->d->x = pt.x;
208  it->d->y = pt.y;
209  it->d->z = pt.z;
210  }
211 }
212 
213 /*---------------------------------------------------------------
214  computeKurtosis
215  ---------------------------------------------------------------*/
217 {
218  MRPT_START
219 
220  // kurtosis = \mu^4 / (\sigma^2) -3
221  CVectorDouble kurts, mu4, m, var;
222  kurts.assign(3, .0);
223  mu4.assign(3, .0);
224  m.assign(3, .0);
225  var.assign(3, .0);
226 
227  // Means:
228  for (CParticleList::iterator it = m_particles.begin();
229  it != m_particles.end(); ++it)
230  {
231  m[0] += it->d->x;
232  m[1] += it->d->y;
233  m[2] += it->d->z;
234  }
235  m *= 1.0 / m_particles.size();
236 
237  // variances:
238  for (CParticleList::iterator it = m_particles.begin();
239  it != m_particles.end(); ++it)
240  {
241  var[0] += square(it->d->x - m[0]);
242  var[1] += square(it->d->y - m[1]);
243  var[2] += square(it->d->z - m[2]);
244  }
245  var *= 1.0 / m_particles.size();
246  var[0] = square(var[0]);
247  var[1] = square(var[1]);
248  var[2] = square(var[2]);
249 
250  // Moment:
251  for (CParticleList::iterator it = m_particles.begin();
252  it != m_particles.end(); ++it)
253  {
254  mu4[0] += pow(it->d->x - m[0], 4.0);
255  mu4[1] += pow(it->d->y - m[1], 4.0);
256  mu4[2] += pow(it->d->z - m[2], 4.0);
257  }
258  mu4 *= 1.0 / m_particles.size();
259 
260  // Kurtosis's
261  kurts.array() = mu4.array() / var.array();
262 
263  return math::maximum(kurts);
264 
265  MRPT_END
266 }
267 
268 /*---------------------------------------------------------------
269  drawSingleSample
270  ---------------------------------------------------------------*/
272 {
273  MRPT_UNUSED_PARAM(outSample);
274  THROW_EXCEPTION("TO DO!");
275 }
276 
277 /*---------------------------------------------------------------
278  bayesianFusion
279  ---------------------------------------------------------------*/
281  const CPointPDF& p1_, const CPointPDF& p2_,
282  const double minMahalanobisDistToDrop)
283 {
284  MRPT_UNUSED_PARAM(p1_);
285  MRPT_UNUSED_PARAM(p2_);
286  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
287  MRPT_START
288 
289  THROW_EXCEPTION("TODO!!!");
290 
291  MRPT_END
292 }
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:5074
This file implements several operations that operate element-wise on individual or pairs of container...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file, where each line is: X Y Z LOG_W.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
Lightweight 3D point (float version).
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
double x
X,Y,Z coordinates.
void getMean(CPoint3D &mean_point) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
CONTAINER::Scalar maximum(const CONTAINER &v)
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D point.
Definition: CPoint3D.h:33
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:379
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#define MRPT_END
Definition: exceptions.h:266
void setSize(size_t numberParticles, const mrpt::math::TPoint3Df &defaultValue=mrpt::math::TPoint3Df{0, 0, 0})
Erase all the previous particles and change the number of particles, with a given initial value...
GLuint in
Definition: glext.h:7274
GLenum GLint GLint y
Definition: glext.h:3538
void clear()
Clear all the particles (free memory)
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
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:148
GLsizeiptr size
Definition: glext.h:3923
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:47
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:39
double computeKurtosis()
Compute the kurtosis of the distribution.
GLfloat GLfloat p
Definition: glext.h:6305
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
const Scalar * const_iterator
Definition: eigen_plugins.h:27
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019