MRPT  2.0.1
CPosePDFGrid.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/poses/CPose3D.h>
16 #include <mrpt/random.h>
18 #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() = default;
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 std::tuple<CMatrixDouble33, CPose2D> CPosePDFGrid::getCovarianceAndMean() const
74 {
75  CPosePDFParticles auxParts;
76  auxParts.resetDeterministic(
77  TPose2D(0, 0, 0), m_sizePhi * m_sizeY * m_sizeX);
78  size_t idx = 0;
79  for (size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
80  {
81  for (size_t y = 0; y < m_sizeY; y++)
82  for (size_t x = 0; x < m_sizeX; x++)
83  {
84  auxParts.m_particles[idx].log_w =
85  log(*getByIndex(x, y, phiInd));
86  auxParts.m_particles[idx].d =
87  TPose2D(idx2x(x), idx2y(y), idx2phi(phiInd));
88  }
89  }
90  return auxParts.getCovarianceAndMean();
91 }
92 
93 uint8_t CPosePDFGrid::serializeGetVersion() const { return 0; }
94 void CPosePDFGrid::serializeTo(mrpt::serialization::CArchive& out) const
95 {
96  // The size:
97  out << m_xMin << m_xMax << m_yMin << m_yMax << m_phiMin << m_phiMax
98  << m_resolutionXY << m_resolutionPhi << static_cast<int32_t>(m_sizeX)
99  << static_cast<int32_t>(m_sizeY) << static_cast<int32_t>(m_sizePhi)
100  << static_cast<int32_t>(m_sizeXY) << static_cast<int32_t>(m_idxLeftX)
101  << static_cast<int32_t>(m_idxLeftY)
102  << static_cast<int32_t>(m_idxLeftPhi);
103 
104  // The data:
105  out << m_data;
106 }
107 void CPosePDFGrid::serializeFrom(
108  mrpt::serialization::CArchive& in, uint8_t version)
109 {
110  switch (version)
111  {
112  case 0:
113  {
114  // The size:
115  in >> m_xMin >> m_xMax >> m_yMin >> m_yMax >> m_phiMin >>
116  m_phiMax >> m_resolutionXY >> m_resolutionPhi;
117 
118  int32_t sizeX, sizeY, sizePhi, sizeXY, idxLeftX, idxLeftY,
119  idxLeftPhi;
120 
121  in >> sizeX >> sizeY >> sizePhi >> sizeXY >> idxLeftX >> idxLeftY >>
122  idxLeftPhi;
123 
124  m_sizeX = sizeX;
125  m_sizeY = sizeY;
126  m_sizePhi = sizePhi;
127  m_sizeXY = sizeXY;
128  m_idxLeftX = idxLeftX;
129  m_idxLeftY = idxLeftY;
130  m_idxLeftPhi = idxLeftPhi;
131 
132  // The data:
133  in >> m_data;
134  }
135  break;
136  default:
138  };
139 }
140 
141 bool CPosePDFGrid::saveToTextFile(const std::string& dataFile) const
142 {
143  const auto dimsFile = dataFile + std::string("_dims.txt");
144 
145  std::ofstream f_d(dataFile), f_s(dimsFile);
146  if (!f_d.is_open() || !f_s.is_open()) return false;
147 
148  // Save dims:
149  f_s << mrpt::format(
150  "%u %u %u %f %f %f %f %f %f\n", (unsigned)m_sizeX, (unsigned)m_sizeY,
151  (unsigned)m_sizePhi, m_xMin, m_xMax, m_yMin, m_yMax, m_phiMin,
152  m_phiMax);
153 
154  // Save one rectangular matrix each time:
155  for (unsigned int phiInd = 0; phiInd < m_sizePhi; phiInd++)
156  {
157  for (unsigned int y = 0; y < m_sizeY; y++)
158  {
159  for (unsigned int x = 0; x < m_sizeX; x++)
160  f_d << mrpt::format("%.5e ", *getByIndex(x, y, phiInd));
161  f_d << std::endl;
162  }
163  }
164 
165  return true; // Done!
166 }
167 
168 /*---------------------------------------------------------------
169  changeCoordinatesReference
170  ---------------------------------------------------------------*/
171 void CPosePDFGrid::changeCoordinatesReference([
172  [maybe_unused]] const CPose3D& newReferenceBase)
173 {
174  THROW_EXCEPTION("Not implemented yet!");
175 }
176 
177 /*---------------------------------------------------------------
178  bayesianFusion
179  ---------------------------------------------------------------*/
180 void CPosePDFGrid::bayesianFusion(
181  [[maybe_unused]] const CPosePDF& p1, [[maybe_unused]] const CPosePDF& p2,
182  [[maybe_unused]] const double minMahalanobisDistToDrop)
183 {
184  THROW_EXCEPTION("Not implemented yet!");
185 }
186 
187 /*---------------------------------------------------------------
188  inverse
189  ---------------------------------------------------------------*/
190 void CPosePDFGrid::inverse([[maybe_unused]] CPosePDF& o) const
191 {
192  THROW_EXCEPTION("Not implemented yet!");
193 }
194 
195 /*---------------------------------------------------------------
196  drawSingleSample
197  ---------------------------------------------------------------*/
198 void CPosePDFGrid::drawSingleSample([[maybe_unused]] CPose2D& outPart) const
199 {
200  THROW_EXCEPTION("Not implemented yet!");
201 }
202 
203 /*---------------------------------------------------------------
204  drawSingleSample
205  ---------------------------------------------------------------*/
206 void CPosePDFGrid::drawManySamples(
207  [[maybe_unused]] size_t N,
208  [[maybe_unused]] std::vector<CVectorDouble>& outSamples) const
209 {
210  THROW_EXCEPTION("Not implemented yet!");
211 }
212 
213 /*---------------------------------------------------------------
214  CPosePDFGrid
215  ---------------------------------------------------------------*/
217 {
218  double SUM = 0;
219 
220  // SUM:
221  for (auto it = m_data.begin(); it != m_data.end(); ++it) SUM += *it;
222 
223  if (SUM > 0)
224  {
225  // Normalize:
226  for (double& it : m_data) it /= SUM;
227  }
228 }
229 
230 /*---------------------------------------------------------------
231  uniformDistribution
232  ---------------------------------------------------------------*/
233 void CPosePDFGrid::uniformDistribution()
234 {
235  double val = 1.0f / m_data.size();
236 
237  for (double& it : m_data) it = val;
238 }
Computes weighted and un-weighted averages of SE(2) poses.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
CParticleList m_particles
The array of particles.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This base provides a set of functions for maths stuff.
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
int val
Definition: mrpt_jpeglib.h:957
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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:54
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Lightweight 2D pose.
Definition: TPose2D.h:22
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
Definition: CPosePDFGrid.h:24
This is a template class for storing a 3D (2D+heading) grid containing any kind of data...
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020