MRPT  1.9.9
CWeightedPointsMap.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  "CWeightedPointsMap,weightedPointsMap", mrpt::maps::CWeightedPointsMap)
28 
30 void CWeightedPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
32  const std::string& sectionNamePrefix)
33 {
34  insertionOpts.loadFromConfigFile(
35  source, sectionNamePrefix + string("_insertOpts"));
36  likelihoodOpts.loadFromConfigFile(
37  source, sectionNamePrefix + string("_likelihoodOpts"));
38 }
39 
40 void CWeightedPointsMap::TMapDefinition::dumpToTextStream_map_specific(
41  std::ostream& out) const
42 {
43  this->insertionOpts.dumpToTextStream(out);
44  this->likelihoodOpts.dumpToTextStream(out);
45 }
46 
47 mrpt::maps::CMetricMap* CWeightedPointsMap::internal_CreateFromMapDefinition(
49 {
51  *dynamic_cast<const CWeightedPointsMap::TMapDefinition*>(&_def);
53  obj->insertionOptions = def.insertionOpts;
54  obj->likelihoodOptions = def.likelihoodOpts;
55  return obj;
56 }
57 // =========== End of Map definition Block =========
58 
60 
61 /*---------------------------------------------------------------
62  Constructor
63  ---------------------------------------------------------------*/
65 /*---------------------------------------------------------------
66  Destructor
67  ---------------------------------------------------------------*/
68 CWeightedPointsMap::~CWeightedPointsMap() {}
69 /*---------------------------------------------------------------
70  reserve & resize methods
71  ---------------------------------------------------------------*/
72 void CWeightedPointsMap::reserve(size_t newLength)
73 {
74  m_x.reserve(newLength);
75  m_y.reserve(newLength);
76  m_z.reserve(newLength);
77  pointWeight.reserve(newLength);
78 }
79 
80 // Resizes all point buffers so they can hold the given number of points: newly
81 // created points are set to default values,
82 // and old contents are not changed.
83 void CWeightedPointsMap::resize(size_t newLength)
84 {
85  m_x.resize(newLength, 0);
86  m_y.resize(newLength, 0);
87  m_z.resize(newLength, 0);
88  pointWeight.resize(newLength, 1);
89 }
90 
91 // Resizes all point buffers so they can hold the given number of points,
92 // *erasing* all previous contents
93 // and leaving all points to default values.
94 void CWeightedPointsMap::setSize(size_t newLength)
95 {
96  m_x.assign(newLength, 0);
97  m_y.assign(newLength, 0);
98  m_z.assign(newLength, 0);
99  pointWeight.assign(newLength, 1);
100 }
101 
102 void CWeightedPointsMap::setPointFast(size_t index, float x, float y, float z)
103 {
104  m_x[index] = x;
105  m_y[index] = y;
106  m_z[index] = z;
107  // this->pointWeight: Unmodified
108  // mark_as_modified(); -> Fast
109 }
110 
111 void CWeightedPointsMap::insertPointFast(float x, float y, float z)
112 {
113  m_x.push_back(x);
114  m_y.push_back(y);
115  m_z.push_back(z);
116  this->pointWeight.push_back(1);
117  // mark_as_modified(); -> Fast
118 }
119 
120 /*---------------------------------------------------------------
121  Copy constructor
122  ---------------------------------------------------------------*/
123 void CWeightedPointsMap::copyFrom(const CPointsMap& obj)
124 {
125  CPointsMap::base_copyFrom(
126  obj); // This also does a ::resize(N) of all data fields.
127 
128  const CWeightedPointsMap* pW =
129  dynamic_cast<const CWeightedPointsMap*>(&obj);
130  if (pW)
131  {
132  pointWeight = pW->pointWeight;
133  }
134 }
135 
136 /*---------------------------------------------------------------
137  addFrom_classSpecific
138  ---------------------------------------------------------------*/
139 void CWeightedPointsMap::addFrom_classSpecific(
140  const CPointsMap& anotherMap, const size_t nPreviousPoints)
141 {
142  const size_t nOther = anotherMap.size();
143 
144  // Specific data for this class:
145  const CWeightedPointsMap* anotheMap_w =
146  dynamic_cast<const CWeightedPointsMap*>(&anotherMap);
147 
148  if (anotheMap_w)
149  {
150  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
151  pointWeight[j] = anotheMap_w->pointWeight[i];
152  }
153 }
154 
155 uint8_t CWeightedPointsMap::serializeGetVersion() const { return 2; }
156 void CWeightedPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
157 {
158  uint32_t n = m_x.size();
159 
160  // First, write the number of points:
161  out << n;
162 
163  if (n > 0)
164  {
165  out.WriteBufferFixEndianness(&m_x[0], n);
166  out.WriteBufferFixEndianness(&m_y[0], n);
167  out.WriteBufferFixEndianness(&m_z[0], n);
168  out.WriteBufferFixEndianness(&pointWeight[0], n);
169  }
170 
171  out << genericMapParams; // v2
172  insertionOptions.writeToStream(
173  out); // version 9: insert options are saved with its own method
174  likelihoodOptions.writeToStream(out); // Added in version 5
175 }
176 
177 void CWeightedPointsMap::serializeFrom(
179 {
180  switch (version)
181  {
182  case 0:
183  case 1:
184  case 2:
185  {
186  mark_as_modified();
187 
188  // Read the number of points:
189  uint32_t n;
190  in >> n;
191 
192  this->resize(n);
193 
194  if (n > 0)
195  {
196  in.ReadBufferFixEndianness(&m_x[0], n);
197  in.ReadBufferFixEndianness(&m_y[0], n);
198  in.ReadBufferFixEndianness(&m_z[0], n);
199  in.ReadBufferFixEndianness(&pointWeight[0], n);
200  }
201 
202  if (version >= 1)
203  {
204  if (version >= 2)
205  in >> genericMapParams;
206  else
207  {
208  bool disableSaveAs3DObject;
209  in >> disableSaveAs3DObject;
210  genericMapParams.enableSaveAs3DObject =
211  !disableSaveAs3DObject;
212  }
213 
214  insertionOptions.readFromStream(in); // version 9: insert
215  // options are saved with
216  // its own method
217  }
218  else
219  {
220  insertionOptions = TInsertionOptions();
221  in >> insertionOptions.minDistBetweenLaserPoints >>
222  insertionOptions.addToExistingPointsMap >>
223  insertionOptions.also_interpolate >>
224  insertionOptions.disableDeletion >>
225  insertionOptions.fuseWithExisting >>
226  insertionOptions.isPlanarMap >>
227  insertionOptions.maxDistForInterpolatePoints;
228  {
229  bool disableSaveAs3DObject;
230  in >> disableSaveAs3DObject;
231  genericMapParams.enableSaveAs3DObject =
232  !disableSaveAs3DObject;
233  }
234  in >> insertionOptions.horizontalTolerance;
235  }
236 
237  likelihoodOptions.readFromStream(in); // Added in version 5
238  }
239  break;
240  default:
242  };
243 }
244 
245 /*---------------------------------------------------------------
246  Clear
247  ---------------------------------------------------------------*/
248 void CWeightedPointsMap::internal_clear()
249 {
250  // This swap() thing is the only way to really deallocate the memory.
251  vector_strong_clear(m_x);
252  vector_strong_clear(m_y);
253  vector_strong_clear(m_z);
254  vector_strong_clear(pointWeight);
255 
256  mark_as_modified();
257 }
258 
259 namespace mrpt::maps::detail
260 {
262 
263 template <>
265 {
266  /** Helper method fot the generic implementation of
267  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
268  * points - this is the place to reserve memory in lric for extra working
269  * variables. */
271  CWeightedPointsMap& me,
273  {
274  MRPT_UNUSED_PARAM(me);
275  MRPT_UNUSED_PARAM(lric);
276  }
277  /** Helper method fot the generic implementation of
278  * CPointsMap::loadFromRangeScan(), to be called once per range data */
280  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
282  {
283  MRPT_UNUSED_PARAM(me);
284  MRPT_UNUSED_PARAM(gx);
285  MRPT_UNUSED_PARAM(gy);
286  MRPT_UNUSED_PARAM(gz);
287  MRPT_UNUSED_PARAM(lric);
288  }
289  /** Helper method fot the generic implementation of
290  * CPointsMap::loadFromRangeScan(), to be called after each
291  * "{x,y,z}.push_back(...);" */
293  CWeightedPointsMap& me,
295  {
296  MRPT_UNUSED_PARAM(lric);
297  me.pointWeight.push_back(1);
298  }
299 
300  /** Helper method fot the generic implementation of
301  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
302  * points - this is the place to reserve memory in lric for extra working
303  * variables. */
305  CWeightedPointsMap& me,
307  {
308  MRPT_UNUSED_PARAM(me);
309  MRPT_UNUSED_PARAM(lric);
310  }
311  /** Helper method fot the generic implementation of
312  * CPointsMap::loadFromRangeScan(), to be called once per range data */
314  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
316  {
317  MRPT_UNUSED_PARAM(me);
318  MRPT_UNUSED_PARAM(gx);
319  MRPT_UNUSED_PARAM(gy);
320  MRPT_UNUSED_PARAM(gz);
321  MRPT_UNUSED_PARAM(lric);
322  }
323  /** Helper method fot the generic implementation of
324  * CPointsMap::loadFromRangeScan(), to be called after each
325  * "{x,y,z}.push_back(...);" */
327  CWeightedPointsMap& me,
329  {
330  MRPT_UNUSED_PARAM(lric);
331  me.pointWeight.push_back(1);
332  }
333  /** Helper method fot the generic implementation of
334  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
335  * end */
337  CWeightedPointsMap& me,
339  {
340  MRPT_UNUSED_PARAM(me);
341  MRPT_UNUSED_PARAM(lric);
342  }
343 };
344 }
345 /** See CPointsMap::loadFromRangeScan() */
346 void CWeightedPointsMap::loadFromRangeScan(
347  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
348 {
350  templ_loadFromRangeScan(*this, rangeScan, robotPose);
351 }
352 
353 /** See CPointsMap::loadFromRangeScan() */
354 void CWeightedPointsMap::loadFromRangeScan(
355  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
356 {
358  templ_loadFromRangeScan(*this, rangeScan, robotPose);
359 }
360 
361 // ================================ PLY files import & export virtual methods
362 // ================================
363 
364 /** In a base class, reserve memory to prepare subsequent calls to
365  * PLY_import_set_vertex */
366 void CWeightedPointsMap::PLY_import_set_vertex_count(const size_t N)
367 {
368  this->setSize(N);
369 }
370 
371 
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
#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:127
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.
static void internal_loadFromRangeScan3D_init(CWeightedPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
STL namespace.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
static void internal_loadFromRangeScan2D_prepareOneRange(CWeightedPointsMap &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 char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:205
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
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...
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
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_loadFromRangeScan2D_postPushBack(CWeightedPointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
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
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
static void internal_loadFromRangeScan3D_prepareOneRange(CWeightedPointsMap &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...
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
mrpt::aligned_std_vector< uint32_t > pointWeight
The points weights.
GLuint in
Definition: glext.h:7274
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
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
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
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
static void internal_loadFromRangeScan3D_postOneRange(CWeightedPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
static void internal_loadFromRangeScan2D_init(CWeightedPointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:76
static void internal_loadFromRangeScan3D_postPushBack(CWeightedPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020