Main MRPT website > C++ reference for MRPT 1.9.9
CSimplePointsMap.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 "maps-precomp.h" // Precomp header
11 
14 #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  "CSimplePointsMap,pointsMap", mrpt::maps::CSimplePointsMap)
28 
30 void CSimplePointsMap::TMapDefinition::loadFromConfigFile_map_specific(
32  const std::string& s)
33 {
34  insertionOpts.loadFromConfigFile(c, s + string("_insertOpts"));
35  likelihoodOpts.loadFromConfigFile(c, s + string("_likelihoodOpts"));
36  renderOpts.loadFromConfigFile(c, s + string("_renderOpts"));
37 }
38 
39 void CSimplePointsMap::TMapDefinition::dumpToTextStream_map_specific(
40  std::ostream& out) const
41 {
42  this->insertionOpts.dumpToTextStream(out);
43  this->likelihoodOpts.dumpToTextStream(out);
44  this->renderOpts.dumpToTextStream(out);
45 }
46 
47 mrpt::maps::CMetricMap* CSimplePointsMap::internal_CreateFromMapDefinition(
49 {
51  *dynamic_cast<const CSimplePointsMap::TMapDefinition*>(&_def);
53  obj->insertionOptions = def.insertionOpts;
54  obj->likelihoodOptions = def.likelihoodOpts;
55  obj->renderOptions = def.renderOpts;
56  return obj;
57 }
58 // =========== End of Map definition Block =========
59 
61 
63 void CSimplePointsMap::reserve(size_t newLength)
64 {
65  m_x.reserve(newLength);
66  m_y.reserve(newLength);
67  m_z.reserve(newLength);
68 }
69 
70 // Resizes all point buffers so they can hold the given number of points: newly
71 // created points are set to default values,
72 // and old contents are not changed.
73 void CSimplePointsMap::resize(size_t newLength)
74 {
75  this->reserve(newLength); // to ensure 4N capacity
76  m_x.resize(newLength, 0);
77  m_y.resize(newLength, 0);
78  m_z.resize(newLength, 0);
79  mark_as_modified();
80 }
81 
82 // Resizes all point buffers so they can hold the given number of points,
83 // *erasing* all previous contents
84 // and leaving all points to default values.
85 void CSimplePointsMap::setSize(size_t newLength)
86 {
87  this->reserve(newLength); // to ensure 4N capacity
88  m_x.assign(newLength, 0);
89  m_y.assign(newLength, 0);
90  m_z.assign(newLength, 0);
91  mark_as_modified();
92 }
93 
94 void CSimplePointsMap::copyFrom(const CPointsMap& obj)
95 {
96  CPointsMap::base_copyFrom(
97  obj); // This also does a ::resize(N) of all data fields.
98 }
99 
100 uint8_t CSimplePointsMap::serializeGetVersion() const { return 10; }
101 void CSimplePointsMap::serializeTo(mrpt::serialization::CArchive& out) const
102 {
103  uint32_t n = m_x.size();
104 
105  // First, write the number of points:
106  out << n;
107 
108  if (n > 0)
109  {
110  out.WriteBufferFixEndianness(&m_x[0], n);
111  out.WriteBufferFixEndianness(&m_y[0], n);
112  out.WriteBufferFixEndianness(&m_z[0], n);
113  }
114  out << genericMapParams; // v9
115  insertionOptions.writeToStream(out); // v9
116  likelihoodOptions.writeToStream(out); // v5
117  renderOptions.writeToStream(out); // v10
118 }
119 
120 /*---------------------------------------------------------------
121  readFromStream
122  Implements the reading from a CStream capability of
123  CSerializable objects
124  ---------------------------------------------------------------*/
125 void CSimplePointsMap::serializeFrom(
127 {
128  switch (version)
129  {
130  case 8:
131  case 9:
132  case 10:
133  {
134  mark_as_modified();
135 
136  // Read the number of points:
137  uint32_t n;
138  in >> n;
139 
140  this->resize(n);
141 
142  if (n > 0)
143  {
144  in.ReadBufferFixEndianness(&m_x[0], n);
145  in.ReadBufferFixEndianness(&m_y[0], n);
146  in.ReadBufferFixEndianness(&m_z[0], n);
147  }
148  if (version >= 9)
149  in >> genericMapParams;
150  else
151  {
152  bool disableSaveAs3DObject;
153  in >> disableSaveAs3DObject;
154  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
155  }
156  insertionOptions.readFromStream(in);
157  likelihoodOptions.readFromStream(in);
158  if (version>=10) renderOptions.readFromStream(in);
159  }
160  break;
161 
162  case 0:
163  case 1:
164  case 2:
165  case 3:
166  case 4:
167  case 5:
168  case 6:
169  case 7:
170  {
171  mark_as_modified();
172 
173  // Read the number of points:
174  uint32_t n;
175  in >> n;
176 
177  this->resize(n);
178 
179  if (n > 0)
180  {
181  in.ReadBufferFixEndianness(&m_x[0], n);
182  in.ReadBufferFixEndianness(&m_y[0], n);
183  in.ReadBufferFixEndianness(&m_z[0], n);
184 
185  // Version 1: weights are also stored:
186  // Version 4: Type becomes long int -> uint32_t for
187  // portability!!
188  if (version >= 1)
189  {
190  if (version >= 4)
191  {
192  if (version >= 7)
193  {
194  // Weights were removed from this class in v7 (MRPT
195  // 0.9.5),
196  // so nothing else to do.
197  }
198  else
199  {
200  // Go on with old serialization format, but discard
201  // weights:
202  std::vector<uint32_t> dummy_pointWeight(n);
203  in.ReadBufferFixEndianness(
204  &dummy_pointWeight[0], n);
205  }
206  }
207  else
208  {
209  std::vector<uint32_t> dummy_pointWeight(n);
210  in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
211  }
212  }
213  }
214 
215  if (version >= 2)
216  {
217  // version 2: options saved too
218  in >> insertionOptions.minDistBetweenLaserPoints >>
219  insertionOptions.addToExistingPointsMap >>
220  insertionOptions.also_interpolate >>
221  insertionOptions.disableDeletion >>
222  insertionOptions.fuseWithExisting >>
223  insertionOptions.isPlanarMap;
224 
225  if (version < 6)
226  {
227  bool old_matchStaticPointsOnly;
228  in >> old_matchStaticPointsOnly;
229  }
230 
231  in >> insertionOptions.maxDistForInterpolatePoints;
232 
233  {
234  bool disableSaveAs3DObject;
235  in >> disableSaveAs3DObject;
236  genericMapParams.enableSaveAs3DObject =
237  !disableSaveAs3DObject;
238  }
239  }
240 
241  if (version >= 3)
242  {
243  in >> insertionOptions.horizontalTolerance;
244  }
245 
246  if (version >= 5) // version 5: added likelihoodOptions
247  likelihoodOptions.readFromStream(in);
248 
249  if (version >= 8) // version 8: added insertInvalidPoints
250  in >> insertionOptions.insertInvalidPoints;
251  }
252  break;
253  default:
255  };
256 }
257 
258 /*---------------------------------------------------------------
259  Clear
260  ---------------------------------------------------------------*/
261 void CSimplePointsMap::internal_clear()
262 {
263  // This swap() thing is the only way to really deallocate the memory.
264  vector_strong_clear(m_x);
265  vector_strong_clear(m_y);
266  vector_strong_clear(m_z);
267 
268  mark_as_modified();
269 }
270 
271 void CSimplePointsMap::setPointFast(size_t index, float x, float y, float z)
272 {
273  m_x[index] = x;
274  m_y[index] = y;
275  m_z[index] = z;
276 }
277 
278 void CSimplePointsMap::insertPointFast(float x, float y, float z)
279 {
280  m_x.push_back(x);
281  m_y.push_back(y);
282  m_z.push_back(z);
283 }
284 
285 namespace mrpt
286 {
287 namespace maps
288 {
289 namespace detail
290 {
292 
293 template <>
295 {
296  /** Helper method fot the generic implementation of
297  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
298  * points - this is the place to reserve memory in lric for extra working
299  * variables. */
301  CSimplePointsMap& me,
303  {
304  MRPT_UNUSED_PARAM(me);
305  MRPT_UNUSED_PARAM(lric);
306  }
307  /** Helper method fot the generic implementation of
308  * CPointsMap::loadFromRangeScan(), to be called once per range data */
310  CSimplePointsMap& me, const float gx, const float gy, const float gz,
312  {
313  MRPT_UNUSED_PARAM(me);
314  MRPT_UNUSED_PARAM(gx);
315  MRPT_UNUSED_PARAM(gy);
316  MRPT_UNUSED_PARAM(gz);
317  MRPT_UNUSED_PARAM(lric);
318  }
319  /** Helper method fot the generic implementation of
320  * CPointsMap::loadFromRangeScan(), to be called after each
321  * "{x,y,z}.push_back(...);" */
323  CSimplePointsMap& me,
325  {
326  MRPT_UNUSED_PARAM(me);
327  MRPT_UNUSED_PARAM(lric);
328  }
329 
330  /** Helper method fot the generic implementation of
331  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
332  * points - this is the place to reserve memory in lric for extra working
333  * variables. */
335  CSimplePointsMap& me,
337  {
338  MRPT_UNUSED_PARAM(me);
339  MRPT_UNUSED_PARAM(lric);
340  }
341  /** Helper method fot the generic implementation of
342  * CPointsMap::loadFromRangeScan(), to be called once per range data */
344  CSimplePointsMap& me, const float gx, const float gy, const float gz,
346  {
347  MRPT_UNUSED_PARAM(me);
348  MRPT_UNUSED_PARAM(gx);
349  MRPT_UNUSED_PARAM(gy);
350  MRPT_UNUSED_PARAM(gz);
351  MRPT_UNUSED_PARAM(lric);
352  }
353  /** Helper method fot the generic implementation of
354  * CPointsMap::loadFromRangeScan(), to be called after each
355  * "{x,y,z}.push_back(...);" */
357  CSimplePointsMap& me,
359  {
360  MRPT_UNUSED_PARAM(me);
361  MRPT_UNUSED_PARAM(lric);
362  }
363  /** Helper method fot the generic implementation of
364  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
365  * end */
367  CSimplePointsMap& me,
369  {
370  MRPT_UNUSED_PARAM(me);
371  MRPT_UNUSED_PARAM(lric);
372  }
373 };
374 }
375 }
376 }
377 
378 /** See CPointsMap::loadFromRangeScan() */
379 void CSimplePointsMap::loadFromRangeScan(
380  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
381 {
383  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
384 }
385 
386 /** See CPointsMap::loadFromRangeScan() */
387 void CSimplePointsMap::loadFromRangeScan(
388  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
389 {
391  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
392 }
393 
394 // ================================ PLY files import & export virtual methods
395 // ================================
396 
397 /** In a base class, reserve memory to prepare subsequent calls to
398  * PLY_import_set_vertex */
399 void CSimplePointsMap::PLY_import_set_vertex_count(const size_t N)
400 {
401  this->setSize(N);
402 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
GLdouble GLdouble z
Definition: glext.h:3872
static void internal_loadFromRangeScan3D_init(CSimplePointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
virtual 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.
static void internal_loadFromRangeScan2D_init(CSimplePointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:5074
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
Definition: CArchive.h:123
mrpt::maps::CPointsMap::TRenderOptions renderOpts
Rendering as 3D object options.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
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.
GLuint index
Definition: glext.h:4054
static void internal_loadFromRangeScan3D_postPushBack(CSimplePointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
const GLubyte * c
Definition: glext.h:6313
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.
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static void internal_loadFromRangeScan3D_postOneRange(CSimplePointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
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:48
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
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
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:94
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
static void internal_loadFromRangeScan2D_postPushBack(CSimplePointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
static void internal_loadFromRangeScan2D_prepareOneRange(CSimplePointsMap &me, const float gx, const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all params from a config file/source.
unsigned __int32 uint32_t
Definition: rptypes.h:47
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:16
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:76
static void internal_loadFromRangeScan3D_prepareOneRange(CSimplePointsMap &me, const float gx, const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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