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-2020, 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(
32  const mrpt::config::CConfigFileBase& source,
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  [[maybe_unused]] CWeightedPointsMap& me,
250  lric)
251  {
252  }
253  /** Helper method fot the generic implementation of
254  * CPointsMap::loadFromRangeScan(), to be called once per range data */
256  [[maybe_unused]] CWeightedPointsMap& me,
257  [[maybe_unused]] const float gx, [[maybe_unused]] const float gy,
258  [[maybe_unused]] const float gz,
260  lric)
261  {
262  }
263  /** Helper method fot the generic implementation of
264  * CPointsMap::loadFromRangeScan(), to be called after each
265  * "{x,y,z}.push_back(...);" */
267  CWeightedPointsMap& me,
269  lric)
270  {
271  me.pointWeight.push_back(1);
272  }
273 
274  /** Helper method fot the generic implementation of
275  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
276  * points - this is the place to reserve memory in lric for extra working
277  * variables. */
279  [[maybe_unused]] CWeightedPointsMap& me,
281  lric)
282  {
283  }
284  /** Helper method fot the generic implementation of
285  * CPointsMap::loadFromRangeScan(), to be called once per range data */
287  [[maybe_unused]] CWeightedPointsMap& me,
288  [[maybe_unused]] const float gx, [[maybe_unused]] const float gy,
289  [[maybe_unused]] const float gz,
291  lric)
292  {
293  }
294  /** Helper method fot the generic implementation of
295  * CPointsMap::loadFromRangeScan(), to be called after each
296  * "{x,y,z}.push_back(...);" */
298  CWeightedPointsMap& me,
300  lric)
301  {
302  me.pointWeight.push_back(1);
303  }
304  /** Helper method fot the generic implementation of
305  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
306  * end */
308  [[maybe_unused]] CWeightedPointsMap& me,
310  lric)
311  {
312  }
313 };
314 } // namespace mrpt::maps::detail
315 /** See CPointsMap::loadFromRangeScan() */
316 void CWeightedPointsMap::loadFromRangeScan(
317  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
318 {
320  templ_loadFromRangeScan(*this, rangeScan, robotPose);
321 }
322 
323 /** See CPointsMap::loadFromRangeScan() */
324 void CWeightedPointsMap::loadFromRangeScan(
325  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
326 {
328  templ_loadFromRangeScan(*this, rangeScan, robotPose);
329 }
330 
331 // ================================ PLY files import & export virtual methods
332 // ================================
333 
334 /** In a base class, reserve memory to prepare subsequent calls to
335  * PLY_import_set_vertex */
336 void CWeightedPointsMap::PLY_import_set_vertex_count(const size_t N)
337 {
338  this->setSize(N);
339 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
static void internal_loadFromRangeScan3D_prepareOneRange([[maybe_unused]] CWeightedPointsMap &me, [[maybe_unused]] const float gx, [[maybe_unused]] const float gy, [[maybe_unused]] const float gz, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
static void internal_loadFromRangeScan2D_postPushBack(CWeightedPointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
#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.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
STL namespace.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
static void internal_loadFromRangeScan3D_postPushBack(CWeightedPointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
static void internal_loadFromRangeScan2D_init([[maybe_unused]] CWeightedPointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
#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:220
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
static void internal_loadFromRangeScan3D_postOneRange([[maybe_unused]] CWeightedPointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
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 internal_loadFromRangeScan2D_prepareOneRange([[maybe_unused]] CWeightedPointsMap &me, [[maybe_unused]] const float gx, [[maybe_unused]] const float gy, [[maybe_unused]] const float gz, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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. ...
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.
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:95
static void internal_loadFromRangeScan3D_init([[maybe_unused]] CWeightedPointsMap &me, [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
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: CArchive.h:94
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:440
images resize(NUM_IMGS)
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:77



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020