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  "mrpt::maps::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 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);
200  in.ReadBufferFixEndianness(
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  CSimplePointsMap& me,
289  {
290  MRPT_UNUSED_PARAM(me);
291  MRPT_UNUSED_PARAM(lric);
292  }
293  /** Helper method fot the generic implementation of
294  * CPointsMap::loadFromRangeScan(), to be called once per range data */
296  CSimplePointsMap& me, const float gx, const float gy, const float gz,
298  {
299  MRPT_UNUSED_PARAM(me);
300  MRPT_UNUSED_PARAM(gx);
301  MRPT_UNUSED_PARAM(gy);
302  MRPT_UNUSED_PARAM(gz);
303  MRPT_UNUSED_PARAM(lric);
304  }
305  /** Helper method fot the generic implementation of
306  * CPointsMap::loadFromRangeScan(), to be called after each
307  * "{x,y,z}.push_back(...);" */
309  CSimplePointsMap& me,
311  {
312  MRPT_UNUSED_PARAM(me);
313  MRPT_UNUSED_PARAM(lric);
314  }
315 
316  /** Helper method fot the generic implementation of
317  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
318  * points - this is the place to reserve memory in lric for extra working
319  * variables. */
321  CSimplePointsMap& me,
323  {
324  MRPT_UNUSED_PARAM(me);
325  MRPT_UNUSED_PARAM(lric);
326  }
327  /** Helper method fot the generic implementation of
328  * CPointsMap::loadFromRangeScan(), to be called once per range data */
330  CSimplePointsMap& me, const float gx, const float gy, const float gz,
332  {
333  MRPT_UNUSED_PARAM(me);
334  MRPT_UNUSED_PARAM(gx);
335  MRPT_UNUSED_PARAM(gy);
336  MRPT_UNUSED_PARAM(gz);
337  MRPT_UNUSED_PARAM(lric);
338  }
339  /** Helper method fot the generic implementation of
340  * CPointsMap::loadFromRangeScan(), to be called after each
341  * "{x,y,z}.push_back(...);" */
343  CSimplePointsMap& me,
345  {
346  MRPT_UNUSED_PARAM(me);
347  MRPT_UNUSED_PARAM(lric);
348  }
349  /** Helper method fot the generic implementation of
350  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
351  * end */
353  CSimplePointsMap& me,
355  {
356  MRPT_UNUSED_PARAM(me);
357  MRPT_UNUSED_PARAM(lric);
358  }
359 };
360 } // namespace mrpt::maps::detail
361 /** See CPointsMap::loadFromRangeScan() */
362 void CSimplePointsMap::loadFromRangeScan(
363  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
364 {
366  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
367 }
368 
369 /** See CPointsMap::loadFromRangeScan() */
370 void CSimplePointsMap::loadFromRangeScan(
371  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
372 {
374  CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
375 }
376 
377 // ================================ PLY files import & export virtual methods
378 // ================================
379 
380 /** In a base class, reserve memory to prepare subsequent calls to
381  * PLY_import_set_vertex */
382 void CSimplePointsMap::PLY_import_set_vertex_count(const size_t N)
383 {
384  this->setSize(N);
385 }
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...
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)
To be added to all CSerializable-classes implementation files.
GLenum GLsizei n
Definition: glext.h:5136
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
#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.
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.
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: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
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
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
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...
images resize(NUM_IMGS)
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: cfb2df9d3 Fri Nov 15 06:57:14 2019 +0100 at vie nov 15 07:00:11 CET 2019