MRPT  1.9.9
CPose3DPDFParticles.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/TPose3D.h>
13 #include <mrpt/math/wrap2pi.h>
14 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/random.h>
20 #include <mrpt/system/os.h>
21 
22 using namespace mrpt;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 
27 
29 {
30  m_particles.resize(M);
31  TPose3D nullPose(0, 0, 0, 0, 0, 0);
32  resetDeterministic(nullPose);
33 }
34 
36 {
38  if (this == &o) return; // It may be used sometimes
40  {
41  const auto* pdf = dynamic_cast<const CPose3DPDFParticles*>(&o);
42  ASSERT_(pdf);
43  m_particles = pdf->m_particles;
44  }
46  {
47  THROW_EXCEPTION("TO DO!!");
48  }
49  MRPT_END
50 }
51 
53 {
55  // Default to (0,..,0)
56  p = CPose3D();
57  if (m_particles.empty()) return;
58 
59  // Calc average on SE(3)
60  mrpt::poses::SE_average<3> se_averager;
61  for (const auto& part : m_particles)
62  {
63  const double w = exp(part.log_w);
64  se_averager.append(part.d, w);
65  }
66  se_averager.get_average(p);
67  MRPT_END
68 }
69 
70 std::tuple<CMatrixDouble66, CPose3D> CPose3DPDFParticles::getCovarianceAndMean()
71  const
72 {
74 
76  CPose3D mean;
77 
78  getMean(mean); // First! the mean value:
79 
80  // Now the covariance:
81  cov.setZero();
83  vars.setZero(); // The diagonal of the final covariance matrix
84 
85  // Elements off the diagonal of the covariance matrix:
86  double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
87  double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
88  double std_zya = 0, std_zp = 0, std_zr = 0;
89  double std_yap = 0, std_yar = 0;
90  double std_pr = 0;
91 
92  // Mean values in [0, 2pi] range:
93  double mean_yaw = mean.yaw();
94  double mean_pitch = mean.pitch();
95  double mean_roll = mean.roll();
96  if (mean_yaw < 0) mean_yaw += M_2PI;
97  if (mean_pitch < 0) mean_pitch += M_2PI;
98  if (mean_roll < 0) mean_roll += M_2PI;
99 
100  // enough information to estimate the covariance?
101  if (m_particles.size() < 2) return {cov, mean};
102 
103  // Sum all weight values:
104  double W = 0;
105  for (const auto& p : m_particles) W += exp(p.log_w);
106 
107  ASSERT_(W > 0);
108 
109  // Compute covariance:
110  for (const auto& p : m_particles)
111  {
112  double w = exp(p.log_w) / W;
113 
114  // Manage 1 PI range:
115  double err_yaw = wrapToPi(std::abs(p.d.yaw - mean_yaw));
116  double err_pitch = wrapToPi(std::abs(p.d.pitch - mean_pitch));
117  double err_roll = wrapToPi(std::abs(p.d.roll - mean_roll));
118 
119  double err_x = p.d.x - mean.x();
120  double err_y = p.d.y - mean.y();
121  double err_z = p.d.z - mean.z();
122 
123  vars[0] += square(err_x) * w;
124  vars[1] += square(err_y) * w;
125  vars[2] += square(err_z) * w;
126  vars[3] += square(err_yaw) * w;
127  vars[4] += square(err_pitch) * w;
128  vars[5] += square(err_roll) * w;
129 
130  std_xy += err_x * err_y * w;
131  std_xz += err_x * err_z * w;
132  std_xya += err_x * err_yaw * w;
133  std_xp += err_x * err_pitch * w;
134  std_xr += err_x * err_roll * w;
135 
136  std_yz += err_y * err_z * w;
137  std_yya += err_y * err_yaw * w;
138  std_yp += err_y * err_pitch * w;
139  std_yr += err_y * err_roll * w;
140 
141  std_zya += err_z * err_yaw * w;
142  std_zp += err_z * err_pitch * w;
143  std_zr += err_z * err_roll * w;
144 
145  std_yap += err_yaw * err_pitch * w;
146  std_yar += err_yaw * err_roll * w;
147 
148  std_pr += err_pitch * err_roll * w;
149  } // end for it
150 
151  // Unbiased estimation of variance:
152  cov(0, 0) = vars[0];
153  cov(1, 1) = vars[1];
154  cov(2, 2) = vars[2];
155  cov(3, 3) = vars[3];
156  cov(4, 4) = vars[4];
157  cov(5, 5) = vars[5];
158 
159  cov(1, 0) = cov(0, 1) = std_xy;
160  cov(2, 0) = cov(0, 2) = std_xz;
161  cov(3, 0) = cov(0, 3) = std_xya;
162  cov(4, 0) = cov(0, 4) = std_xp;
163  cov(5, 0) = cov(0, 5) = std_xr;
164 
165  cov(2, 1) = cov(1, 2) = std_yz;
166  cov(3, 1) = cov(1, 3) = std_yya;
167  cov(4, 1) = cov(1, 4) = std_yp;
168  cov(5, 1) = cov(1, 5) = std_yr;
169 
170  cov(3, 2) = cov(2, 3) = std_zya;
171  cov(4, 2) = cov(2, 4) = std_zp;
172  cov(5, 2) = cov(2, 5) = std_zr;
173 
174  cov(4, 3) = cov(3, 4) = std_yap;
175  cov(5, 3) = cov(3, 5) = std_yar;
176 
177  cov(5, 4) = cov(4, 5) = std_pr;
178 
179  return {cov, mean};
180  MRPT_END
181 }
182 
183 uint8_t CPose3DPDFParticles::serializeGetVersion() const { return 1; }
185 {
186  writeParticlesToStream(out); // v1: CPose3D -> TPose3D
187 }
189  mrpt::serialization::CArchive& in, uint8_t version)
190 {
191  switch (version)
192  {
193  case 0:
194  {
196  mrpt::poses::CPose3D, PARTICLE_STORAGE>
197  old;
198  old.readParticlesFromStream(in);
199  m_particles.clear();
201  old.m_particles.begin(), old.m_particles.end(),
202  std::back_inserter(m_particles),
203  [](const auto& p) -> CParticleData {
204  return CParticleData(p.d.asTPose(), p.log_w);
205  });
206  }
207  break;
208  case 1:
209  {
210  readParticlesFromStream(in);
211  }
212  break;
213  default:
215  };
216 }
217 
218 /*---------------------------------------------------------------
219  saveToTextFile
220  Save PDF's m_particles to a text file. In each line it
221  will go: "x y phi weight"
222  ---------------------------------------------------------------*/
224 {
225  using namespace mrpt::system;
226 
227  FILE* f = os::fopen(file.c_str(), "wt");
228  if (!f) return false;
229 
230  os::fprintf(f, "%% x y z yaw[rad] pitch[rad] roll[rad] log_weight\n");
231 
232  for (const auto& p : m_particles)
233  os::fprintf(
234  f, "%f %f %f %f %f %f %e\n", p.d.x, p.d.y, p.d.z, p.d.yaw,
235  p.d.pitch, p.d.roll, p.log_w);
236 
237  os::fclose(f);
238  return true;
239 }
240 
242 {
243  return m_particles[i].d;
244 }
245 
247  const CPose3D& newReferenceBase)
248 {
249  for (auto& p : m_particles)
250  p.d = (newReferenceBase + CPose3D(p.d)).asTPose();
251 }
252 
254 {
255  MRPT_UNUSED_PARAM(outPart);
256  THROW_EXCEPTION("TO DO!");
257 }
258 
260  size_t N, std::vector<CVectorDouble>& outSamples) const
261 {
263  MRPT_UNUSED_PARAM(outSamples);
264  THROW_EXCEPTION("TO DO!");
265 }
266 
268 {
269  MRPT_UNUSED_PARAM(Ap);
270  THROW_EXCEPTION("TO DO!");
271 }
272 
274 {
276  THROW_EXCEPTION("TO DO!");
277 }
278 
280 {
281  MRPT_START
283  auto* out = dynamic_cast<CPose3DPDFParticles*>(&o);
284  ASSERT_(out != nullptr);
285  // Prepare the output:
286  out->copyFrom(*this);
287  const CPose3D zero(0, 0, 0);
288  for (auto& p : out->m_particles) p.d = (zero - CPose3D(p.d)).asTPose();
289  MRPT_END
290 }
291 
293 {
294  mrpt::math::TPose3D ret{0, 0, 0, 0, 0, 0};
295  double max_w = -std::numeric_limits<double>::max();
296  for (const auto& p : m_particles)
297  {
298  if (p.log_w > max_w)
299  {
300  ret = p.d;
301  max_w = p.log_w;
302  }
303  }
304  return ret;
305 }
306 
308  const CPose3DPDF& p1, const CPose3DPDF& p2)
309 {
310  MRPT_UNUSED_PARAM(p1);
311  MRPT_UNUSED_PARAM(p2);
312  THROW_EXCEPTION("Not implemented yet!");
313 }
314 
316  const TPose3D& location, size_t particlesCount)
317 {
318  if (particlesCount > 0) m_particles.resize(particlesCount);
319 
320  for (auto& p : m_particles)
321  {
322  p.d = location;
323  p.log_w = 0;
324  }
325 }
326 
328  const mrpt::math::TPose3D& cmin, const mrpt::math::TPose3D& cmax,
329  const int particlesCount)
330 {
331  MRPT_START
332  if (particlesCount > 0) m_particles.resize(particlesCount);
333 
334  auto& rnd = mrpt::random::getRandomGenerator();
335 
336  for (auto& p : m_particles)
337  {
338  p.d.x = rnd.drawUniform(cmin.x, cmax.x);
339  p.d.y = rnd.drawUniform(cmin.y, cmax.y);
340  p.d.z = rnd.drawUniform(cmin.z, cmax.z);
341  p.d.yaw = rnd.drawUniform(cmin.yaw, cmax.yaw);
342  p.d.pitch = rnd.drawUniform(cmin.pitch, cmax.pitch);
343  p.d.roll = rnd.drawUniform(cmin.roll, cmax.roll);
344  p.log_w = 0;
345  }
346  MRPT_END
347 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
#define MRPT_START
Definition: exceptions.h:241
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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:67
mrpt::math::TPose3D getMostLikelyParticle() const
Returns the particle with the highest weight.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
GLint location
Definition: glext.h:4101
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:37
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
double x
X,Y,Z, coords.
Definition: TPose3D.h:31
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:33
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:4199
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:120
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.
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:89
void operator+=(const CPose3D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
T square(const T x)
Inline function for the square of a number.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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
GLsizei const GLchar ** string
Definition: glext.h:4116
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
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...
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:35
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:410
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:54
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:84
mrpt::vision::TStereoCalibResults out
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion.
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
GLuint in
Definition: glext.h:7391
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
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:257
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.
GLuint GLenum GLenum transform
Definition: glext.h:7089
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::math::TPose3D getParticlePose(int i) const
Returns the pose of the i&#39;th particle.
GLfloat GLfloat p
Definition: glext.h:6398
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
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: e47402b84 Wed Oct 23 01:09:07 2019 +0200 at miƩ oct 23 01:10:13 CEST 2019