MRPT  1.9.9
CPointPDFParticles.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 
12 #include <mrpt/math/ops_containers.h> // maximum()
13 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/system/os.h>
18 #include <Eigen/Dense>
19 
20 using namespace mrpt;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 using namespace mrpt::system;
24 
26 
28 {
29  setSize(numParticles);
30 }
31 
32 /** Clear all the particles (free memory) */
33 void CPointPDFParticles::clear() { setSize(0); }
34 /*---------------------------------------------------------------
35  setSize
36  ---------------------------------------------------------------*/
38  size_t numberParticles, const mrpt::math::TPoint3Df& defaultValue)
39 {
40  // Free old particles: automatic via smart ptr
41  m_particles.resize(numberParticles);
42  for (auto& it : m_particles)
43  {
44  it.log_w = 0;
45  it.d.reset(new TPoint3Df(defaultValue));
46  }
47 }
48 
49 /*---------------------------------------------------------------
50  getMean
51  Returns an estimate of the pose, (the mean, or mathematical expectation of the
52  PDF)
53  ---------------------------------------------------------------*/
55 {
57  if (m_particles.empty())
58  THROW_EXCEPTION("Cannot compute mean since there are zero particles.");
59 
60  CParticleList::const_iterator it;
61  double sumW = 0;
62  double x = 0, y = 0, z = 0;
63  for (it = m_particles.begin(); it != m_particles.end(); it++)
64  {
65  const double w = exp(it->log_w);
66  x += it->d->x * w;
67  y += it->d->y * w;
68  z += it->d->z * w;
69  sumW += w;
70  }
71 
72  ASSERT_(sumW != 0);
73 
74  sumW = 1.0 / sumW;
75 
76  p.x(x * sumW);
77  p.y(y * sumW);
78  p.z(z * sumW);
79 
80  MRPT_END
81 }
82 
83 std::tuple<CMatrixDouble33, CPoint3D> CPointPDFParticles::getCovarianceAndMean()
84  const
85 {
87 
88  CPoint3D mean;
90 
91  getMean(mean);
92  cov.setZero();
93 
94  size_t i, n = m_particles.size();
95  double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
96 
97  double lin_w_sum = 0;
98 
99  for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w);
100  if (lin_w_sum == 0) lin_w_sum = 1;
101 
102  for (i = 0; i < n; i++)
103  {
104  double w = exp(m_particles[i].log_w) / lin_w_sum;
105 
106  double err_x = m_particles[i].d->x - mean.x();
107  double err_y = m_particles[i].d->y - mean.y();
108  double err_phi = m_particles[i].d->z - mean.z();
109 
110  var_x += square(err_x) * w;
111  var_y += square(err_y) * w;
112  var_p += square(err_phi) * w;
113  var_xy += err_x * err_y * w;
114  var_xp += err_x * err_phi * w;
115  var_yp += err_y * err_phi * w;
116  }
117 
118  if (n >= 2)
119  {
120  // Unbiased estimation of variance:
121  cov(0, 0) = var_x;
122  cov(1, 1) = var_y;
123  cov(2, 2) = var_p;
124 
125  cov(1, 0) = cov(0, 1) = var_xy;
126  cov(2, 0) = cov(0, 2) = var_xp;
127  cov(1, 2) = cov(2, 1) = var_yp;
128  }
129 
130  return {cov, mean};
131  MRPT_END
132 }
133 
136 {
137  uint32_t N = size();
138  out << N;
139 
140  for (const auto& m_particle : m_particles)
141  out << m_particle.log_w << m_particle.d->x << m_particle.d->y
142  << m_particle.d->z;
143 }
144 
147 {
148  switch (version)
149  {
150  case 0:
151  {
152  uint32_t N;
153  in >> N;
154  setSize(N);
155 
156  for (auto& m_particle : m_particles)
157  in >> m_particle.log_w >> m_particle.d->x >> m_particle.d->y >>
158  m_particle.d->z;
159  }
160  break;
161  default:
163  };
164 }
165 
167 {
168  if (this == &o) return; // It may be used sometimes
169 
170  // Convert to samples:
171  THROW_EXCEPTION("NO");
172 }
173 
174 /*---------------------------------------------------------------
175 
176  ---------------------------------------------------------------*/
178 {
179  MRPT_START
180 
181  FILE* f = os::fopen(file.c_str(), "wt");
182  if (!f) return false;
183 
184  size_t i, N = m_particles.size();
185  for (i = 0; i < N; i++)
186  os::fprintf(
187  f, "%f %f %f %e\n", m_particles[i].d->x, m_particles[i].d->y,
188  m_particles[i].d->z, m_particles[i].log_w);
189 
190  os::fclose(f);
191  return true;
192  MRPT_END
193 }
194 
195 /*---------------------------------------------------------------
196  changeCoordinatesReference
197  ---------------------------------------------------------------*/
199  const CPose3D& newReferenceBase)
200 {
201  TPoint3D pt;
202  for (auto& m_particle : m_particles)
203  {
204  newReferenceBase.composePoint(
205  m_particle.d->x, m_particle.d->y, m_particle.d->z, // In
206  pt.x, pt.y, pt.z // Out
207  );
208  m_particle.d->x = pt.x;
209  m_particle.d->y = pt.y;
210  m_particle.d->z = pt.z;
211  }
212 }
213 
214 /*---------------------------------------------------------------
215  computeKurtosis
216  ---------------------------------------------------------------*/
218 {
219  MRPT_START
220 
221  // kurtosis = \mu^4 / (\sigma^2) -3
222  Eigen::Vector3d kurts, mu4, m, var;
223  kurts.fill(0);
224  mu4.fill(0);
225  m.fill(0);
226  var.fill(0);
227 
228  // Means:
229  for (auto& m_particle : m_particles)
230  {
231  m[0] += m_particle.d->x;
232  m[1] += m_particle.d->y;
233  m[2] += m_particle.d->z;
234  }
235  m *= 1.0 / m_particles.size();
236 
237  // variances:
238  for (auto& m_particle : m_particles)
239  {
240  var[0] += square(m_particle.d->x - m[0]);
241  var[1] += square(m_particle.d->y - m[1]);
242  var[2] += square(m_particle.d->z - m[2]);
243  }
244  var *= 1.0 / m_particles.size();
245  var[0] = square(var[0]);
246  var[1] = square(var[1]);
247  var[2] = square(var[2]);
248 
249  // Moment:
250  for (auto& m_particle : m_particles)
251  {
252  mu4[0] += pow(m_particle.d->x - m[0], 4.0);
253  mu4[1] += pow(m_particle.d->y - m[1], 4.0);
254  mu4[2] += pow(m_particle.d->z - m[2], 4.0);
255  }
256  mu4 *= 1.0 / m_particles.size();
257 
258  // Kurtosis's
259  kurts.array() = mu4.array() / var.array();
260 
261  return math::maximum(kurts);
262 
263  MRPT_END
264 }
265 
266 /*---------------------------------------------------------------
267  drawSingleSample
268  ---------------------------------------------------------------*/
270 {
271  MRPT_UNUSED_PARAM(outSample);
272  THROW_EXCEPTION("TO DO!");
273 }
274 
275 /*---------------------------------------------------------------
276  bayesianFusion
277  ---------------------------------------------------------------*/
279  const CPointPDF& p1_, const CPointPDF& p2_,
280  const double minMahalanobisDistToDrop)
281 {
282  MRPT_UNUSED_PARAM(p1_);
283  MRPT_UNUSED_PARAM(p2_);
284  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
285  MRPT_START
286 
287  THROW_EXCEPTION("TODO!!!");
288 
289  MRPT_END
290 }
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< 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:368
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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:275
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLenum GLsizei n
Definition: glext.h:5136
This file implements several operations that operate element-wise on individual or pairs of container...
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:4199
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
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!)
CONTAINER::Scalar maximum(const CONTAINER &v)
void getMean(CPoint3D &mean_point) const override
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
GLsizei const GLchar ** string
Definition: glext.h:4116
A class used to store a 3D point.
Definition: CPoint3D.h:31
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:410
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
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:84
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 changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#define MRPT_END
Definition: exceptions.h:245
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:7391
double mean(const CONTAINER &v)
Computes the mean value of a vector.
GLenum GLint GLint y
Definition: glext.h:3542
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:257
GLsizeiptr size
Definition: glext.h:3934
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
unsigned __int32 uint32_t
Definition: rptypes.h:50
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
double computeKurtosis()
Compute the kurtosis of the distribution.
GLfloat GLfloat p
Definition: glext.h:6398
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
#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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019