Main MRPT website > C++ reference for MRPT 1.5.6
CPosePDFGrid.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 
12 
14 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/utils/CStream.h>
17 #include <mrpt/random.h>
18 #include <mrpt/system/os.h>
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::utils;
24 using namespace mrpt::math;
25 using namespace mrpt::poses;
26 using namespace mrpt::system;
27 
29 
30 /*---------------------------------------------------------------
31  Constructor
32  ---------------------------------------------------------------*/
34  double xMin,
35  double xMax,
36  double yMin,
37  double yMax,
38  double resolutionXY,
39  double resolutionPhi,
40  double phiMin,
41  double phiMax
42  ) : CPose2DGridTemplate<double>(xMin,xMax,yMin,yMax,resolutionXY,resolutionPhi,phiMin,phiMax)
43 {
44  uniformDistribution();
45 }
46 
47 void CPosePDFGrid::copyFrom(const CPosePDF &o)
48 {
49  if (this == &o) return; // It may be used sometimes
50 
51  THROW_EXCEPTION("Not implemented yet!");
52 
53 }
54 
55 /*---------------------------------------------------------------
56  Destructor
57  ---------------------------------------------------------------*/
58 CPosePDFGrid::~CPosePDFGrid( )
59 {
60 
61 }
62 
63 /*---------------------------------------------------------------
64  getMean
65  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed
66  as a weighted average over all particles.
67  ---------------------------------------------------------------*/
68 void CPosePDFGrid::getMean(CPose2D &p) const
69 {
70  // Calc average on SE(2)
71  mrpt::poses::SE_average<2> se_averager;
72  for (size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
73  for (size_t y=0;y<m_sizeY;y++)
74  for (size_t x=0;x<m_sizeX;x++)
75  {
76  const double w = *getByIndex(x,y,phiInd);
77  se_averager.append( CPose2D( idx2x(x),idx2y(y), idx2phi(phiInd) ),w );
78  }
79  se_averager.get_average(p);
80 }
81 
82 /*---------------------------------------------------------------
83  getCovarianceAndMean
84  ---------------------------------------------------------------*/
85 void CPosePDFGrid::getCovarianceAndMean(CMatrixDouble33 &cov, CPose2D &p) const
86 {
87  CPosePDFParticles auxParts;
88  auxParts.resetDeterministic( CPose2D(0,0,0), m_sizePhi*m_sizeY*m_sizeX );
89  size_t idx=0;
90  for (size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
91  {
92  for (size_t y=0;y<m_sizeY;y++)
93  for (size_t x=0;x<m_sizeX;x++)
94  {
95  auxParts.m_particles[idx].log_w = log( *getByIndex(x,y,phiInd) );
96  *auxParts.m_particles[idx].d = CPose2D( idx2x(x),idx2y(y), idx2phi(phiInd) );
97  }
98  }
99  auxParts.getCovarianceAndMean(cov,p);
100 }
101 
102 /*---------------------------------------------------------------
103  writeToStream
104  ---------------------------------------------------------------*/
105 void CPosePDFGrid::writeToStream(mrpt::utils::CStream &out,int *version) const
106 {
107  if (version)
108  *version = 0;
109  else
110  {
111  // The size:
112  out << m_xMin << m_xMax
113  << m_yMin << m_yMax
114  << m_phiMin << m_phiMax
115  << m_resolutionXY << m_resolutionPhi
116  << static_cast<int32_t>(m_sizeX) << static_cast<int32_t>(m_sizeY) << static_cast<int32_t>(m_sizePhi) << static_cast<int32_t>(m_sizeXY)
117  << static_cast<int32_t>(m_idxLeftX) << static_cast<int32_t>(m_idxLeftY) << static_cast<int32_t>(m_idxLeftPhi);
118 
119  // The data:
120  out << m_data;
121  }
122 }
123 
124 /*---------------------------------------------------------------
125  readFromStream
126  ---------------------------------------------------------------*/
127 void CPosePDFGrid::readFromStream(mrpt::utils::CStream &in, int version)
128 {
129  switch(version)
130  {
131  case 0:
132  {
133  // The size:
134  in >> m_xMin >> m_xMax
135  >> m_yMin >> m_yMax
136  >> m_phiMin >> m_phiMax
137  >> m_resolutionXY >> m_resolutionPhi;
138 
139  int32_t sizeX,sizeY,sizePhi,sizeXY,idxLeftX,idxLeftY,idxLeftPhi;
140 
141  in >> sizeX >> sizeY >> sizePhi >> sizeXY >> idxLeftX >> idxLeftY >> idxLeftPhi;
142 
143  m_sizeX = sizeX;
144  m_sizeY = sizeY;
145  m_sizePhi = sizePhi;
146  m_sizeXY = sizeXY;
147  m_idxLeftX= idxLeftX;
148  m_idxLeftY= idxLeftY;
149  m_idxLeftPhi=idxLeftPhi;
150 
151  // The data:
152  in >> m_data;
153  } break;
154  default:
156 
157  };
158 }
159 
160 /*---------------------------------------------------------------
161  saveToTextFile
162  ---------------------------------------------------------------*/
164  const std::string &dataFile) const
165 {
166  char dimsFile[1000];
167  os::sprintf(dimsFile,1000,"%s_dims.txt",dataFile.c_str());
168 
169  FILE *f_d = os::fopen(dataFile.c_str(),"wt");
170  if (!f_d) return;
171 
172  FILE *f_s = os::fopen(dimsFile,"wt");
173  if (!f_s)
174  {
175  os::fclose(f_d);
176  return;
177  }
178 
179  // Save dims:
180  os::fprintf(f_s,"%u %u %u %f %f %f %f %f %f\n",
181  (unsigned)m_sizeX,
182  (unsigned)m_sizeY,
183  (unsigned)m_sizePhi,
184  m_xMin, m_xMax,
185  m_yMin, m_yMax,
186  m_phiMin, m_phiMax
187  );
188 
189 
190  // Save one rectangular matrix each time:
191  for (unsigned int phiInd = 0; phiInd < m_sizePhi; phiInd++)
192  {
193  for (unsigned int y=0;y<m_sizeY;y++)
194  {
195  for (unsigned int x=0;x<m_sizeX;x++)
196  os::fprintf(f_d,"%.5e ", *getByIndex(x,y,phiInd) );
197  os::fprintf(f_d,"\n");
198  }
199  }
200 
201  os::fclose(f_s);
202  os::fclose(f_d);
203  return; // Done!
204 }
205 
206 /*---------------------------------------------------------------
207  changeCoordinatesReference
208  ---------------------------------------------------------------*/
209 void CPosePDFGrid::changeCoordinatesReference(const CPose3D &newReferenceBase )
210 {
211  MRPT_UNUSED_PARAM(newReferenceBase);
212  THROW_EXCEPTION("Not implemented yet!");
213 }
214 
215 /*---------------------------------------------------------------
216  bayesianFusion
217  ---------------------------------------------------------------*/
218 void CPosePDFGrid::bayesianFusion(const CPosePDF &p1,const CPosePDF &p2, const double &minMahalanobisDistToDrop )
219 {
220  MRPT_UNUSED_PARAM(p1);MRPT_UNUSED_PARAM(p2);MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
221  THROW_EXCEPTION("Not implemented yet!");
222 }
223 
224 /*---------------------------------------------------------------
225  inverse
226  ---------------------------------------------------------------*/
228 {
230  THROW_EXCEPTION("Not implemented yet!");
231 }
232 
233 /*---------------------------------------------------------------
234  drawSingleSample
235  ---------------------------------------------------------------*/
236 void CPosePDFGrid::drawSingleSample( CPose2D &outPart ) const
237 {
238  MRPT_UNUSED_PARAM(outPart);
239  THROW_EXCEPTION("Not implemented yet!");
240 }
241 
242 /*---------------------------------------------------------------
243  drawSingleSample
244  ---------------------------------------------------------------*/
245 void CPosePDFGrid::drawManySamples(
246  size_t N,
247  std::vector<CVectorDouble> &outSamples ) const
248 {
249  MRPT_UNUSED_PARAM(N); MRPT_UNUSED_PARAM(outSamples);
250 
251  THROW_EXCEPTION("Not implemented yet!");
252 }
253 
254 /*---------------------------------------------------------------
255  CPosePDFGrid
256  ---------------------------------------------------------------*/
258 {
259  double SUM = 0;
260 
261  // SUM:
262  for (vector<double>::const_iterator it=m_data.begin();it!=m_data.end();++it) SUM += *it;
263 
264  if (SUM>0)
265  {
266  // Normalize:
267  for (vector<double>::iterator it=m_data.begin();it!=m_data.end();++it) *it /= SUM;
268  }
269 }
270 
271 /*---------------------------------------------------------------
272  uniformDistribution
273  ---------------------------------------------------------------*/
274 void CPosePDFGrid::uniformDistribution()
275 {
276  double val = 1.0f / m_data.size();
277 
278  for (vector<double>::iterator it=m_data.begin();it!=m_data.end();++it)
279  *it = val;
280 }
Computes weighted and un-weighted averages of SE(2) poses.
Definition: SO_SE_average.h:88
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
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
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
GLuint in
Definition: glew.h:7146
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CParticleList m_particles
The array of particles.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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:135
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
int version
Definition: mrpt_jpeglib.h:898
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLfloat GLfloat p
Definition: glew.h:10113
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
__int32 int32_t
Definition: rptypes.h:48
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void resetDeterministic(const CPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
int BASE_IMPEXP sprintf(char *buf, size_t bufSize, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:191
GLuint GLfloat * val
Definition: glew.h:7785
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
Definition: CPosePDFGrid.h:30
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.
This is a template class for storing a 3D (2D+heading) grid containing any kind of data...



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