Main MRPT website > C++ reference for MRPT 1.9.9
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-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/poses/CPose3D.h>
16 #include <mrpt/random.h>
17 #include <mrpt/system/os.h>
19 #include <fstream>
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::math;
24 using namespace mrpt::poses;
25 using namespace mrpt::system;
26 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33  double xMin, double xMax, double yMin, double yMax, double resolutionXY,
34  double resolutionPhi, double phiMin, double phiMax)
35  : CPose2DGridTemplate<double>(
36  xMin, xMax, yMin, yMax, resolutionXY, resolutionPhi, phiMin, phiMax)
37 {
38  uniformDistribution();
39 }
40 
41 void CPosePDFGrid::copyFrom(const CPosePDF& o)
42 {
43  if (this == &o) return; // It may be used sometimes
44 
45  THROW_EXCEPTION("Not implemented yet!");
46 }
47 
48 /*---------------------------------------------------------------
49  Destructor
50  ---------------------------------------------------------------*/
51 CPosePDFGrid::~CPosePDFGrid() {}
52 /*---------------------------------------------------------------
53  getMean
54  Returns an estimate of the pose, (the mean, or mathematical expectation of the
55  PDF), computed
56  as a weighted average over all particles.
57  ---------------------------------------------------------------*/
58 void CPosePDFGrid::getMean(CPose2D& p) const
59 {
60  // Calc average on SE(2)
61  mrpt::poses::SE_average<2> se_averager;
62  for (size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
63  for (size_t y = 0; y < m_sizeY; y++)
64  for (size_t x = 0; x < m_sizeX; x++)
65  {
66  const double w = *getByIndex(x, y, phiInd);
67  se_averager.append(
68  CPose2D(idx2x(x), idx2y(y), idx2phi(phiInd)), w);
69  }
70  se_averager.get_average(p);
71 }
72 
73 /*---------------------------------------------------------------
74  getCovarianceAndMean
75  ---------------------------------------------------------------*/
76 void CPosePDFGrid::getCovarianceAndMean(CMatrixDouble33& cov, CPose2D& p) const
77 {
78  CPosePDFParticles auxParts;
79  auxParts.resetDeterministic(
80  TPose2D(0, 0, 0), m_sizePhi * m_sizeY * m_sizeX);
81  size_t idx = 0;
82  for (size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
83  {
84  for (size_t y = 0; y < m_sizeY; y++)
85  for (size_t x = 0; x < m_sizeX; x++)
86  {
87  auxParts.m_particles[idx].log_w =
88  log(*getByIndex(x, y, phiInd));
89  auxParts.m_particles[idx].d =
90  TPose2D(idx2x(x), idx2y(y), idx2phi(phiInd));
91  }
92  }
93  auxParts.getCovarianceAndMean(cov, p);
94 }
95 
96 uint8_t CPosePDFGrid::serializeGetVersion() const { return 0; }
97 void CPosePDFGrid::serializeTo(mrpt::serialization::CArchive& out) const
98 {
99  // The size:
100  out << m_xMin << m_xMax << m_yMin << m_yMax << m_phiMin << m_phiMax
101  << m_resolutionXY << m_resolutionPhi << static_cast<int32_t>(m_sizeX)
102  << static_cast<int32_t>(m_sizeY) << static_cast<int32_t>(m_sizePhi)
103  << static_cast<int32_t>(m_sizeXY) << static_cast<int32_t>(m_idxLeftX)
104  << static_cast<int32_t>(m_idxLeftY)
105  << static_cast<int32_t>(m_idxLeftPhi);
106 
107  // The data:
108  out << m_data;
109 }
110 void CPosePDFGrid::serializeFrom(
112 {
113  switch (version)
114  {
115  case 0:
116  {
117  // The size:
118  in >> m_xMin >> m_xMax >> m_yMin >> m_yMax >> m_phiMin >>
119  m_phiMax >> m_resolutionXY >> m_resolutionPhi;
120 
121  int32_t sizeX, sizeY, sizePhi, sizeXY, idxLeftX, idxLeftY,
122  idxLeftPhi;
123 
124  in >> sizeX >> sizeY >> sizePhi >> sizeXY >> idxLeftX >> idxLeftY >>
125  idxLeftPhi;
126 
127  m_sizeX = sizeX;
128  m_sizeY = sizeY;
129  m_sizePhi = sizePhi;
130  m_sizeXY = sizeXY;
131  m_idxLeftX = idxLeftX;
132  m_idxLeftY = idxLeftY;
133  m_idxLeftPhi = idxLeftPhi;
134 
135  // The data:
136  in >> m_data;
137  }
138  break;
139  default:
141  };
142 }
143 
144 bool CPosePDFGrid::saveToTextFile(const std::string& dataFile) const
145 {
146  const auto dimsFile = dataFile + std::string("_dims.txt");
147 
148  std::ofstream f_d(dataFile), f_s(dimsFile);
149  if (!f_d.is_open() || !f_s.is_open()) return false;
150 
151  // Save dims:
152  f_s << mrpt::format(
153  "%u %u %u %f %f %f %f %f %f\n", (unsigned)m_sizeX, (unsigned)m_sizeY,
154  (unsigned)m_sizePhi, m_xMin, m_xMax, m_yMin, m_yMax, m_phiMin,
155  m_phiMax);
156 
157  // Save one rectangular matrix each time:
158  for (unsigned int phiInd = 0; phiInd < m_sizePhi; phiInd++)
159  {
160  for (unsigned int y = 0; y < m_sizeY; y++)
161  {
162  for (unsigned int x = 0; x < m_sizeX; x++)
163  f_d << mrpt::format("%.5e ", *getByIndex(x, y, phiInd));
164  f_d << std::endl;
165  }
166  }
167 
168  return true; // Done!
169 }
170 
171 /*---------------------------------------------------------------
172  changeCoordinatesReference
173  ---------------------------------------------------------------*/
174 void CPosePDFGrid::changeCoordinatesReference(const CPose3D& newReferenceBase)
175 {
176  MRPT_UNUSED_PARAM(newReferenceBase);
177  THROW_EXCEPTION("Not implemented yet!");
178 }
179 
180 /*---------------------------------------------------------------
181  bayesianFusion
182  ---------------------------------------------------------------*/
183 void CPosePDFGrid::bayesianFusion(
184  const CPosePDF& p1, const CPosePDF& p2,
185  const double minMahalanobisDistToDrop)
186 {
187  MRPT_UNUSED_PARAM(p1);
188  MRPT_UNUSED_PARAM(p2);
189  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
190  THROW_EXCEPTION("Not implemented yet!");
191 }
192 
193 /*---------------------------------------------------------------
194  inverse
195  ---------------------------------------------------------------*/
197 {
199  THROW_EXCEPTION("Not implemented yet!");
200 }
201 
202 /*---------------------------------------------------------------
203  drawSingleSample
204  ---------------------------------------------------------------*/
205 void CPosePDFGrid::drawSingleSample(CPose2D& outPart) const
206 {
207  MRPT_UNUSED_PARAM(outPart);
208  THROW_EXCEPTION("Not implemented yet!");
209 }
210 
211 /*---------------------------------------------------------------
212  drawSingleSample
213  ---------------------------------------------------------------*/
214 void CPosePDFGrid::drawManySamples(
215  size_t N, std::vector<CVectorDouble>& outSamples) const
216 {
218  MRPT_UNUSED_PARAM(outSamples);
219 
220  THROW_EXCEPTION("Not implemented yet!");
221 }
222 
223 /*---------------------------------------------------------------
224  CPosePDFGrid
225  ---------------------------------------------------------------*/
227 {
228  double SUM = 0;
229 
230  // SUM:
231  for (vector<double>::const_iterator it = m_data.begin(); it != m_data.end();
232  ++it)
233  SUM += *it;
234 
235  if (SUM > 0)
236  {
237  // Normalize:
238  for (vector<double>::iterator it = m_data.begin(); it != m_data.end();
239  ++it)
240  *it /= SUM;
241  }
242 }
243 
244 /*---------------------------------------------------------------
245  uniformDistribution
246  ---------------------------------------------------------------*/
247 void CPosePDFGrid::uniformDistribution()
248 {
249  double val = 1.0f / m_data.size();
250 
251  for (vector<double>::iterator it = m_data.begin(); it != m_data.end(); ++it)
252  *it = val;
253 }
Computes weighted and un-weighted averages of SE(2) poses.
Scalar * iterator
Definition: eigen_plugins.h:26
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
CParticleList m_particles
The array of particles.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const 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...
STL namespace.
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
void resetDeterministic(const mrpt::math::TPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
This base provides a set of functions for maths stuff.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
int val
Definition: mrpt_jpeglib.h:955
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLsizei const GLchar ** string
Definition: glext.h:4101
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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:46
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
GLenum GLint GLint y
Definition: glext.h:3538
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:148
GLenum GLint x
Definition: glext.h:3538
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
Definition: CPosePDFGrid.h:27
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
This is a template class for storing a 3D (2D+heading) grid containing any kind of data...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019