Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
13 #include <mrpt/system/os.h>
14 #include <mrpt/utils/CStream.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::math;
21 using namespace mrpt::utils;
22 using namespace mrpt::system;
23 using namespace std;
24 
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
30 CPose3DPDFSOG::CPose3DPDFSOG( size_t nModes ) :
31  m_modes(nModes)
32 {
33 }
34 
35 /*---------------------------------------------------------------
36  clear
37  ---------------------------------------------------------------*/
39 {
40  m_modes.clear();
41 }
42 
43 /*---------------------------------------------------------------
44  Resize
45  ---------------------------------------------------------------*/
46 void CPose3DPDFSOG::resize(const size_t N)
47 {
48  m_modes.resize(N);
49 }
50 
51 
52 /*---------------------------------------------------------------
53  getMean
54  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
55  ---------------------------------------------------------------*/
57 {
58  if (!m_modes.empty())
59  {
60  // Calc average on SE(3)
61  mrpt::poses::SE_average<3> se_averager;
62  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
63  {
64  const double w = exp(it->log_w);
65  se_averager.append( (it)->val.mean,w );
66  }
67  se_averager.get_average(p);
68  }
69  else
70  {
71  p.setFromValues(0,0,0,0,0,0);
72  }
73 }
74 
75 /*---------------------------------------------------------------
76  getCovarianceAndMean
77  ---------------------------------------------------------------*/
79 {
80  size_t N = m_modes.size();
81 
82  getMean(mean);
84 
85  if (N)
86  {
87  // 1) Get the mean:
88  double sumW = 0;
89  mrpt::math::CMatrixDouble estMean( mean );
90 
93  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
94  {
95  double w;
96  sumW += w = exp((it)->log_w);
97  estMean_i = mrpt::math::CMatrixDouble61((it)->val.mean);
98  MMt.multiply_AAt(estMean_i);
99  MMt+=(it)->val.cov;
100  MMt*=w;
101  estCov += MMt; //w * ( (it)->val.cov + ((estMean_i-estMean)*(~(estMean_i-estMean))) );
102  }
103 
104  if (sumW!=0)
105  estCov *= (1.0/sumW);
106  }
107 
108  estCovOut = estCov;
109 }
110 
111 /*---------------------------------------------------------------
112  writeToStream
113  ---------------------------------------------------------------*/
115 {
116  if (version)
117  *version = 2;
118  else
119  {
120  uint32_t N = m_modes.size();
121  out << N;
122  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
123  {
124  out << (it)->log_w;
125  out << (it)->val.mean;
126  out << (it)->val.cov;
127  }
128  }
129 }
130 
131 /*---------------------------------------------------------------
132  readFromStream
133  ---------------------------------------------------------------*/
135 {
136  switch(version)
137  {
138  case 0:
139  case 1:
140  case 2:
141  {
142  uint32_t N;
143  in >> N;
144  this->resize(N);
145 
146  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
147  {
148  in >> (it)->log_w;
149 
150  // In version 0, weights were linear!!
151  if (version==0) (it)->log_w = log(max(1e-300,(it)->log_w));
152 
153  in >> (it)->val.mean;
154 
155 
156  if (version==1) // were floats
157  {
158  THROW_EXCEPTION("Unsupported serialized version: too old")
159  }
160  else
161  {
162  in >> (it)->val.cov;
163  }
164  }
165  } break;
166  default:
168 
169  };
170 }
171 
173 {
174  MRPT_START
175 
176  if (this == &o) return; // It may be used sometimes
177 
179  {
180  *this = *static_cast<const CPose3DPDFSOG*>(&o);
181  }
182  else
183  {
184  this->resize(1);
185  m_modes[0].log_w = 0;
187  o.getCovarianceAndMean( C, m_modes[0].val.mean );
188  m_modes[0].val.cov = C;
189  }
190 
191  MRPT_END
192 }
193 
194 /*---------------------------------------------------------------
195  saveToTextFile
196  ---------------------------------------------------------------*/
198 {
199  FILE *f=os::fopen(file.c_str(),"wt");
200  if (!f) return;
201 
202 
203  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
204  os::fprintf(f,"%e %e %e %e %e %e %e %e %e %e\n",
205  exp((it)->log_w),
206  (it)->val.mean.x(), (it)->val.mean.y(), (it)->val.mean.z(),
207  (it)->val.cov(0,0),(it)->val.cov(1,1),(it)->val.cov(2,2),
208  (it)->val.cov(0,1),(it)->val.cov(0,2),(it)->val.cov(1,2) );
209  os::fclose(f);
210 }
211 
212 /*---------------------------------------------------------------
213  changeCoordinatesReference
214  ---------------------------------------------------------------*/
216 {
217  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
218  (it)->val.changeCoordinatesReference( newReferenceBase );
219 }
220 
221 /*---------------------------------------------------------------
222  bayesianFusion
223  ---------------------------------------------------------------*/
225 {
226  MRPT_START
227 
228  // p1: CPose3DPDFSOG, p2: CPosePDFGaussian:
229 
232 
233  THROW_EXCEPTION("TODO!!!");
234 #if 0
235 /*
236  CPose3DPDFSOG *p1 = (CPose3DPDFSOG*)&p1_;
237  CPose3DPDFSOG *p2 = (CPose3DPDFSOG*)&p2_;
238 
239  // Compute the new kernel means, covariances, and weights after multiplying to the Gaussian "p2":
240  CPosePDFGaussian auxGaussianProduct,auxSOG_Kernel_i;
241  TGaussianMode newKernel;
242 
243 
244 
245  CMatrixD covInv( p2->cov.inv() );
246  CMatrixD eta(3,1);
247  eta(0,0) = p2->mean.x;
248  eta(1,0) = p2->mean.y;
249  eta(2,0) = p2->mean.phi;
250  eta = covInv * eta;
251 
252  // Normal distribution canonical form constant:
253  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
254  double a = -0.5*( 3*log(M_2PI) - log( covInv.det() ) + (~eta * p2->cov * eta)(0,0) );
255 
256  this->m_modes.clear();
257  for (std::deque<TGaussianMode>::iterator it =p1->m_modes.begin();it!=p1->m_modes.end();++it)
258  {
259  auxSOG_Kernel_i.mean = it->mean;
260  auxSOG_Kernel_i.cov = it->cov;
261  auxGaussianProduct.bayesianFusion( auxSOG_Kernel_i, *p2 );
262 
263  // ----------------------------------------------------------------------
264  // The new weight is given by:
265  //
266  // w'_i = w_i * exp( a + a_i - a' )
267  //
268  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1)) + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
269  //
270  // ----------------------------------------------------------------------
271  newKernel.mean = auxGaussianProduct.mean;
272  newKernel.cov = auxGaussianProduct.cov;
273 
274  CMatrixD covInv_i( auxSOG_Kernel_i.cov.inv() );
275  CMatrixD eta_i(3,1);
276  eta_i(0,0) = auxSOG_Kernel_i.mean.x;
277  eta_i(1,0) = auxSOG_Kernel_i.mean.y;
278  eta_i(2,0) = auxSOG_Kernel_i.mean.phi;
279  eta_i = covInv_i * eta_i;
280 
281  CMatrixD new_covInv_i( newKernel.cov.inv() );
282  CMatrixD new_eta_i(3,1);
283  new_eta_i(0,0) = newKernel.mean.x;
284  new_eta_i(1,0) = newKernel.mean.y;
285  new_eta_i(2,0) = newKernel.mean.phi;
286  new_eta_i = new_covInv_i * new_eta_i;
287 
288  double a_i = -0.5*( 3*log(M_2PI) - log( new_covInv_i.det() ) + (~eta_i * auxSOG_Kernel_i.cov * eta_i)(0,0) );
289  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) );
290 
291  newKernel.w = it->w * exp( a + a_i - new_a_i );
292 
293  // Add to the results (in "this") the new kernel:
294  this->m_modes.push_back( newKernel );
295  }
296 */
297  normalizeWeights();
298 #endif
299  MRPT_END
300 }
301 
302 /*---------------------------------------------------------------
303  assureSymmetry
304  ---------------------------------------------------------------*/
306 {
307  MRPT_START
308  // Differences, when they exist, appear in the ~15'th significant
309  // digit, so... just take one of them arbitrarily!
310  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
311  {
312  for (size_t i=0;i<6;i++)
313  for (size_t j=i+1;j<6;j++)
314  (it)->val.cov.get_unsafe(i,j) = (it)->val.cov.get_unsafe(j,i);
315  }
316 
317  MRPT_END
318 }
319 
320 /*---------------------------------------------------------------
321  normalizeWeights
322  ---------------------------------------------------------------*/
324 {
325  MRPT_START
326 
327  if (m_modes.empty()) return;
328 
329  double maxW = m_modes[0].log_w;
330  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
331  maxW = max(maxW,(it)->log_w);
332 
333  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
334  (it)->log_w -= maxW;
335 
336  MRPT_END
337 }
338 
339 /*---------------------------------------------------------------
340  drawSingleSample
341  ---------------------------------------------------------------*/
343 {
344  MRPT_UNUSED_PARAM(outPart);
345  THROW_EXCEPTION("TO DO!");
346 }
347 
348 /*---------------------------------------------------------------
349  drawManySamples
350  ---------------------------------------------------------------*/
351 void CPose3DPDFSOG::drawManySamples( size_t N, std::vector<CVectorDouble> & outSamples ) const
352 {
353  MRPT_UNUSED_PARAM(N); MRPT_UNUSED_PARAM(outSamples);
354  THROW_EXCEPTION("TO DO!");
355 }
356 
357 /*---------------------------------------------------------------
358  inverse
359  ---------------------------------------------------------------*/
361 {
362  MRPT_START
364  CPose3DPDFSOG *out = static_cast<CPose3DPDFSOG*>( &o );
365 
366  // Prepare the output SOG:
367  out->resize(m_modes.size());
368 
369  const_iterator it;
370  iterator outIt;
371 
372  for (it=m_modes.begin(),outIt=out->m_modes.begin();it!=m_modes.end();it++,outIt++)
373  {
374  (it)->val.inverse( (outIt)->val );
375  (outIt)->log_w = (it)->log_w;
376  }
377 
378 
379  MRPT_END
380 }
381 
382 /*---------------------------------------------------------------
383  appendFrom
384  ---------------------------------------------------------------*/
386 {
387  MRPT_START
388 
389  ASSERT_( &o != this ); // Don't be bad...
390  if (o.m_modes.empty()) return;
391 
392  // Make copies:
393  for (const_iterator it = o.m_modes.begin();it!=o.m_modes.end();++it)
394  m_modes.push_back( *it );
395 
396  normalizeWeights();
397  MRPT_END
398 }
399 
400 
401 /*---------------------------------------------------------------
402  getMostLikelyMode
403  ---------------------------------------------------------------*/
405 {
406  if (this->empty())
407  {
408  outVal = CPose3DPDFGaussian();
409  }
410  else
411  {
412  const_iterator it_best = m_modes.end();
413  for (const_iterator it = m_modes.begin();it!=m_modes.end();++it)
414  if (it_best==m_modes.end() || it->log_w>it_best->log_w)
415  it_best = it;
416 
417  outVal = it_best->val;
418  }
419 }
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:58
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
void appendFrom(const CPose3DPDFSOG &o)
Append the Gaussian modes from "o" to the current set of modes of "this" density. ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
GLuint in
Definition: glew.h:7146
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void getMostLikelyMode(CPose3DPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
EIGEN_STRONG_INLINE bool empty() const
TModesList m_modes
Access directly to this array for modify the modes as desired.
Definition: CPose3DPDFSOG.h:69
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:57
void inverse(CPose3DPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) MRPT_OVERRIDE
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
#define MRPT_START
Computes weighted and un-weighted averages of SE(3) poses.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void clear()
Clear all the gaussian modes.
This file implements matrix/vector text and binary serialization.
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:254
#define ASSERT_(f)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const MRPT_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.
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...
GLuint GLfloat * val
Definition: glew.h:7785
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
unsigned __int32 uint32_t
Definition: rptypes.h:49
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
Definition: eigen_frwds.h:56
void drawSingleSample(CPose3D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
void resize(const size_t N)
Set the number of SOG modes.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018