MRPT  2.0.3
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 }
mrpt::math::cov
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
os.h
mrpt::poses::CPose3DPDFParticles::saveToTextFile
bool saveToTextFile(const std::string &file) const override
Save PDF's m_particles to a text file.
Definition: CPose3DPDFParticles.cpp:223
mrpt::poses::CPose3DPDFParticles::operator+=
void operator+=(const CPose3D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
Definition: CPose3DPDFParticles.cpp:266
poses-precomp.h
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
mrpt::poses::CPose3DPDFParticles::resetDeterministic
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.
Definition: CPose3DPDFParticles.cpp:311
mrpt::poses::CPose3DPDF::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::poses::CPose3DPDFParticles::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose3DPDFParticles.cpp:184
mrpt::poses::SE_average< 3 >::get_average
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:244
mrpt::poses::CPose3DPDF
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
Definition: CPose3DPDF.h:39
CPose3DPDFGaussian.h
wrap2pi.h
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::poses::CPose3DPDFParticles::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFParticles.cpp:246
mrpt::math::TPose3D::y
double y
Definition: TPose3D.h:32
mrpt::poses::CPose3DPDFParticles::bayesianFusion
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion.
Definition: CPose3DPDFParticles.cpp:304
mrpt::poses::CPose3DPDFParticles::getMostLikelyParticle
mrpt::math::TPose3D getMostLikelyParticle() const
Returns the particle with the highest weight.
Definition: CPose3DPDFParticles.cpp:289
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPose3DPDFParticles::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose3DPDFParticles.cpp:183
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
random.h
mrpt::bayes::CProbabilityParticle
A template class for holding a the data and the weight of a particle.
Definition: CProbabilityParticle.h:36
mrpt::poses::CPose3DPDFParticles::resetUniform
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".
Definition: CPose3DPDFParticles.cpp:323
mrpt::poses::SE_average< 3 >::append
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
mrpt::poses::CPose3DPDFParticles::drawManySamples
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,...
Definition: CPose3DPDFParticles.cpp:259
mrpt::math::TPose3D::z
double z
Definition: TPose3D.h:32
TPose3D.h
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::poses::CPose3DPDFParticles::drawSingleSample
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!...
Definition: CPose3DPDFParticles.cpp:253
mrpt::poses::CPose3DPDFParticles::getCovarianceAndMean
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.
Definition: CPose3DPDFParticles.cpp:70
mrpt::math::TPose3D::x
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
mrpt::bayes::CParticleFilterData
This template class declares the array of particles and its internal data, managing some memory-relat...
Definition: CParticleFilterData.h:183
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:29
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::poses::CPose3DPDFParticles::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose3DPDFParticles.cpp:188
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::math::TPose3D::pitch
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
mrpt::poses::CPose3DPDFParticles::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
Definition: CPose3DPDFParticles.cpp:35
CPose3D.h
mrpt::poses::CPose3DPDFParticles::inverse
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPose3DPDFParticles.cpp:276
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
mrpt::math::TPose3D::roll
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::math::MatrixVectorBase::setZero
void setZero()
Definition: MatrixVectorBase.h:112
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::poses::SE_average< 3 >
Computes weighted and un-weighted averages of SE(3) poses.
Definition: SO_SE_average.h:157
mrpt::poses::CPose3DPDFParticles::getParticlePose
mrpt::math::TPose3D getParticlePose(int i) const
Returns the pose of the i'th particle.
Definition: CPose3DPDFParticles.cpp:241
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDFParticles::getMean
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
Definition: CPose3DPDFParticles.cpp:52
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
M_2PI
#define M_2PI
Definition: common.h:58
mrpt::poses::CPose3DPDFParticles::append
void append(CPose3DPDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
Definition: CPose3DPDFParticles.cpp:271
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::system
Definition: backtrace.h:14
CPose3DPDFParticles.h
SO_SE_average.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020