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-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 
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();
200  std::transform(
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  ---------------------------------------------------------------*/
223 bool CPose3DPDFParticles::saveToTextFile(const std::string& file) const
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  [maybe_unused]] CPose3D& outPart) const
255 {
256  THROW_EXCEPTION("TO DO!");
257 }
258 
260  [[maybe_unused]] size_t N,
261  [[maybe_unused]] std::vector<CVectorDouble>& outSamples) const
262 {
263  THROW_EXCEPTION("TO DO!");
264 }
265 
266 void CPose3DPDFParticles::operator+=([[maybe_unused]] const CPose3D& Ap)
267 {
268  THROW_EXCEPTION("TO DO!");
269 }
270 
272 {
273  THROW_EXCEPTION("TO DO!");
274 }
275 
277 {
278  MRPT_START
280  auto* out = dynamic_cast<CPose3DPDFParticles*>(&o);
281  ASSERT_(out != nullptr);
282  // Prepare the output:
283  out->copyFrom(*this);
284  const CPose3D zero(0, 0, 0);
285  for (auto& p : out->m_particles) p.d = (zero - CPose3D(p.d)).asTPose();
286  MRPT_END
287 }
288 
290 {
291  mrpt::math::TPose3D ret{0, 0, 0, 0, 0, 0};
292  double max_w = -std::numeric_limits<double>::max();
293  for (const auto& p : m_particles)
294  {
295  if (p.log_w > max_w)
296  {
297  ret = p.d;
298  max_w = p.log_w;
299  }
300  }
301  return ret;
302 }
303 
305  [[maybe_unused]] const CPose3DPDF& p1,
306  [[maybe_unused]] const CPose3DPDF& p2)
307 {
308  THROW_EXCEPTION("Not implemented yet!");
309 }
310 
312  const TPose3D& location, size_t particlesCount)
313 {
314  if (particlesCount > 0) m_particles.resize(particlesCount);
315 
316  for (auto& p : m_particles)
317  {
318  p.d = location;
319  p.log_w = 0;
320  }
321 }
322 
324  const mrpt::math::TPose3D& cmin, const mrpt::math::TPose3D& cmax,
325  const int particlesCount)
326 {
327  MRPT_START
328  if (particlesCount > 0) m_particles.resize(particlesCount);
329 
330  auto& rnd = mrpt::random::getRandomGenerator();
331 
332  for (auto& p : m_particles)
333  {
334  p.d.x = rnd.drawUniform(cmin.x, cmax.x);
335  p.d.y = rnd.drawUniform(cmin.y, cmax.y);
336  p.d.z = rnd.drawUniform(cmin.z, cmax.z);
337  p.d.yaw = rnd.drawUniform(cmin.yaw, cmax.yaw);
338  p.d.pitch = rnd.drawUniform(cmin.pitch, cmax.pitch);
339  p.d.roll = rnd.drawUniform(cmin.roll, cmax.roll);
340  p.log_w = 0;
341  }
342  MRPT_END
343 }
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
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
#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:32
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:34
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.
#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:102
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.
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
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:36
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:408
return_t square(const num_t x)
Inline function for the square of a number.
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:85
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:24
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.
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.
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.



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