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-2019, 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  "CSimplePointsMap,pointsMap", mrpt::maps::CSimplePointsMap)
28 
29 CSimplePointsMap::TMapDefinition::TMapDefinition() = default;
30 void CSimplePointsMap::TMapDefinition::loadFromConfigFile_map_specific(
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 CSimplePointsMap::CSimplePointsMap() = default;
61 void CSimplePointsMap::reserve(size_t newLength)
62 {
63  m_x.reserve(newLength);
64  m_y.reserve(newLength);
65  m_z.reserve(newLength);
66 }
67 
68 // Resizes all point buffers so they can hold the given number of points: newly
69 // created points are set to default values,
70 // and old contents are not changed.
71 void CSimplePointsMap::resize(size_t newLength)
72 {
73  this->reserve(newLength); // to ensure 4N capacity
74  m_x.resize(newLength, 0);
75  m_y.resize(newLength, 0);
76  m_z.resize(newLength, 0);
77  mark_as_modified();
78 }
79 
80 // Resizes all point buffers so they can hold the given number of points,
81 // *erasing* all previous contents
82 // and leaving all points to default values.
83 void CSimplePointsMap::setSize(size_t newLength)
84 {
85  this->reserve(newLength); // to ensure 4N capacity
86  m_x.assign(newLength, 0);
87  m_y.assign(newLength, 0);
88  m_z.assign(newLength, 0);
89  mark_as_modified();
90 }
91 
92 void CSimplePointsMap::impl_copyFrom(const CPointsMap& obj)
93 {
94  // This also does a ::resize(N) of all data fields.
95  CPointsMap::base_copyFrom(obj);
96 }
97 
98 uint8_t CSimplePointsMap::serializeGetVersion() const { return 10; }
99 void CSimplePointsMap::serializeTo(mrpt::serialization::CArchive& out) const
100 {
101  uint32_t n = m_x.size();
102 
103  // First, write the number of points:
104  out << n;
105 
106  if (n > 0)
107  {
108  out.WriteBufferFixEndianness(&m_x[0], n);
109  out.WriteBufferFixEndianness(&m_y[0], n);
110  out.WriteBufferFixEndianness(&m_z[0], n);
111  }
112  out << genericMapParams; // v9
113  insertionOptions.writeToStream(out); // v9
114  likelihoodOptions.writeToStream(out); // v5
115  renderOptions.writeToStream(out); // v10
116 }
117 
118 /*---------------------------------------------------------------
119  readFromStream
120  Implements the reading from a CStream capability of
121  CSerializable objects
122  ---------------------------------------------------------------*/
123 void CSimplePointsMap::serializeFrom(
125 {
126  switch (version)
127  {
128  case 8:
129  case 9:
130  case 10:
131  {
132  mark_as_modified();
133 
134  // Read the number of points:
135  uint32_t n;
136  in >> n;
137 
138  this->resize(n);
139 
140  if (n > 0)
141  {
142  in.ReadBufferFixEndianness(&m_x[0], n);
143  in.ReadBufferFixEndianness(&m_y[0], n);
144  in.ReadBufferFixEndianness(&m_z[0], n);
145  }
146  if (version >= 9)
147  in >> genericMapParams;
148  else
149  {
150  bool disableSaveAs3DObject;
151  in >> disableSaveAs3DObject;
152  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
153  }
154  insertionOptions.readFromStream(in);
155  likelihoodOptions.readFromStream(in);
156  if (version >= 10) renderOptions.readFromStream(in);
157  }
158  break;
159 
160  case 0:
161  case 1:
162  case 2:
163  case 3:
164  case 4:
165  case 5:
166  case 6:
167  case 7:
168  {
169  mark_as_modified();
170 
171  // Read the number of points:
172  uint32_t n;
173  in >> n;
174 
175  this->resize(n);
176 
177  if (n > 0)
178  {
179  in.ReadBufferFixEndianness(&m_x[0], n);
180  in.ReadBufferFixEndianness(&m_y[0], n);
181  in.ReadBufferFixEndianness(&m_z[0], n);
182 
183  // Version 1: weights are also stored:
184  // Version 4: Type becomes long int -> uint32_t for
185  // portability!!
186  if (version >= 1)
187  {
188  if (version >= 4)
189  {
190  if (version >= 7)
191  {
192  // Weights were removed from this class in v7 (MRPT
193  // 0.9.5),
194  // so nothing else to do.
195  }
196  else
197  {
198  // Go on with old serialization format, but discard
199  // weights:
200  std::vector<uint32_t> dummy_pointWeight(n);
201  in.ReadBufferFixEndianness(
202  &dummy_pointWeight[0], n);
203  }
204  }
205  else
206  {
207  std::vector<uint32_t> dummy_pointWeight(n);
208  in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
209  }
210  }
211  }
212 
213  if (version >= 2)
214  {
215  // version 2: options saved too
216  in >> insertionOptions.minDistBetweenLaserPoints >>
217  insertionOptions.addToExistingPointsMap >>
218  insertionOptions.also_interpolate >>
219  insertionOptions.disableDeletion >>
220  insertionOptions.fuseWithExisting >>
221  insertionOptions.isPlanarMap;
222 
223  if (version < 6)
224  {
225  bool old_matchStaticPointsOnly;
226  in >> old_matchStaticPointsOnly;
227  }
228 
229  in >> insertionOptions.maxDistForInterpolatePoints;
230 
231  {
232  bool disableSaveAs3DObject;
233  in >> disableSaveAs3DObject;
234  genericMapParams.enableSaveAs3DObject =
235  !disableSaveAs3DObject;
236  }
237  }
238 
239  if (version >= 3)
240  {
241  in >> insertionOptions.horizontalTolerance;
242  }
243 
244  if (version >= 5) // version 5: added likelihoodOptions
245  likelihoodOptions.readFromStream(in);
246 
247  if (version >= 8) // version 8: added insertInvalidPoints
248  in >> insertionOptions.insertInvalidPoints;
249  }
250  break;
251  default:
253  };
254 }
255 
256 /*---------------------------------------------------------------
257  Clear
258  ---------------------------------------------------------------*/
259 void CSimplePointsMap::internal_clear()
260 {
261  // This swap() thing is the only way to really deallocate the memory.
262  vector_strong_clear(m_x);
263  vector_strong_clear(m_y);
264  vector_strong_clear(m_z);
265 
266  mark_as_modified();
267 }
268 
269 void CSimplePointsMap::insertPointFast(float x, float y, float z)
270 {
271  m_x.push_back(x);
272  m_y.push_back(y);
273  m_z.push_back(z);
274 }
275 
276 namespace mrpt::maps::detail
277 {
279 
280 template <>
282 {
283  /** Helper method fot the generic implementation of
284  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
285  * points - this is the place to reserve memory in lric for extra working
286  * variables. */
288  CSimplePointsMap& me,
290  {
291  MRPT_UNUSED_PARAM(me);
292  MRPT_UNUSED_PARAM(lric);
293  }
294  /** Helper method fot the generic implementation of
295  * CPointsMap::loadFromRangeScan(), to be called once per range data */
297  CSimplePointsMap& me, const float gx, const float gy, const float gz,
299  {
300  MRPT_UNUSED_PARAM(me);
301  MRPT_UNUSED_PARAM(gx);
302  MRPT_UNUSED_PARAM(gy);
303  MRPT_UNUSED_PARAM(gz);
304  MRPT_UNUSED_PARAM(lric);
305  }
306  /** Helper method fot the generic implementation of
307  * CPointsMap::loadFromRangeScan(), to be called after each
308  * "{x,y,z}.push_back(...);" */
310  CSimplePointsMap& me,
312  {
313  MRPT_UNUSED_PARAM(me);
314  MRPT_UNUSED_PARAM(lric);
315  }
316 
317  /** Helper method fot the generic implementation of
318  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
319  * points - this is the place to reserve memory in lric for extra working
320  * variables. */
322  CSimplePointsMap& me,
324  {
325  MRPT_UNUSED_PARAM(me);
326  MRPT_UNUSED_PARAM(lric);
327  }
328  /** Helper method fot the generic implementation of
329  * CPointsMap::loadFromRangeScan(), to be called once per range data */
331  CSimplePointsMap& me, const float gx, const float gy, const float gz,
333  {
334  MRPT_UNUSED_PARAM(me);
335  MRPT_UNUSED_PARAM(gx);
336  MRPT_UNUSED_PARAM(gy);
337  MRPT_UNUSED_PARAM(gz);
338  MRPT_UNUSED_PARAM(lric);
339  }
340  /** Helper method fot the generic implementation of
341  * CPointsMap::loadFromRangeScan(), to be called after each
342  * "{x,y,z}.push_back(...);" */
344  CSimplePointsMap& me,
346  {
347  MRPT_UNUSED_PARAM(me);
348  MRPT_UNUSED_PARAM(lric);
349  }
350  /** Helper method fot the generic implementation of
351  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
352  * end */
354  CSimplePointsMap& me,
356  {
357  MRPT_UNUSED_PARAM(me);
358  MRPT_UNUSED_PARAM(lric);
359  }
360 };
361 } // namespace mrpt::maps::detail
362 /** See CPointsMap::loadFromRangeScan() */
363 void CSimplePointsMap::loadFromRangeScan(
364  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
365 {
367  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
368 }
369 
370 /** See CPointsMap::loadFromRangeScan() */
371 void CSimplePointsMap::loadFromRangeScan(
372  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
373 {
375  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
376 }
377 
378 // ================================ PLY files import & export virtual methods
379 // ================================
380 
381 /** In a base class, reserve memory to prepare subsequent calls to
382  * PLY_import_set_vertex */
383 void CSimplePointsMap::PLY_import_set_vertex_count(const size_t N)
384 {
385  this->setSize(N);
386 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
GLdouble GLdouble z
Definition: glext.h:3879
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...
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:5136
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:127
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:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:138
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.
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:6406
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:4116
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:52
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:53
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
GLuint in
Definition: glext.h:7391
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:95
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
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...
unsigned __int32 uint32_t
Definition: rptypes.h:50
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:77
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: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019