MRPT  1.9.9
CPose3DPDFParticles.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/CPose3D.h>
15 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/system/os.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 
25 
27 {
28  m_particles.resize(M);
29  TPose3D nullPose(0, 0, 0, 0, 0, 0);
30  resetDeterministic(nullPose);
31 }
32 
34 {
36  if (this == &o) return; // It may be used sometimes
38  {
39  const CPose3DPDFParticles* pdf =
40  dynamic_cast<const CPose3DPDFParticles*>(&o);
41  ASSERT_(pdf);
42  m_particles = pdf->m_particles;
43  }
45  {
46  THROW_EXCEPTION("TO DO!!");
47  }
48  MRPT_END
49 }
50 
52 {
54  // Default to (0,..,0)
55  p = CPose3D();
56  if (m_particles.empty()) return;
57 
58  // Calc average on SE(3)
59  mrpt::poses::SE_average<3> se_averager;
60  for (const auto& part : m_particles)
61  {
62  const double w = exp(part.log_w);
63  se_averager.append(part.d, w);
64  }
65  se_averager.get_average(p);
66  MRPT_END
67 }
68 
71 {
73 
74  getMean(mean); // First! the mean value:
75 
76  // Now the covariance:
77  cov.zeros();
78  CVectorDouble vars;
79  vars.assign(6, 0.0); // The diagonal of the final covariance matrix
80 
81  // Elements off the diagonal of the covariance matrix:
82  double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
83  double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
84  double std_zya = 0, std_zp = 0, std_zr = 0;
85  double std_yap = 0, std_yar = 0;
86  double std_pr = 0;
87 
88  // Mean values in [0, 2pi] range:
89  double mean_yaw = mean.yaw();
90  double mean_pitch = mean.pitch();
91  double mean_roll = mean.roll();
92  if (mean_yaw < 0) mean_yaw += M_2PI;
93  if (mean_pitch < 0) mean_pitch += M_2PI;
94  if (mean_roll < 0) mean_roll += M_2PI;
95 
96  // Enought information to estimate the covariance?
97  if (m_particles.size() < 2) return;
98 
99  // Sum all weight values:
100  double W = 0;
101  for (const auto& p : m_particles) W += exp(p.log_w);
102 
103  ASSERT_(W > 0);
104 
105  // Compute covariance:
106  for (const auto& p : m_particles)
107  {
108  double w = exp(p.log_w) / W;
109 
110  // Manage 1 PI range:
111  double err_yaw = wrapToPi(std::abs(p.d.yaw - mean_yaw));
112  double err_pitch = wrapToPi(std::abs(p.d.pitch - mean_pitch));
113  double err_roll = wrapToPi(std::abs(p.d.roll - mean_roll));
114 
115  double err_x = p.d.x - mean.x();
116  double err_y = p.d.y - mean.y();
117  double err_z = p.d.z - mean.z();
118 
119  vars[0] += square(err_x) * w;
120  vars[1] += square(err_y) * w;
121  vars[2] += square(err_z) * w;
122  vars[3] += square(err_yaw) * w;
123  vars[4] += square(err_pitch) * w;
124  vars[5] += square(err_roll) * w;
125 
126  std_xy += err_x * err_y * w;
127  std_xz += err_x * err_z * w;
128  std_xya += err_x * err_yaw * w;
129  std_xp += err_x * err_pitch * w;
130  std_xr += err_x * err_roll * w;
131 
132  std_yz += err_y * err_z * w;
133  std_yya += err_y * err_yaw * w;
134  std_yp += err_y * err_pitch * w;
135  std_yr += err_y * err_roll * w;
136 
137  std_zya += err_z * err_yaw * w;
138  std_zp += err_z * err_pitch * w;
139  std_zr += err_z * err_roll * w;
140 
141  std_yap += err_yaw * err_pitch * w;
142  std_yar += err_yaw * err_roll * w;
143 
144  std_pr += err_pitch * err_roll * w;
145  } // end for it
146 
147  // Unbiased estimation of variance:
148  cov(0, 0) = vars[0];
149  cov(1, 1) = vars[1];
150  cov(2, 2) = vars[2];
151  cov(3, 3) = vars[3];
152  cov(4, 4) = vars[4];
153  cov(5, 5) = vars[5];
154 
155  cov(1, 0) = cov(0, 1) = std_xy;
156  cov(2, 0) = cov(0, 2) = std_xz;
157  cov(3, 0) = cov(0, 3) = std_xya;
158  cov(4, 0) = cov(0, 4) = std_xp;
159  cov(5, 0) = cov(0, 5) = std_xr;
160 
161  cov(2, 1) = cov(1, 2) = std_yz;
162  cov(3, 1) = cov(1, 3) = std_yya;
163  cov(4, 1) = cov(1, 4) = std_yp;
164  cov(5, 1) = cov(1, 5) = std_yr;
165 
166  cov(3, 2) = cov(2, 3) = std_zya;
167  cov(4, 2) = cov(2, 4) = std_zp;
168  cov(5, 2) = cov(2, 5) = std_zr;
169 
170  cov(4, 3) = cov(3, 4) = std_yap;
171  cov(5, 3) = cov(3, 5) = std_yar;
172 
173  cov(5, 4) = cov(4, 5) = std_pr;
174 
175  MRPT_END
176 }
177 
180 {
181  writeParticlesToStream(out); // v1: CPose3D -> TPose3D
182 }
185 {
186  switch (version)
187  {
188  case 0:
189  {
192  m_particles.clear();
194  old.m_particles.begin(), old.m_particles.end(),
195  std::back_inserter(m_particles),
196  [](const auto& p) -> CParticleData {
197  return CParticleData(p.d.asTPose(), p.log_w);
198  });
199  }
200  break;
201  case 1:
202  {
203  readParticlesFromStream(in);
204  }
205  break;
206  default:
208  };
209 }
210 
211 /*---------------------------------------------------------------
212  saveToTextFile
213  Save PDF's m_particles to a text file. In each line it
214  will go: "x y phi weight"
215  ---------------------------------------------------------------*/
217 {
218  using namespace mrpt::system;
219 
220  FILE* f = os::fopen(file.c_str(), "wt");
221  if (!f) return false;
222 
223  os::fprintf(f, "%% x y z yaw[rad] pitch[rad] roll[rad] log_weight\n");
224 
225  for (const auto& p : m_particles)
226  os::fprintf(
227  f, "%f %f %f %f %f %f %e\n", p.d.x, p.d.y, p.d.z, p.d.yaw,
228  p.d.pitch, p.d.roll, p.log_w);
229 
230  os::fclose(f);
231  return true;
232 }
233 
235 {
236  return m_particles[i].d;
237 }
238 
240  const CPose3D& newReferenceBase)
241 {
242  for (auto& p : m_particles)
243  p.d = (newReferenceBase + CPose3D(p.d)).asTPose();
244 }
245 
247 {
248  MRPT_UNUSED_PARAM(outPart);
249  THROW_EXCEPTION("TO DO!");
250 }
251 
253  size_t N, std::vector<CVectorDouble>& outSamples) const
254 {
256  MRPT_UNUSED_PARAM(outSamples);
257  THROW_EXCEPTION("TO DO!");
258 }
259 
261 {
262  MRPT_UNUSED_PARAM(Ap);
263  THROW_EXCEPTION("TO DO!");
264 }
265 
267 {
269  THROW_EXCEPTION("TO DO!");
270 }
271 
273 {
274  MRPT_START
276  CPose3DPDFParticles* out = static_cast<CPose3DPDFParticles*>(&o);
277  // Prepare the output:
278  out->copyFrom(*this);
279  const CPose3D zero(0, 0, 0);
280  for (auto& p : out->m_particles) p.d = (zero - CPose3D(p.d)).asTPose();
281  MRPT_END
282 }
283 
285 {
286  mrpt::math::TPose3D ret{0, 0, 0, 0, 0, 0};
287  double max_w = -std::numeric_limits<double>::max();
288  for (const auto& p : m_particles)
289  {
290  if (p.log_w > max_w)
291  {
292  ret = p.d;
293  max_w = p.log_w;
294  }
295  }
296  return ret;
297 }
298 
300  const CPose3DPDF& p1, const CPose3DPDF& p2)
301 {
302  MRPT_UNUSED_PARAM(p1);
303  MRPT_UNUSED_PARAM(p2);
304  THROW_EXCEPTION("Not implemented yet!");
305 }
306 
308  const TPose3D& location, size_t particlesCount)
309 {
310  if (particlesCount > 0) m_particles.resize(particlesCount);
311 
312  for (auto& p : m_particles)
313  {
314  p.d = location;
315  p.log_w = 0;
316  }
317 }
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
#define MRPT_START
Definition: exceptions.h:262
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s m_particles to a text file.
#define M_2PI
Definition: common.h:58
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::math::TPose3D getMostLikelyParticle() const
Returns the particle with the highest weight.
CParticleList m_particles
The array of particles.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
GLint location
Definition: glext.h:4086
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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.
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.
void append(CPose3DPDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:84
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
void operator+=(const CPose3D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
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...
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.
Computes weighted and un-weighted averages of SE(3) poses.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
A template class for holding a the data and the weight of a particle.
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion.
#define MRPT_END
Definition: exceptions.h:266
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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
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.
GLuint GLenum GLenum transform
Definition: glext.h:6975
mrpt::math::TPose3D getParticlePose(int i) const
Returns the pose of the i&#39;th particle.
GLfloat GLfloat p
Definition: glext.h:6305
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020