Main MRPT website > C++ reference for MRPT 1.5.5
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-2017, 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 
13 #include <mrpt/utils/CStream.h>
14 
15 #include "CPointsMap_crtp_common.h"
16 
17 using namespace std;
18 using namespace mrpt;
19 using namespace mrpt::maps;
20 using namespace mrpt::obs;
21 using namespace mrpt::utils;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 
25 // =========== Begin of Map definition ============
27 
29 {
30 }
31 
32 void CSimplePointsMap::TMapDefinition::loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
33 {
34  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
35  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix+string("_likelihoodOpts") );
36 }
37 
38 void CSimplePointsMap::TMapDefinition::dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
39 {
40  this->insertionOpts.dumpToTextStream(out);
41  this->likelihoodOpts.dumpToTextStream(out);
42 }
43 
44 mrpt::maps::CMetricMap* CSimplePointsMap::internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &_def)
45 {
46  const CSimplePointsMap::TMapDefinition &def = *dynamic_cast<const CSimplePointsMap::TMapDefinition*>(&_def);
50  return obj;
51 }
52 // =========== End of Map definition Block =========
53 
54 
56 
57 /*---------------------------------------------------------------
58  Constructor
59  ---------------------------------------------------------------*/
61 {
62  reserve( 400 );
63 }
64 
65 /*---------------------------------------------------------------
66  Destructor
67  ---------------------------------------------------------------*/
68 CSimplePointsMap::~CSimplePointsMap()
69 {
70 }
71 
72 /*---------------------------------------------------------------
73  reserve & resize methods
74  ---------------------------------------------------------------*/
75 void CSimplePointsMap::reserve(size_t newLength)
76 {
77  newLength = mrpt::utils::length2length4N(newLength);
78 
79  x.reserve( newLength );
80  y.reserve( newLength );
81  z.reserve( newLength );
82 }
83 
84 // Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
85 // and old contents are not changed.
86 void CSimplePointsMap::resize(size_t newLength)
87 {
88  this->reserve(newLength); // to ensure 4N capacity
89  x.resize( newLength, 0 );
90  y.resize( newLength, 0 );
91  z.resize( newLength, 0 );
92  mark_as_modified();
93 }
94 
95 // Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
96 // and leaving all points to default values.
97 void CSimplePointsMap::setSize(size_t newLength)
98 {
99  this->reserve(newLength); // to ensure 4N capacity
100  x.assign( newLength, 0);
101  y.assign( newLength, 0);
102  z.assign( newLength, 0);
103  mark_as_modified();
104 }
105 
106 
107 /*---------------------------------------------------------------
108  Copy constructor
109  ---------------------------------------------------------------*/
110 void CSimplePointsMap::copyFrom(const CPointsMap &obj)
111 {
112  CPointsMap::base_copyFrom(obj); // This also does a ::resize(N) of all data fields.
113 }
114 
115 
116 /*---------------------------------------------------------------
117  writeToStream
118  Implements the writing to a CStream capability of
119  CSerializable objects
120  ---------------------------------------------------------------*/
121 void CSimplePointsMap::writeToStream(mrpt::utils::CStream &out, int *version) const
122 {
123  if (version)
124  *version = 9;
125  else
126  {
127  uint32_t n = x.size();
128 
129  // First, write the number of points:
130  out << n;
131 
132  if (n>0)
133  {
134  out.WriteBufferFixEndianness(&x[0],n);
135  out.WriteBufferFixEndianness(&y[0],n);
136  out.WriteBufferFixEndianness(&z[0],n);
137  }
138  out << genericMapParams; // v9
139 
140  insertionOptions.writeToStream(out); // version 9: insert options are saved with its own method:
141  likelihoodOptions.writeToStream(out); // Added in version 5:
142  }
143 }
144 
145 /*---------------------------------------------------------------
146  readFromStream
147  Implements the reading from a CStream capability of
148  CSerializable objects
149  ---------------------------------------------------------------*/
150 void CSimplePointsMap::readFromStream(mrpt::utils::CStream &in, int version)
151 {
152  switch(version)
153  {
154  case 8:
155  case 9:
156  {
157  mark_as_modified();
158 
159  // Read the number of points:
160  uint32_t n;
161  in >> n;
162 
163  this->resize(n);
164 
165  if (n>0)
166  {
167  in.ReadBufferFixEndianness(&x[0],n);
168  in.ReadBufferFixEndianness(&y[0],n);
169  in.ReadBufferFixEndianness(&z[0],n);
170  }
171  if (version>=9)
172  in >> genericMapParams;
173  else
174  {
175  bool disableSaveAs3DObject;
176  in >> disableSaveAs3DObject;
177  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
178  }
179 
180  insertionOptions.readFromStream(in);
181  likelihoodOptions.readFromStream(in);
182  } break;
183 
184  case 0:
185  case 1:
186  case 2:
187  case 3:
188  case 4:
189  case 5:
190  case 6:
191  case 7:
192  {
193  mark_as_modified();
194 
195  // Read the number of points:
196  uint32_t n;
197  in >> n;
198 
199  this->resize(n);
200 
201  if (n>0)
202  {
203  in.ReadBufferFixEndianness(&x[0],n);
204  in.ReadBufferFixEndianness(&y[0],n);
205  in.ReadBufferFixEndianness(&z[0],n);
206 
207  // Version 1: weights are also stored:
208  // Version 4: Type becomes long int -> uint32_t for portability!!
209  if (version>=1)
210  {
211  if (version>=4)
212  {
213  if (version>=7)
214  {
215  // Weights were removed from this class in v7 (MRPT 0.9.5),
216  // so nothing else to do.
217  }
218  else
219  {
220  // Go on with old serialization format, but discard weights:
221  std::vector<uint32_t> dummy_pointWeight(n);
222  in.ReadBufferFixEndianness(&dummy_pointWeight[0],n);
223  }
224  }
225  else
226  {
227  std::vector<uint32_t> dummy_pointWeight(n);
228  in.ReadBufferFixEndianness(&dummy_pointWeight[0],n);
229  }
230  }
231  }
232 
233  if (version>=2)
234  {
235  // version 2: options saved too
236  in >> insertionOptions.minDistBetweenLaserPoints
237  >> insertionOptions.addToExistingPointsMap
238  >> insertionOptions.also_interpolate
239  >> insertionOptions.disableDeletion
240  >> insertionOptions.fuseWithExisting
241  >> insertionOptions.isPlanarMap;
242 
243  if (version<6)
244  {
245  bool old_matchStaticPointsOnly;
246  in >> old_matchStaticPointsOnly;
247  }
248 
249  in >> insertionOptions.maxDistForInterpolatePoints;
250 
251  {
252  bool disableSaveAs3DObject;
253  in >> disableSaveAs3DObject;
254  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
255  }
256  }
257 
258  if (version>=3)
259  {
260  in >> insertionOptions.horizontalTolerance;
261  }
262 
263  if (version>=5) // version 5: added likelihoodOptions
264  likelihoodOptions.readFromStream(in);
265 
266  if (version>=8) // version 8: added insertInvalidPoints
267  in >> insertionOptions.insertInvalidPoints;
268 
269  } break;
270  default:
272 
273  };
274 
275 }
276 
277 /*---------------------------------------------------------------
278  Clear
279  ---------------------------------------------------------------*/
280 void CSimplePointsMap::internal_clear()
281 {
282  // This swap() thing is the only way to really deallocate the memory.
286 
287  mark_as_modified();
288 }
289 
290 void CSimplePointsMap::setPointFast(size_t index,float x,float y,float z)
291 {
292  this->x[index] = x;
293  this->y[index] = y;
294  this->z[index] = z;
295 }
296 
297 void CSimplePointsMap::insertPointFast( float x, float y, float z )
298 {
299  this->x.push_back(x);
300  this->y.push_back(y);
301  this->z.push_back(z);
302 }
303 
304 
305 namespace mrpt {
306  namespace maps {
307  namespace detail {
309 
310  template <> struct pointmap_traits<CSimplePointsMap>
311  {
312 
313  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only once before inserting points - this is the place to reserve memory in lric for extra working variables. */
315  MRPT_UNUSED_PARAM(me);
316  MRPT_UNUSED_PARAM(lric);
317  }
318  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once per range data */
319  inline static void internal_loadFromRangeScan2D_prepareOneRange(CSimplePointsMap &me, const float gx,const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange2DInsertContext & lric ) {
320  MRPT_UNUSED_PARAM(me);
321  MRPT_UNUSED_PARAM(gx);
322  MRPT_UNUSED_PARAM(gy);
323  MRPT_UNUSED_PARAM(gz);
324  MRPT_UNUSED_PARAM(lric);
325  }
326  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after each "{x,y,z}.push_back(...);" */
328  MRPT_UNUSED_PARAM(me);
329  MRPT_UNUSED_PARAM(lric);
330  }
331 
332  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only once before inserting points - this is the place to reserve memory in lric for extra working variables. */
334  MRPT_UNUSED_PARAM(me);
335  MRPT_UNUSED_PARAM(lric);
336  }
337  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once per range data */
338  inline static void internal_loadFromRangeScan3D_prepareOneRange(CSimplePointsMap &me, const float gx,const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange3DInsertContext & lric ) {
339  MRPT_UNUSED_PARAM(me);
340  MRPT_UNUSED_PARAM(gx);
341  MRPT_UNUSED_PARAM(gy);
342  MRPT_UNUSED_PARAM(gz);
343  MRPT_UNUSED_PARAM(lric);
344  }
345  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after each "{x,y,z}.push_back(...);" */
347  MRPT_UNUSED_PARAM(me);
348  MRPT_UNUSED_PARAM(lric);
349  }
350  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once per range data, at the end */
352  MRPT_UNUSED_PARAM(me);
353  MRPT_UNUSED_PARAM(lric);
354  }
355  };
356  }
357  }
358 }
359 
360 /** See CPointsMap::loadFromRangeScan() */
361 void CSimplePointsMap::loadFromRangeScan(
362  const CObservation2DRangeScan &rangeScan,
363  const CPose3D *robotPose)
364 {
366 }
367 
368 /** See CPointsMap::loadFromRangeScan() */
369 void CSimplePointsMap::loadFromRangeScan(
370  const CObservation3DRangeScan &rangeScan,
371  const CPose3D *robotPose)
372 {
374 }
375 
376 // ================================ PLY files import & export virtual methods ================================
377 
378 /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
379 void CSimplePointsMap::PLY_import_set_vertex_count(const size_t N)
380 {
381  this->setSize(N);
382 }
383 
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix) MRPT_OVERRIDE
Load all params from a config file/source.
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.
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. ...
GLuint in
Definition: glew.h:7146
This class allows loading and storing values and vectors of different types from a configuration text...
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: CStream.h:139
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
GLsizei n
Definition: glew.h:5051
T length2length4N(T len)
Returns the smaller number >=len such that it's a multiple of 4.
Definition: bits.h:209
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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...
GLhandleARB obj
Definition: glew.h:3276
int version
Definition: mrpt_jpeglib.h:898
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
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...
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory...
Definition: bits.h:205
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
GLdouble GLdouble z
Definition: glew.h:1464
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
GLuint index
Definition: glew.h:1721
TLikelihoodOptions likelihoodOptions
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: CStream.h:95
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
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:49
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
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...



Page generated by Doxygen 1.8.6 for MRPT 1.5.5 Git: e06b63d Fri Dec 1 14:41:11 2017 +0100 at sáb dic 2 13:00:21 CET 2017