Main MRPT website > C++ reference for 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
260 {
261 namespace maps
262 {
263 namespace detail
264 {
266 
267 template <>
269 {
270  /** Helper method fot the generic implementation of
271  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
272  * points - this is the place to reserve memory in lric for extra working
273  * variables. */
275  CWeightedPointsMap& me,
277  {
278  MRPT_UNUSED_PARAM(me);
279  MRPT_UNUSED_PARAM(lric);
280  }
281  /** Helper method fot the generic implementation of
282  * CPointsMap::loadFromRangeScan(), to be called once per range data */
284  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
286  {
287  MRPT_UNUSED_PARAM(me);
288  MRPT_UNUSED_PARAM(gx);
289  MRPT_UNUSED_PARAM(gy);
290  MRPT_UNUSED_PARAM(gz);
291  MRPT_UNUSED_PARAM(lric);
292  }
293  /** Helper method fot the generic implementation of
294  * CPointsMap::loadFromRangeScan(), to be called after each
295  * "{x,y,z}.push_back(...);" */
297  CWeightedPointsMap& me,
299  {
300  MRPT_UNUSED_PARAM(lric);
301  me.pointWeight.push_back(1);
302  }
303 
304  /** Helper method fot the generic implementation of
305  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
306  * points - this is the place to reserve memory in lric for extra working
307  * variables. */
309  CWeightedPointsMap& me,
311  {
312  MRPT_UNUSED_PARAM(me);
313  MRPT_UNUSED_PARAM(lric);
314  }
315  /** Helper method fot the generic implementation of
316  * CPointsMap::loadFromRangeScan(), to be called once per range data */
318  CWeightedPointsMap& me, const float gx, const float gy, const float gz,
320  {
321  MRPT_UNUSED_PARAM(me);
322  MRPT_UNUSED_PARAM(gx);
323  MRPT_UNUSED_PARAM(gy);
324  MRPT_UNUSED_PARAM(gz);
325  MRPT_UNUSED_PARAM(lric);
326  }
327  /** Helper method fot the generic implementation of
328  * CPointsMap::loadFromRangeScan(), to be called after each
329  * "{x,y,z}.push_back(...);" */
331  CWeightedPointsMap& me,
333  {
334  MRPT_UNUSED_PARAM(lric);
335  me.pointWeight.push_back(1);
336  }
337  /** Helper method fot the generic implementation of
338  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
339  * end */
341  CWeightedPointsMap& me,
343  {
344  MRPT_UNUSED_PARAM(me);
345  MRPT_UNUSED_PARAM(lric);
346  }
347 };
348 }
349 }
350 }
351 
352 /** See CPointsMap::loadFromRangeScan() */
353 void CWeightedPointsMap::loadFromRangeScan(
354  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
355 {
357  templ_loadFromRangeScan(*this, rangeScan, robotPose);
358 }
359 
360 /** See CPointsMap::loadFromRangeScan() */
361 void CWeightedPointsMap::loadFromRangeScan(
362  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
363 {
365  templ_loadFromRangeScan(*this, rangeScan, robotPose);
366 }
367 
368 // ================================ PLY files import & export virtual methods
369 // ================================
370 
371 /** In a base class, reserve memory to prepare subsequent calls to
372  * PLY_import_set_vertex */
373 void CWeightedPointsMap::PLY_import_set_vertex_count(const size_t N)
374 {
375  this->setSize(N);
376 }
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan2D_init
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...
Definition: CWeightedPointsMap.cpp:274
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::maps::CWeightedPointsMap::TMapDefinition::likelihoodOpts
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: CWeightedPointsMap.h:151
mrpt::maps::CPointsMap::size
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
mrpt::maps::CWeightedPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CWeightedPointsMap.h:30
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan2D_postPushBack
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...
Definition: CWeightedPointsMap.cpp:296
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
mrpt::vector_strong_clear
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory.
Definition: bits_mem.h:16
mrpt::maps::CPointsMap::TInsertionOptions
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:205
mrpt::maps::CPointsMap::TLaserRange3DInsertContext
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:94
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:91
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::maps::CWeightedPointsMap::TMapDefinition
Definition: CWeightedPointsMap.h:147
mrpt::maps::CPointsMap::TLaserRange2DInsertContext
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:76
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::maps::CPointsMap
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
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:34
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::maps::detail::loadFromRangeImpl::templ_loadFromRangeScan
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
Definition: CPointsMap_crtp_common.h:25
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan3D_prepareOneRange
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...
Definition: CWeightedPointsMap.cpp:317
bits_mem.h
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan3D_postPushBack
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...
Definition: CWeightedPointsMap.cpp:330
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan2D_prepareOneRange
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...
Definition: CWeightedPointsMap.cpp:283
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
mrpt::obs::CObservation3DRangeScan
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
Definition: CObservation3DRangeScan.h:224
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::maps::CWeightedPointsMap::pointWeight
mrpt::aligned_std_vector< uint32_t > pointWeight
The points weights.
Definition: CWeightedPointsMap.h:133
index
GLuint index
Definition: glext.h:4054
CPointsMap_crtp_common.h
mrpt::maps::detail::pointmap_traits
Definition: CPointsMap.h:39
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:114
setSize
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...
Definition: eigen_plugins.h:343
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan3D_postOneRange
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...
Definition: CWeightedPointsMap.cpp:340
maps-precomp.h
CWeightedPointsMap.h
mrpt::maps::detail::pointmap_traits< CWeightedPointsMap >::internal_loadFromRangeScan3D_init
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...
Definition: CWeightedPointsMap.cpp:308
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
z
GLdouble GLdouble z
Definition: glext.h:3872
in
GLuint in
Definition: glext.h:7274
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::maps::CWeightedPointsMap::TMapDefinition::insertionOpts
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
Definition: CWeightedPointsMap.h:149
mrpt::maps
Definition: CBeacon.h:24
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::serialization::CArchive::WriteBufferFixEndianness
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:123
mrpt::maps::TMetricMapInitializer::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all params from a config file/source.
Definition: TMetricMapInitializer.cpp:38
y
GLenum GLint GLint y
Definition: glext.h:3538
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
x
GLenum GLint x
Definition: glext.h:3538



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST