MRPT  2.0.4
CPose3DPDFGrid.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 
30  const mrpt::math::TPose3D& bb_min, const mrpt::math::TPose3D& bb_max,
31  double resolution_XYZ, double resolution_YPR)
32  : CPose3DGridTemplate<double>(
33  bb_min, bb_max, resolution_XYZ, resolution_YPR)
34 {
35  uniformDistribution();
36 }
37 
38 void CPose3DPDFGrid::copyFrom(const CPose3DPDF& o)
39 {
40  if (this == &o) return; // It may be used sometimes
41 
42  THROW_EXCEPTION("Not implemented yet!");
43 }
44 
45 void CPose3DPDFGrid::getMean(CPose3D& p) const
46 {
47  // Calc average on SE(3)
48  mrpt::poses::SE_average<3> se_averager;
49 
50  for (size_t cR = 0; cR < m_sizeRoll; cR++)
51  for (size_t cP = 0; cP < m_sizePitch; cP++)
52  for (size_t cY = 0; cY < m_sizeYaw; cY++)
53  for (size_t cz = 0; cz < m_sizeZ; cz++)
54  for (size_t cy = 0; cy < m_sizeY; cy++)
55  for (size_t cx = 0; cx < m_sizeX; cx++)
56  {
57  const double w =
58  *getByIndex(cx, cy, cz, cY, cP, cR);
59  se_averager.append(
60  CPose3D(
61  idx2x(cx), idx2y(cy), idx2z(cz),
62  idx2yaw(cY), idx2pitch(cP), idx2roll(cR)),
63  w);
64  }
65  se_averager.get_average(p);
66 }
67 
68 std::tuple<CMatrixDouble66, CPose3D> CPose3DPDFGrid::getCovarianceAndMean()
69  const
70 {
71  CPose3DPDFParticles auxParts;
72  auxParts.resetDeterministic(TPose3D(), m_size_xyzYPR);
73 
74  size_t idx = 0;
75  for (size_t cR = 0; cR < m_sizeRoll; cR++)
76  for (size_t cP = 0; cP < m_sizePitch; cP++)
77  for (size_t cY = 0; cY < m_sizeYaw; cY++)
78  for (size_t cz = 0; cz < m_sizeZ; cz++)
79  for (size_t cy = 0; cy < m_sizeY; cy++)
80  for (size_t cx = 0; cx < m_sizeX; cx++)
81  {
82  const double w =
83  *getByIndex(cx, cy, cz, cY, cP, cR);
84  auxParts.m_particles[idx].log_w = std::log(w);
85  auxParts.m_particles[idx].d = mrpt::math::TPose3D(
86  idx2x(cx), idx2y(cy), idx2z(cz), idx2yaw(cY),
87  idx2pitch(cP), idx2roll(cR));
88 
89  ++idx;
90  }
91  return auxParts.getCovarianceAndMean();
92 }
93 
94 uint8_t CPose3DPDFGrid::serializeGetVersion() const { return 0; }
95 void CPose3DPDFGrid::serializeTo(mrpt::serialization::CArchive& out) const
96 {
97  // The size:
98  out << m_bb_min << m_bb_max << m_resolutionXYZ << m_resolutionYPR;
99  out.WriteAs<int32_t>(m_sizeX);
100  out.WriteAs<int32_t>(m_sizeY);
101  out.WriteAs<int32_t>(m_sizeZ);
102  out.WriteAs<int32_t>(m_sizeYaw);
103  out.WriteAs<int32_t>(m_sizePitch);
104  out.WriteAs<int32_t>(m_sizeRoll);
105  out << m_sizeX << m_sizeY << m_sizeZ << m_sizeYaw << m_sizePitch
106  << m_sizeRoll;
107  out << m_min_cidX << m_min_cidY << m_min_cidZ << m_min_cidYaw
108  << m_min_cidPitch << m_min_cidRoll;
109 
110  // The data:
111  out << m_data;
112 }
113 void CPose3DPDFGrid::serializeFrom(
114  mrpt::serialization::CArchive& in, uint8_t version)
115 {
116  switch (version)
117  {
118  case 0:
119  {
120  in >> m_bb_min >> m_bb_max >> m_resolutionXYZ >> m_resolutionYPR;
121  m_sizeX = in.ReadAs<int32_t>();
122  m_sizeY = in.ReadAs<int32_t>();
123  m_sizeZ = in.ReadAs<int32_t>();
124  m_sizeYaw = in.ReadAs<int32_t>();
125  m_sizePitch = in.ReadAs<int32_t>();
126  m_sizeRoll = in.ReadAs<int32_t>();
127  in >> m_sizeX >> m_sizeY >> m_sizeZ >> m_sizeYaw >> m_sizePitch >>
128  m_sizeRoll;
129  in >> m_min_cidX >> m_min_cidY >> m_min_cidZ >> m_min_cidYaw >>
130  m_min_cidPitch >> m_min_cidRoll;
131 
132  // The data:
133  in >> m_data;
134 
135  update_cached_size_products();
136 
137  ASSERT_EQUAL_(m_data.size(), m_size_xyzYPR);
138  }
139  break;
140  default:
142  };
143 }
144 
145 bool CPose3DPDFGrid::saveToTextFile(const std::string& dataFile) const
146 {
147  THROW_EXCEPTION("Not implemented yet");
148 
149  // return true; // Done!
150 }
151 
152 void CPose3DPDFGrid::changeCoordinatesReference([
153  [maybe_unused]] const CPose3D& newReferenceBase)
154 {
155  THROW_EXCEPTION("Not implemented yet!");
156 }
157 
158 void CPose3DPDFGrid::bayesianFusion(
159  [[maybe_unused]] const CPose3DPDF& p1,
160  [[maybe_unused]] const CPose3DPDF& p2)
161 {
162  THROW_EXCEPTION("Not implemented yet!");
163 }
164 
165 void CPose3DPDFGrid::inverse([[maybe_unused]] CPose3DPDF& o) const
166 {
167  THROW_EXCEPTION("Not implemented yet!");
168 }
169 
170 void CPose3DPDFGrid::drawSingleSample([[maybe_unused]] CPose3D& outPart) const
171 {
172  THROW_EXCEPTION("Not implemented yet!");
173 }
174 
175 void CPose3DPDFGrid::drawManySamples(
176  [[maybe_unused]] size_t N,
177  [[maybe_unused]] std::vector<CVectorDouble>& outSamples) const
178 {
179  THROW_EXCEPTION("Not implemented yet!");
180 }
181 
183 {
184  double SUM = 0;
185 
186  // SUM:
187  for (auto it = m_data.begin(); it != m_data.end(); ++it) SUM += *it;
188 
189  if (SUM > 0)
190  {
191  const auto f = 1.0 / SUM;
192  for (double& it : m_data) it *= f;
193  }
194 }
195 
196 void CPose3DPDFGrid::uniformDistribution()
197 {
198  const double val = 1.0 / m_data.size();
199 
200  for (double& it : m_data) it = val;
201 }
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
This is a template class for storing a 6-dimensional grid, with components corresponding to Euler ang...
#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...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
int val
Definition: mrpt_jpeglib.h:957
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
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.
Computes weighted and un-weighted averages of SE(3) poses.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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:85
mrpt::vision::TStereoCalibResults out
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x...
const auto bb_max
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
const auto bb_min
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
Declares a class that represents a Probability Density function (PDF) of a 3D pose.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020