MRPT  1.9.9
CWeightedPointsMap.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::CWeightedPointsMap,weightedPointsMap",
29 
30 CWeightedPointsMap::TMapDefinition::TMapDefinition() = default;
31 void CWeightedPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
33  const std::string& sectionNamePrefix)
34 {
35  insertionOpts.loadFromConfigFile(
36  source, sectionNamePrefix + string("_insertOpts"));
37  likelihoodOpts.loadFromConfigFile(
38  source, sectionNamePrefix + string("_likelihoodOpts"));
39 }
40 
41 void CWeightedPointsMap::TMapDefinition::dumpToTextStream_map_specific(
42  std::ostream& out) const
43 {
44  this->insertionOpts.dumpToTextStream(out);
45  this->likelihoodOpts.dumpToTextStream(out);
46 }
47 
48 mrpt::maps::CMetricMap* CWeightedPointsMap::internal_CreateFromMapDefinition(
50 {
52  *dynamic_cast<const CWeightedPointsMap::TMapDefinition*>(&_def);
53  auto* obj = new CWeightedPointsMap();
54  obj->insertionOptions = def.insertionOpts;
55  obj->likelihoodOptions = def.likelihoodOpts;
56  return obj;
57 }
58 // =========== End of Map definition Block =========
59 
61 
62 void CWeightedPointsMap::reserve(size_t newLength)
63 {
64  m_x.reserve(newLength);
65  m_y.reserve(newLength);
66  m_z.reserve(newLength);
67  pointWeight.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 CWeightedPointsMap::resize(size_t newLength)
74 {
75  m_x.resize(newLength, 0);
76  m_y.resize(newLength, 0);
77  m_z.resize(newLength, 0);
78  pointWeight.resize(newLength, 1);
79 }
80 
81 // Resizes all point buffers so they can hold the given number of points,
82 // *erasing* all previous contents
83 // and leaving all points to default values.
84 void CWeightedPointsMap::setSize(size_t newLength)
85 {
86  m_x.assign(newLength, 0);
87  m_y.assign(newLength, 0);
88  m_z.assign(newLength, 0);
89  pointWeight.assign(newLength, 1);
90 }
91 
92 void CWeightedPointsMap::insertPointFast(float x, float y, float z)
93 {
94  m_x.push_back(x);
95  m_y.push_back(y);
96  m_z.push_back(z);
97  this->pointWeight.push_back(1);
98  // mark_as_modified(); -> Fast
99 }
100 
101 void CWeightedPointsMap::impl_copyFrom(const CPointsMap& obj)
102 {
103  // This also does a ::resize(N) of all data fields.
104  CPointsMap::base_copyFrom(obj);
105 
106  const auto* pW = dynamic_cast<const CWeightedPointsMap*>(&obj);
107  if (pW)
108  {
109  pointWeight = pW->pointWeight;
110  }
111 }
112 
113 /*---------------------------------------------------------------
114  addFrom_classSpecific
115  ---------------------------------------------------------------*/
116 void CWeightedPointsMap::addFrom_classSpecific(
117  const CPointsMap& anotherMap, const size_t nPreviousPoints)
118 {
119  const size_t nOther = anotherMap.size();
120 
121  // Specific data for this class:
122  const auto* anotheMap_w =
123  dynamic_cast<const CWeightedPointsMap*>(&anotherMap);
124 
125  if (anotheMap_w)
126  {
127  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
128  pointWeight[j] = anotheMap_w->pointWeight[i];
129  }
130 }
131 
132 uint8_t CWeightedPointsMap::serializeGetVersion() const { return 2; }
133 void CWeightedPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
134 {
135  uint32_t n = m_x.size();
136 
137  // First, write the number of points:
138  out << n;
139 
140  if (n > 0)
141  {
142  out.WriteBufferFixEndianness(&m_x[0], n);
143  out.WriteBufferFixEndianness(&m_y[0], n);
144  out.WriteBufferFixEndianness(&m_z[0], n);
145  out.WriteBufferFixEndianness(&pointWeight[0], n);
146  }
147 
148  out << genericMapParams; // v2
149  insertionOptions.writeToStream(
150  out); // version 9: insert options are saved with its own method
151  likelihoodOptions.writeToStream(out); // Added in version 5
152 }
153 
154 void CWeightedPointsMap::serializeFrom(
155  mrpt::serialization::CArchive& in, uint8_t version)
156 {
157  switch (version)
158  {
159  case 0:
160  case 1:
161  case 2:
162  {
163  mark_as_modified();
164 
165  // Read the number of points:
166  uint32_t n;
167  in >> n;
168 
169  this->resize(n);
170 
171  if (n > 0)
172  {
173  in.ReadBufferFixEndianness(&m_x[0], n);
174  in.ReadBufferFixEndianness(&m_y[0], n);
175  in.ReadBufferFixEndianness(&m_z[0], n);
176  in.ReadBufferFixEndianness(&pointWeight[0], n);
177  }
178 
179  if (version >= 1)
180  {
181  if (version >= 2)
182  in >> genericMapParams;
183  else
184  {
185  bool disableSaveAs3DObject;
186  in >> disableSaveAs3DObject;
187  genericMapParams.enableSaveAs3DObject =
188  !disableSaveAs3DObject;
189  }
190 
191  insertionOptions.readFromStream(in); // version 9: insert
192  // options are saved with
193  // its own method
194  }
195  else
196  {
197  insertionOptions = TInsertionOptions();
198  in >> insertionOptions.minDistBetweenLaserPoints >>
199  insertionOptions.addToExistingPointsMap >>
200  insertionOptions.also_interpolate >>
201  insertionOptions.disableDeletion >>
202  insertionOptions.fuseWithExisting >>
203  insertionOptions.isPlanarMap >>
204  insertionOptions.maxDistForInterpolatePoints;
205  {
206  bool disableSaveAs3DObject;
207  in >> disableSaveAs3DObject;
208  genericMapParams.enableSaveAs3DObject =
209  !disableSaveAs3DObject;
210  }
211  in >> insertionOptions.horizontalTolerance;
212  }
213 
214  likelihoodOptions.readFromStream(in); // Added in version 5
215  }
216  break;
217  default:
219  };
220 }
221 
222 /*---------------------------------------------------------------
223  Clear
224  ---------------------------------------------------------------*/
225 void CWeightedPointsMap::internal_clear()
226 {
227  // This swap() thing is the only way to really deallocate the memory.
228  vector_strong_clear(m_x);
229  vector_strong_clear(m_y);
230  vector_strong_clear(m_z);
231  vector_strong_clear(pointWeight);
232 
233  mark_as_modified();
234 }
235 
236 namespace mrpt::maps::detail
237 {
239 
240 template <>
242 {
243  /** Helper method fot the generic implementation of
244  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
245  * points - this is the place to reserve memory in lric for extra working
246  * variables. */
248  CWeightedPointsMap& me,
250  {
251  MRPT_UNUSED_PARAM(me);
252  MRPT_UNUSED_PARAM(lric);
253  }
254  /** Helper method fot the generic implementation of
255  * CPointsMap::loadFromRangeScan(), to be called once per range data */
257  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
259  {
260  MRPT_UNUSED_PARAM(me);
261  MRPT_UNUSED_PARAM(gx);
262  MRPT_UNUSED_PARAM(gy);
263  MRPT_UNUSED_PARAM(gz);
264  MRPT_UNUSED_PARAM(lric);
265  }
266  /** Helper method fot the generic implementation of
267  * CPointsMap::loadFromRangeScan(), to be called after each
268  * "{x,y,z}.push_back(...);" */
270  CWeightedPointsMap& me,
272  {
273  MRPT_UNUSED_PARAM(lric);
274  me.pointWeight.push_back(1);
275  }
276 
277  /** Helper method fot the generic implementation of
278  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
279  * points - this is the place to reserve memory in lric for extra working
280  * variables. */
282  CWeightedPointsMap& me,
284  {
285  MRPT_UNUSED_PARAM(me);
286  MRPT_UNUSED_PARAM(lric);
287  }
288  /** Helper method fot the generic implementation of
289  * CPointsMap::loadFromRangeScan(), to be called once per range data */
291  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
293  {
294  MRPT_UNUSED_PARAM(me);
295  MRPT_UNUSED_PARAM(gx);
296  MRPT_UNUSED_PARAM(gy);
297  MRPT_UNUSED_PARAM(gz);
298  MRPT_UNUSED_PARAM(lric);
299  }
300  /** Helper method fot the generic implementation of
301  * CPointsMap::loadFromRangeScan(), to be called after each
302  * "{x,y,z}.push_back(...);" */
304  CWeightedPointsMap& me,
306  {
307  MRPT_UNUSED_PARAM(lric);
308  me.pointWeight.push_back(1);
309  }
310  /** Helper method fot the generic implementation of
311  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
312  * end */
314  CWeightedPointsMap& me,
316  {
317  MRPT_UNUSED_PARAM(me);
318  MRPT_UNUSED_PARAM(lric);
319  }
320 };
321 } // namespace mrpt::maps::detail
322 /** See CPointsMap::loadFromRangeScan() */
323 void CWeightedPointsMap::loadFromRangeScan(
324  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
325 {
327  templ_loadFromRangeScan(*this, rangeScan, robotPose);
328 }
329 
330 /** See CPointsMap::loadFromRangeScan() */
331 void CWeightedPointsMap::loadFromRangeScan(
332  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
333 {
335  templ_loadFromRangeScan(*this, rangeScan, robotPose);
336 }
337 
338 // ================================ PLY files import & export virtual methods
339 // ================================
340 
341 /** In a base class, reserve memory to prepare subsequent calls to
342  * PLY_import_set_vertex */
343 void CWeightedPointsMap::PLY_import_set_vertex_count(const size_t N)
344 {
345  this->setSize(N);
346 }
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
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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.
GLenum GLsizei n
Definition: glext.h:5136
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:4085
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:221
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.
This namespace contains representation of robot actions and observations.
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
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_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: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
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:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
mrpt::aligned_std_vector< uint32_t > pointWeight
The points weights.
GLuint in
Definition: glext.h:7391
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
images resize(NUM_IMGS)
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:77
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: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019