MRPT  1.9.9
CPose3DPDFSOG.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 
13 #include <mrpt/system/os.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::math;
21 using namespace mrpt::system;
22 using namespace std;
23 
25 
26 /*---------------------------------------------------------------
27  Constructor
28  ---------------------------------------------------------------*/
29 CPose3DPDFSOG::CPose3DPDFSOG(size_t nModes) : m_modes(nModes) {}
30 /*---------------------------------------------------------------
31  clear
32  ---------------------------------------------------------------*/
33 void CPose3DPDFSOG::clear() { m_modes.clear(); }
34 /*---------------------------------------------------------------
35  Resize
36  ---------------------------------------------------------------*/
37 void CPose3DPDFSOG::resize(const size_t N) { m_modes.resize(N); }
38 /*---------------------------------------------------------------
39  getMean
40  Returns an estimate of the pose, (the mean, or mathematical expectation of the
41  PDF)
42  ---------------------------------------------------------------*/
44 {
45  if (!m_modes.empty())
46  {
47  // Calc average on SE(3)
48  mrpt::poses::SE_average<3> se_averager;
49  for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
50  {
51  const double w = exp(it->log_w);
52  se_averager.append((it)->val.mean, w);
53  }
54  se_averager.get_average(p);
55  }
56  else
57  {
58  p.setFromValues(0, 0, 0, 0, 0, 0);
59  }
60 }
61 
62 /*---------------------------------------------------------------
63  getCovarianceAndMean
64  ---------------------------------------------------------------*/
66  mrpt::math::CMatrixDouble66& estCovOut, CPose3D& mean) const
67 {
68  size_t N = m_modes.size();
69 
70  getMean(mean);
72 
73  if (N)
74  {
75  // 1) Get the mean:
76  double sumW = 0;
78 
81  for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
82  {
83  double w;
84  sumW += w = exp((it)->log_w);
85  estMean_i = mrpt::math::CMatrixDouble61((it)->val.mean);
86  MMt.multiply_AAt(estMean_i);
87  MMt += (it)->val.cov;
88  MMt *= w;
89  estCov += MMt; // w * ( (it)->val.cov +
90  // ((estMean_i-estMean)*(~(estMean_i-estMean))) );
91  }
92 
93  if (sumW != 0) estCov *= (1.0 / sumW);
94  }
95 
96  estCovOut = estCov;
97 }
98 
101 {
102  uint32_t N = m_modes.size();
103  out << N;
104  for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
105  {
106  out << (it)->log_w;
107  out << (it)->val.mean;
108  out << (it)->val.cov;
109  }
110 }
113 {
114  switch (version)
115  {
116  case 0:
117  case 1:
118  case 2:
119  {
120  uint32_t N;
121  in >> N;
122  this->resize(N);
123 
124  for (iterator it = m_modes.begin(); it != m_modes.end(); ++it)
125  {
126  in >> (it)->log_w;
127 
128  // In version 0, weights were linear!!
129  if (version == 0) (it)->log_w = log(max(1e-300, (it)->log_w));
130 
131  in >> (it)->val.mean;
132 
133  if (version == 1) // were floats
134  {
135  THROW_EXCEPTION("Unsupported serialized version: too old");
136  }
137  else
138  {
139  in >> (it)->val.cov;
140  }
141  }
142  }
143  break;
144  default:
146  };
147 }
148 
150 {
151  MRPT_START
152 
153  if (this == &o) return; // It may be used sometimes
154 
156  {
157  *this = *static_cast<const CPose3DPDFSOG*>(&o);
158  }
159  else
160  {
161  this->resize(1);
162  m_modes[0].log_w = 0;
164  o.getCovarianceAndMean(C, m_modes[0].val.mean);
165  m_modes[0].val.cov = C;
166  }
167 
168  MRPT_END
169 }
170 
171 /*---------------------------------------------------------------
172  saveToTextFile
173  ---------------------------------------------------------------*/
175 {
176  FILE* f = os::fopen(file.c_str(), "wt");
177  if (!f) return false;
178 
179  for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
180  os::fprintf(
181  f, "%e %e %e %e %e %e %e %e %e %e\n", exp((it)->log_w),
182  (it)->val.mean.x(), (it)->val.mean.y(), (it)->val.mean.z(),
183  (it)->val.cov(0, 0), (it)->val.cov(1, 1), (it)->val.cov(2, 2),
184  (it)->val.cov(0, 1), (it)->val.cov(0, 2), (it)->val.cov(1, 2));
185  os::fclose(f);
186  return true;
187 }
188 
189 /*---------------------------------------------------------------
190  changeCoordinatesReference
191  ---------------------------------------------------------------*/
193 {
194  for (iterator it = m_modes.begin(); it != m_modes.end(); ++it)
195  (it)->val.changeCoordinatesReference(newReferenceBase);
196 }
197 
198 /*---------------------------------------------------------------
199  bayesianFusion
200  ---------------------------------------------------------------*/
202 {
203  MRPT_START
204 
205  // p1: CPose3DPDFSOG, p2: CPosePDFGaussian:
206 
209 
210  THROW_EXCEPTION("TODO!!!");
211 #if 0
212 /*
213  CPose3DPDFSOG *p1 = (CPose3DPDFSOG*)&p1_;
214  CPose3DPDFSOG *p2 = (CPose3DPDFSOG*)&p2_;
215 
216  // Compute the new kernel means, covariances, and weights after multiplying to the Gaussian "p2":
217  CPosePDFGaussian auxGaussianProduct,auxSOG_Kernel_i;
218  TGaussianMode newKernel;
219 
220 
221 
222  CMatrixD covInv( p2->cov.inv() );
223  CMatrixD eta(3,1);
224  eta(0,0) = p2->mean.x;
225  eta(1,0) = p2->mean.y;
226  eta(2,0) = p2->mean.phi;
227  eta = covInv * eta;
228 
229  // Normal distribution canonical form constant:
230  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
231  double a = -0.5*( 3*log(M_2PI) - log( covInv.det() ) + (~eta * p2->cov * eta)(0,0) );
232 
233  this->m_modes.clear();
234  for (std::deque<TGaussianMode>::iterator it =p1->m_modes.begin();it!=p1->m_modes.end();++it)
235  {
236  auxSOG_Kernel_i.mean = it->mean;
237  auxSOG_Kernel_i.cov = it->cov;
238  auxGaussianProduct.bayesianFusion( auxSOG_Kernel_i, *p2 );
239 
240  // ----------------------------------------------------------------------
241  // The new weight is given by:
242  //
243  // w'_i = w_i * exp( a + a_i - a' )
244  //
245  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1)) + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
246  //
247  // ----------------------------------------------------------------------
248  newKernel.mean = auxGaussianProduct.mean;
249  newKernel.cov = auxGaussianProduct.cov;
250 
251  CMatrixD covInv_i( auxSOG_Kernel_i.cov.inv() );
252  CMatrixD eta_i(3,1);
253  eta_i(0,0) = auxSOG_Kernel_i.mean.x;
254  eta_i(1,0) = auxSOG_Kernel_i.mean.y;
255  eta_i(2,0) = auxSOG_Kernel_i.mean.phi;
256  eta_i = covInv_i * eta_i;
257 
258  CMatrixD new_covInv_i( newKernel.cov.inv() );
259  CMatrixD new_eta_i(3,1);
260  new_eta_i(0,0) = newKernel.mean.x;
261  new_eta_i(1,0) = newKernel.mean.y;
262  new_eta_i(2,0) = newKernel.mean.phi;
263  new_eta_i = new_covInv_i * new_eta_i;
264 
265  double a_i = -0.5*( 3*log(M_2PI) - log( new_covInv_i.det() ) + (~eta_i * auxSOG_Kernel_i.cov * eta_i)(0,0) );
266  double new_a_i = -0.5*( 3*log(M_2PI) - log( new_covInv_i.det() ) + (~new_eta_i * newKernel.cov * new_eta_i)(0,0) );
267 
268  newKernel.w = it->w * exp( a + a_i - new_a_i );
269 
270  // Add to the results (in "this") the new kernel:
271  this->m_modes.push_back( newKernel );
272  }
273 */
274  normalizeWeights();
275 #endif
276  MRPT_END
277 }
278 
279 /*---------------------------------------------------------------
280  assureSymmetry
281  ---------------------------------------------------------------*/
283 {
284  MRPT_START
285  // Differences, when they exist, appear in the ~15'th significant
286  // digit, so... just take one of them arbitrarily!
287  for (iterator it = m_modes.begin(); it != m_modes.end(); ++it)
288  {
289  for (size_t i = 0; i < 6; i++)
290  for (size_t j = i + 1; j < 6; j++)
291  (it)->val.cov.get_unsafe(i, j) = (it)->val.cov.get_unsafe(j, i);
292  }
293 
294  MRPT_END
295 }
296 
297 /*---------------------------------------------------------------
298  normalizeWeights
299  ---------------------------------------------------------------*/
301 {
302  MRPT_START
303 
304  if (m_modes.empty()) return;
305 
306  double maxW = m_modes[0].log_w;
307  for (iterator it = m_modes.begin(); it != m_modes.end(); ++it)
308  maxW = max(maxW, (it)->log_w);
309 
310  for (iterator it = m_modes.begin(); it != m_modes.end(); ++it)
311  (it)->log_w -= maxW;
312 
313  MRPT_END
314 }
315 
316 /*---------------------------------------------------------------
317  drawSingleSample
318  ---------------------------------------------------------------*/
320 {
321  MRPT_UNUSED_PARAM(outPart);
322  THROW_EXCEPTION("TO DO!");
323 }
324 
325 /*---------------------------------------------------------------
326  drawManySamples
327  ---------------------------------------------------------------*/
329  size_t N, std::vector<CVectorDouble>& outSamples) const
330 {
332  MRPT_UNUSED_PARAM(outSamples);
333  THROW_EXCEPTION("TO DO!");
334 }
335 
336 /*---------------------------------------------------------------
337  inverse
338  ---------------------------------------------------------------*/
340 {
341  MRPT_START
343  CPose3DPDFSOG* out = static_cast<CPose3DPDFSOG*>(&o);
344 
345  // Prepare the output SOG:
346  out->resize(m_modes.size());
347 
348  const_iterator it;
349  iterator outIt;
350 
351  for (it = m_modes.begin(), outIt = out->m_modes.begin();
352  it != m_modes.end(); it++, outIt++)
353  {
354  (it)->val.inverse((outIt)->val);
355  (outIt)->log_w = (it)->log_w;
356  }
357 
358  MRPT_END
359 }
360 
361 /*---------------------------------------------------------------
362  appendFrom
363  ---------------------------------------------------------------*/
365 {
366  MRPT_START
367 
368  ASSERT_(&o != this); // Don't be bad...
369  if (o.m_modes.empty()) return;
370 
371  // Make copies:
372  for (const_iterator it = o.m_modes.begin(); it != o.m_modes.end(); ++it)
373  m_modes.push_back(*it);
374 
375  normalizeWeights();
376  MRPT_END
377 }
378 
379 /*---------------------------------------------------------------
380  getMostLikelyMode
381  ---------------------------------------------------------------*/
383 {
384  if (this->empty())
385  {
386  outVal = CPose3DPDFGaussian();
387  }
388  else
389  {
390  const_iterator it_best = m_modes.end();
391  for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
392  if (it_best == m_modes.end() || it->log_w > it_best->log_w)
393  it_best = it;
394 
395  outVal = it_best->val;
396  }
397 }
EIGEN_STRONG_INLINE bool empty() const
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.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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...
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
void getMostLikelyMode(CPose3DPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
void appendFrom(const CPose3DPDFSOG &o)
Append the Gaussian modes from "o" to the current set of modes of "this" density. ...
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
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.
STL namespace.
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
Definition: eigen_frwds.h:65
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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 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,z,yaw,pitch,roll) datum.
TModesList m_modes
Access directly to this array for modify the modes as desired.
Definition: CPose3DPDFSOG.h:65
bool saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
int val
Definition: mrpt_jpeglib.h:955
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:52
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.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
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 clear()
Clear all the gaussian modes.
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:53
This file implements matrix/vector text and binary serialization.
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
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 ...
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
unsigned __int32 uint32_t
Definition: rptypes.h:47
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
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void resize(const size_t N)
Set the number of SOG modes.



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