MRPT  1.9.9
CSimplePointsMap.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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/bits_mem.h>
15 
16 #include "CPointsMap_crtp_common.h"
17 
18 using namespace std;
19 using namespace mrpt;
20 using namespace mrpt::maps;
21 using namespace mrpt::obs;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 
25 // =========== Begin of Map definition ============
27  "mrpt::maps::CSimplePointsMap,pointsMap", mrpt::maps::CSimplePointsMap)
28 
29 CSimplePointsMap::TMapDefinition::TMapDefinition() = default;
30 void CSimplePointsMap::TMapDefinition::loadFromConfigFile_map_specific(
31  const mrpt::config::CConfigFileBase& c, const std::string& s)
32 {
33  insertionOpts.loadFromConfigFile(c, s + string("_insertOpts"));
34  likelihoodOpts.loadFromConfigFile(c, s + string("_likelihoodOpts"));
35  renderOpts.loadFromConfigFile(c, s + string("_renderOpts"));
36 }
37 
38 void CSimplePointsMap::TMapDefinition::dumpToTextStream_map_specific(
39  std::ostream& out) const
40 {
41  this->insertionOpts.dumpToTextStream(out);
42  this->likelihoodOpts.dumpToTextStream(out);
43  this->renderOpts.dumpToTextStream(out);
44 }
45 
46 mrpt::maps::CMetricMap* CSimplePointsMap::internal_CreateFromMapDefinition(
48 {
50  *dynamic_cast<const CSimplePointsMap::TMapDefinition*>(&_def);
51  auto* obj = new CSimplePointsMap();
52  obj->insertionOptions = def.insertionOpts;
53  obj->likelihoodOptions = def.likelihoodOpts;
54  obj->renderOptions = def.renderOpts;
55  return obj;
56 }
57 // =========== End of Map definition Block =========
59 
60 void CSimplePointsMap::reserve(size_t newLength)
61 {
62  m_x.reserve(newLength);
63  m_y.reserve(newLength);
64  m_z.reserve(newLength);
65 }
66 
67 // Resizes all point buffers so they can hold the given number of points: newly
68 // created points are set to default values,
69 // and old contents are not changed.
70 void CSimplePointsMap::resize(size_t newLength)
71 {
72  this->reserve(newLength); // to ensure 4N capacity
73  m_x.resize(newLength, 0);
74  m_y.resize(newLength, 0);
75  m_z.resize(newLength, 0);
76  mark_as_modified();
77 }
78 
79 // Resizes all point buffers so they can hold the given number of points,
80 // *erasing* all previous contents
81 // and leaving all points to default values.
82 void CSimplePointsMap::setSize(size_t newLength)
83 {
84  this->reserve(newLength); // to ensure 4N capacity
85  m_x.assign(newLength, 0);
86  m_y.assign(newLength, 0);
87  m_z.assign(newLength, 0);
88  mark_as_modified();
89 }
90 
91 void CSimplePointsMap::impl_copyFrom(const CPointsMap& obj)
92 {
93  // This also does a ::resize(N) of all data fields.
94  CPointsMap::base_copyFrom(obj);
95 }
96 
97 uint8_t CSimplePointsMap::serializeGetVersion() const { return 10; }
98 void CSimplePointsMap::serializeTo(mrpt::serialization::CArchive& out) const
99 {
100  uint32_t n = m_x.size();
101 
102  // First, write the number of points:
103  out << n;
104 
105  if (n > 0)
106  {
107  out.WriteBufferFixEndianness(&m_x[0], n);
108  out.WriteBufferFixEndianness(&m_y[0], n);
109  out.WriteBufferFixEndianness(&m_z[0], n);
110  }
111  out << genericMapParams; // v9
112  insertionOptions.writeToStream(out); // v9
113  likelihoodOptions.writeToStream(out); // v5
114  renderOptions.writeToStream(out); // v10
115 }
116 
117 /*---------------------------------------------------------------
118  readFromStream
119  Implements the reading from a CStream capability of
120  CSerializable objects
121  ---------------------------------------------------------------*/
122 void CSimplePointsMap::serializeFrom(
123  mrpt::serialization::CArchive& in, uint8_t version)
124 {
125  switch (version)
126  {
127  case 8:
128  case 9:
129  case 10:
130  {
131  mark_as_modified();
132 
133  // Read the number of points:
134  uint32_t n;
135  in >> n;
136 
137  this->resize(n);
138 
139  if (n > 0)
140  {
141  in.ReadBufferFixEndianness(&m_x[0], n);
142  in.ReadBufferFixEndianness(&m_y[0], n);
143  in.ReadBufferFixEndianness(&m_z[0], n);
144  }
145  if (version >= 9)
146  in >> genericMapParams;
147  else
148  {
149  bool disableSaveAs3DObject;
150  in >> disableSaveAs3DObject;
151  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
152  }
153  insertionOptions.readFromStream(in);
154  likelihoodOptions.readFromStream(in);
155  if (version >= 10) renderOptions.readFromStream(in);
156  }
157  break;
158 
159  case 0:
160  case 1:
161  case 2:
162  case 3:
163  case 4:
164  case 5:
165  case 6:
166  case 7:
167  {
168  mark_as_modified();
169 
170  // Read the number of points:
171  uint32_t n;
172  in >> n;
173 
174  this->resize(n);
175 
176  if (n > 0)
177  {
178  in.ReadBufferFixEndianness(&m_x[0], n);
179  in.ReadBufferFixEndianness(&m_y[0], n);
180  in.ReadBufferFixEndianness(&m_z[0], n);
181 
182  // Version 1: weights are also stored:
183  // Version 4: Type becomes long int -> uint32_t for
184  // portability!!
185  if (version >= 1)
186  {
187  if (version >= 4)
188  {
189  if (version >= 7)
190  {
191  // Weights were removed from this class in v7 (MRPT
192  // 0.9.5),
193  // so nothing else to do.
194  }
195  else
196  {
197  // Go on with old serialization format, but discard
198  // weights:
199  std::vector<uint32_t> dummy_pointWeight(n);
201  &dummy_pointWeight[0], n);
202  }
203  }
204  else
205  {
206  std::vector<uint32_t> dummy_pointWeight(n);
207  in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
208  }
209  }
210  }
211 
212  if (version >= 2)
213  {
214  // version 2: options saved too
215  in >> insertionOptions.minDistBetweenLaserPoints >>
216  insertionOptions.addToExistingPointsMap >>
217  insertionOptions.also_interpolate >>
218  insertionOptions.disableDeletion >>
219  insertionOptions.fuseWithExisting >>
220  insertionOptions.isPlanarMap;
221 
222  if (version < 6)
223  {
224  bool old_matchStaticPointsOnly;
225  in >> old_matchStaticPointsOnly;
226  }
227 
228  in >> insertionOptions.maxDistForInterpolatePoints;
229 
230  {
231  bool disableSaveAs3DObject;
232  in >> disableSaveAs3DObject;
233  genericMapParams.enableSaveAs3DObject =
234  !disableSaveAs3DObject;
235  }
236  }
237 
238  if (version >= 3)
239  {
240  in >> insertionOptions.horizontalTolerance;
241  }
242 
243  if (version >= 5) // version 5: added likelihoodOptions
244  likelihoodOptions.readFromStream(in);
245 
246  if (version >= 8) // version 8: added insertInvalidPoints
247  in >> insertionOptions.insertInvalidPoints;
248  }
249  break;
250  default:
252  };
253 }
254 
255 /*---------------------------------------------------------------
256  Clear
257  ---------------------------------------------------------------*/
258 void CSimplePointsMap::internal_clear()
259 {
260  // This swap() thing is the only way to really deallocate the memory.
261  vector_strong_clear(m_x);
262  vector_strong_clear(m_y);
263  vector_strong_clear(m_z);
264 
265  mark_as_modified();
266 }
267 
268 void CSimplePointsMap::insertPointFast(float x, float y, float z)
269 {
270  m_x.push_back(x);
271  m_y.push_back(y);
272  m_z.push_back(z);
273 }
274 
275 namespace mrpt::maps::detail
276 {
278 
279 template <>
281 {
282  /** Helper method fot the generic implementation of
283  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
284  * points - this is the place to reserve memory in lric for extra working
285  * variables. */
287  [[maybe_unused]] CSimplePointsMap& me,
289  lric)
290  {
291  }
292  /** Helper method fot the generic implementation of
293  * CPointsMap::loadFromRangeScan(), to be called once per range data */
295  [[maybe_unused]] CSimplePointsMap& me, [[maybe_unused]] const float gx,
296  [[maybe_unused]] const float gy, [[maybe_unused]] const float gz,
298  lric)
299  {
300  }
301  /** Helper method fot the generic implementation of
302  * CPointsMap::loadFromRangeScan(), to be called after each
303  * "{x,y,z}.push_back(...);" */
305  [[maybe_unused]] CSimplePointsMap& me,
307  lric)
308  {
309  }
310 
311  /** Helper method fot the generic implementation of
312  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
313  * points - this is the place to reserve memory in lric for extra working
314  * variables. */
316  [[maybe_unused]] CSimplePointsMap& me,
318  lric)
319  {
320  }
321  /** Helper method fot the generic implementation of
322  * CPointsMap::loadFromRangeScan(), to be called once per range data */
324  [[maybe_unused]] CSimplePointsMap& me, [[maybe_unused]] const float gx,
325  [[maybe_unused]] const float gy, [[maybe_unused]] const float gz,
327  lric)
328  {
329  }
330  /** Helper method fot the generic implementation of
331  * CPointsMap::loadFromRangeScan(), to be called after each
332  * "{x,y,z}.push_back(...);" */
334  [[maybe_unused]] CSimplePointsMap& me,
336  lric)
337  {
338  }
339  /** Helper method fot the generic implementation of
340  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
341  * end */
343  [[maybe_unused]] CSimplePointsMap& me,
345  lric)
346  {
347  }
348 };
349 } // namespace mrpt::maps::detail
350 /** See CPointsMap::loadFromRangeScan() */
351 void CSimplePointsMap::loadFromRangeScan(
352  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
353 {
355  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
356 }
357 
358 /** See CPointsMap::loadFromRangeScan() */
359 void CSimplePointsMap::loadFromRangeScan(
360  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
361 {
363  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
364 }
365 
366 // ================================ PLY files import & export virtual methods
367 // ================================
368 
369 /** In a base class, reserve memory to prepare subsequent calls to
370  * PLY_import_set_vertex */
371 void CSimplePointsMap::PLY_import_set_vertex_count(const size_t N)
372 {
373  this->setSize(N);
374 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
static void internal_loadFromRangeScan3D_prepareOneRange([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] const float gx, [[maybe_unused]] const float gy, [[maybe_unused]] const float gz, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
mrpt::maps::CPointsMap::TRenderOptions renderOpts
Rendering as 3D object options.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
static void internal_loadFromRangeScan2D_postPushBack([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
This namespace contains representation of robot actions and observations.
static void internal_loadFromRangeScan2D_prepareOneRange([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] const float gx, [[maybe_unused]] const float gy, [[maybe_unused]] const float gz, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
static void internal_loadFromRangeScan2D_init([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
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.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>&#39;s clear() method, but really forcing deallocating the memory...
Definition: bits_mem.h:18
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
static void internal_loadFromRangeScan3D_postPushBack([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
static void internal_loadFromRangeScan3D_init([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:95
static void internal_loadFromRangeScan3D_postOneRange([[maybe_unused]] CSimplePointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
images resize(NUM_IMGS)
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:77



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020