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  "CWeightedPointsMap,weightedPointsMap", mrpt::maps::CWeightedPointsMap)
28 
29 CWeightedPointsMap::TMapDefinition::TMapDefinition() = default;
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);
52  auto* obj = new CWeightedPointsMap();
53  obj->insertionOptions = def.insertionOpts;
54  obj->likelihoodOptions = def.likelihoodOpts;
55  return obj;
56 }
57 // =========== End of Map definition Block =========
58 
60 
62 
63 void CWeightedPointsMap::reserve(size_t newLength)
64 {
65  m_x.reserve(newLength);
66  m_y.reserve(newLength);
67  m_z.reserve(newLength);
68  pointWeight.reserve(newLength);
69 }
70 
71 // Resizes all point buffers so they can hold the given number of points: newly
72 // created points are set to default values,
73 // and old contents are not changed.
74 void CWeightedPointsMap::resize(size_t newLength)
75 {
76  m_x.resize(newLength, 0);
77  m_y.resize(newLength, 0);
78  m_z.resize(newLength, 0);
79  pointWeight.resize(newLength, 1);
80 }
81 
82 // Resizes all point buffers so they can hold the given number of points,
83 // *erasing* all previous contents
84 // and leaving all points to default values.
85 void CWeightedPointsMap::setSize(size_t newLength)
86 {
87  m_x.assign(newLength, 0);
88  m_y.assign(newLength, 0);
89  m_z.assign(newLength, 0);
90  pointWeight.assign(newLength, 1);
91 }
92 
93 void CWeightedPointsMap::insertPointFast(float x, float y, float z)
94 {
95  m_x.push_back(x);
96  m_y.push_back(y);
97  m_z.push_back(z);
98  this->pointWeight.push_back(1);
99  // mark_as_modified(); -> Fast
100 }
101 
102 void CWeightedPointsMap::impl_copyFrom(const CPointsMap& obj)
103 {
104  // This also does a ::resize(N) of all data fields.
105  CPointsMap::base_copyFrom(obj);
106 
107  const auto* pW = dynamic_cast<const CWeightedPointsMap*>(&obj);
108  if (pW)
109  {
110  pointWeight = pW->pointWeight;
111  }
112 }
113 
114 /*---------------------------------------------------------------
115  addFrom_classSpecific
116  ---------------------------------------------------------------*/
117 void CWeightedPointsMap::addFrom_classSpecific(
118  const CPointsMap& anotherMap, const size_t nPreviousPoints)
119 {
120  const size_t nOther = anotherMap.size();
121 
122  // Specific data for this class:
123  const auto* anotheMap_w =
124  dynamic_cast<const CWeightedPointsMap*>(&anotherMap);
125 
126  if (anotheMap_w)
127  {
128  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
129  pointWeight[j] = anotheMap_w->pointWeight[i];
130  }
131 }
132 
133 uint8_t CWeightedPointsMap::serializeGetVersion() const { return 2; }
134 void CWeightedPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
135 {
136  uint32_t n = m_x.size();
137 
138  // First, write the number of points:
139  out << n;
140 
141  if (n > 0)
142  {
143  out.WriteBufferFixEndianness(&m_x[0], n);
144  out.WriteBufferFixEndianness(&m_y[0], n);
145  out.WriteBufferFixEndianness(&m_z[0], n);
146  out.WriteBufferFixEndianness(&pointWeight[0], n);
147  }
148 
149  out << genericMapParams; // v2
150  insertionOptions.writeToStream(
151  out); // version 9: insert options are saved with its own method
152  likelihoodOptions.writeToStream(out); // Added in version 5
153 }
154 
155 void CWeightedPointsMap::serializeFrom(
157 {
158  switch (version)
159  {
160  case 0:
161  case 1:
162  case 2:
163  {
164  mark_as_modified();
165 
166  // Read the number of points:
167  uint32_t n;
168  in >> n;
169 
170  this->resize(n);
171 
172  if (n > 0)
173  {
174  in.ReadBufferFixEndianness(&m_x[0], n);
175  in.ReadBufferFixEndianness(&m_y[0], n);
176  in.ReadBufferFixEndianness(&m_z[0], n);
177  in.ReadBufferFixEndianness(&pointWeight[0], n);
178  }
179 
180  if (version >= 1)
181  {
182  if (version >= 2)
183  in >> genericMapParams;
184  else
185  {
186  bool disableSaveAs3DObject;
187  in >> disableSaveAs3DObject;
188  genericMapParams.enableSaveAs3DObject =
189  !disableSaveAs3DObject;
190  }
191 
192  insertionOptions.readFromStream(in); // version 9: insert
193  // options are saved with
194  // its own method
195  }
196  else
197  {
198  insertionOptions = TInsertionOptions();
199  in >> insertionOptions.minDistBetweenLaserPoints >>
200  insertionOptions.addToExistingPointsMap >>
201  insertionOptions.also_interpolate >>
202  insertionOptions.disableDeletion >>
203  insertionOptions.fuseWithExisting >>
204  insertionOptions.isPlanarMap >>
205  insertionOptions.maxDistForInterpolatePoints;
206  {
207  bool disableSaveAs3DObject;
208  in >> disableSaveAs3DObject;
209  genericMapParams.enableSaveAs3DObject =
210  !disableSaveAs3DObject;
211  }
212  in >> insertionOptions.horizontalTolerance;
213  }
214 
215  likelihoodOptions.readFromStream(in); // Added in version 5
216  }
217  break;
218  default:
220  };
221 }
222 
223 /*---------------------------------------------------------------
224  Clear
225  ---------------------------------------------------------------*/
226 void CWeightedPointsMap::internal_clear()
227 {
228  // This swap() thing is the only way to really deallocate the memory.
229  vector_strong_clear(m_x);
230  vector_strong_clear(m_y);
231  vector_strong_clear(m_z);
232  vector_strong_clear(pointWeight);
233 
234  mark_as_modified();
235 }
236 
237 namespace mrpt::maps::detail
238 {
240 
241 template <>
243 {
244  /** Helper method fot the generic implementation of
245  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
246  * points - this is the place to reserve memory in lric for extra working
247  * variables. */
249  CWeightedPointsMap& me,
251  {
252  MRPT_UNUSED_PARAM(me);
253  MRPT_UNUSED_PARAM(lric);
254  }
255  /** Helper method fot the generic implementation of
256  * CPointsMap::loadFromRangeScan(), to be called once per range data */
258  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
260  {
261  MRPT_UNUSED_PARAM(me);
262  MRPT_UNUSED_PARAM(gx);
263  MRPT_UNUSED_PARAM(gy);
264  MRPT_UNUSED_PARAM(gz);
265  MRPT_UNUSED_PARAM(lric);
266  }
267  /** Helper method fot the generic implementation of
268  * CPointsMap::loadFromRangeScan(), to be called after each
269  * "{x,y,z}.push_back(...);" */
271  CWeightedPointsMap& me,
273  {
274  MRPT_UNUSED_PARAM(lric);
275  me.pointWeight.push_back(1);
276  }
277 
278  /** Helper method fot the generic implementation of
279  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
280  * points - this is the place to reserve memory in lric for extra working
281  * variables. */
283  CWeightedPointsMap& me,
285  {
286  MRPT_UNUSED_PARAM(me);
287  MRPT_UNUSED_PARAM(lric);
288  }
289  /** Helper method fot the generic implementation of
290  * CPointsMap::loadFromRangeScan(), to be called once per range data */
292  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
294  {
295  MRPT_UNUSED_PARAM(me);
296  MRPT_UNUSED_PARAM(gx);
297  MRPT_UNUSED_PARAM(gy);
298  MRPT_UNUSED_PARAM(gz);
299  MRPT_UNUSED_PARAM(lric);
300  }
301  /** Helper method fot the generic implementation of
302  * CPointsMap::loadFromRangeScan(), to be called after each
303  * "{x,y,z}.push_back(...);" */
305  CWeightedPointsMap& me,
307  {
308  MRPT_UNUSED_PARAM(lric);
309  me.pointWeight.push_back(1);
310  }
311  /** Helper method fot the generic implementation of
312  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
313  * end */
315  CWeightedPointsMap& me,
317  {
318  MRPT_UNUSED_PARAM(me);
319  MRPT_UNUSED_PARAM(lric);
320  }
321 };
322 } // namespace mrpt::maps::detail
323 /** See CPointsMap::loadFromRangeScan() */
324 void CWeightedPointsMap::loadFromRangeScan(
325  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
326 {
328  templ_loadFromRangeScan(*this, rangeScan, robotPose);
329 }
330 
331 /** See CPointsMap::loadFromRangeScan() */
332 void CWeightedPointsMap::loadFromRangeScan(
333  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
334 {
336  templ_loadFromRangeScan(*this, rangeScan, robotPose);
337 }
338 
339 // ================================ PLY files import & export virtual methods
340 // ================================
341 
342 /** In a base class, reserve memory to prepare subsequent calls to
343  * PLY_import_set_vertex */
344 void CWeightedPointsMap::PLY_import_set_vertex_count(const size_t N)
345 {
346  this->setSize(N);
347 }
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.
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:128
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...
unsigned char uint8_t
Definition: rptypes.h:44
#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:219
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:53
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:84
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
GLenum GLint x
Definition: glext.h:3542
unsigned __int32 uint32_t
Definition: rptypes.h:50
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:422
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: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019