Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
13 #include <mrpt/utils/CStream.h>
14 
15 #include "CPointsMap_crtp_common.h"
16 
17 using namespace std;
18 using namespace mrpt;
19 using namespace mrpt::maps;
20 using namespace mrpt::obs;
21 using namespace mrpt::utils;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 
25 // =========== Begin of Map definition ============
26 MAP_DEFINITION_REGISTER("CWeightedPointsMap,weightedPointsMap", mrpt::maps::CWeightedPointsMap)
27 
29 {
30 }
31 
32 void CWeightedPointsMap::TMapDefinition::loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
33 {
34  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
35  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix+string("_likelihoodOpts") );
36 }
37 
38 void CWeightedPointsMap::TMapDefinition::dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
39 {
40  this->insertionOpts.dumpToTextStream(out);
41  this->likelihoodOpts.dumpToTextStream(out);
42 }
43 
44 mrpt::maps::CMetricMap* CWeightedPointsMap::internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &_def)
45 {
46  const CWeightedPointsMap::TMapDefinition &def = *dynamic_cast<const CWeightedPointsMap::TMapDefinition*>(&_def);
48  obj->insertionOptions = def.insertionOpts;
49  obj->likelihoodOptions = def.likelihoodOpts;
50  return obj;
51 }
52 // =========== End of Map definition Block =========
53 
54 
56 
57 
58 /*---------------------------------------------------------------
59  Constructor
60  ---------------------------------------------------------------*/
62 {
63  reserve( 400 );
64 }
65 
66 /*---------------------------------------------------------------
67  Destructor
68  ---------------------------------------------------------------*/
69 CWeightedPointsMap::~CWeightedPointsMap()
70 {
71 }
72 
73 /*---------------------------------------------------------------
74  reserve & resize methods
75  ---------------------------------------------------------------*/
76 void CWeightedPointsMap::reserve(size_t newLength)
77 {
78  newLength = mrpt::utils::length2length4N(newLength);
79  x.reserve( newLength );
80  y.reserve( newLength );
81  z.reserve( newLength );
82  pointWeight.reserve(newLength);
83 }
84 
85 // Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
86 // and old contents are not changed.
87 void CWeightedPointsMap::resize(size_t newLength)
88 {
89  this->reserve(newLength); // to ensure 4N capacity
90  x.resize( newLength, 0 );
91  y.resize( newLength, 0 );
92  z.resize( newLength, 0 );
93  pointWeight.resize(newLength, 1);
94 }
95 
96 // Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
97 // and leaving all points to default values.
98 void CWeightedPointsMap::setSize(size_t newLength)
99 {
100  this->reserve(newLength); // to ensure 4N capacity
101  x.assign( newLength, 0);
102  y.assign( newLength, 0);
103  z.assign( newLength, 0);
104  pointWeight.assign( newLength, 1 );
105 }
106 
107 void CWeightedPointsMap::setPointFast(size_t index,float x,float y,float z)
108 {
109  this->x[index] = x;
110  this->y[index] = y;
111  this->z[index] = z;
112  // this->pointWeight: Unmodified
113  // mark_as_modified(); -> Fast
114 }
115 
116 void CWeightedPointsMap::insertPointFast( float x, float y, float z )
117 {
118  this->x.push_back(x);
119  this->y.push_back(y);
120  this->z.push_back(z);
121  this->pointWeight.push_back(1);
122  // mark_as_modified(); -> Fast
123 }
124 
125 /*---------------------------------------------------------------
126  Copy constructor
127  ---------------------------------------------------------------*/
128 void CWeightedPointsMap::copyFrom(const CPointsMap &obj)
129 {
130  CPointsMap::base_copyFrom(obj); // This also does a ::resize(N) of all data fields.
131 
132  const CWeightedPointsMap *pW = dynamic_cast<const CWeightedPointsMap*>(&obj);
133  if (pW)
134  {
135  pointWeight = pW->pointWeight;
136  }
137 }
138 
139 /*---------------------------------------------------------------
140  addFrom_classSpecific
141  ---------------------------------------------------------------*/
142 void CWeightedPointsMap::addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints)
143 {
144  const size_t nOther = anotherMap.size();
145 
146  // Specific data for this class:
147  const CWeightedPointsMap * anotheMap_w = dynamic_cast<const CWeightedPointsMap *>(&anotherMap);
148 
149  if (anotheMap_w)
150  {
151  for (size_t i=0,j=nPreviousPoints;i<nOther;i++, j++)
152  pointWeight[j] = anotheMap_w->pointWeight[i];
153  }
154 }
155 
156 /*---------------------------------------------------------------
157  writeToStream
158  Implements the writing to a CStream capability of
159  CSerializable objects
160  ---------------------------------------------------------------*/
161 void CWeightedPointsMap::writeToStream(mrpt::utils::CStream &out, int *version) const
162 {
163  if (version)
164  *version = 2;
165  else
166  {
167  uint32_t n = x.size();
168 
169  // First, write the number of points:
170  out << n;
171 
172  if (n>0)
173  {
174  out.WriteBufferFixEndianness(&x[0],n);
175  out.WriteBufferFixEndianness(&y[0],n);
176  out.WriteBufferFixEndianness(&z[0],n);
177  out.WriteBufferFixEndianness(&pointWeight[0],n);
178  }
179 
180  out << genericMapParams; // v2
181  insertionOptions.writeToStream(out); // version 9: insert options are saved with its own method
182  likelihoodOptions.writeToStream(out); // Added in version 5
183  }
184 }
185 
186 /*---------------------------------------------------------------
187  readFromStream
188  Implements the reading from a CStream capability of
189  CSerializable objects
190  ---------------------------------------------------------------*/
191 void CWeightedPointsMap::readFromStream(mrpt::utils::CStream &in, int version)
192 {
193  switch(version)
194  {
195  case 0:
196  case 1:
197  case 2:
198  {
199  mark_as_modified();
200 
201  // Read the number of points:
202  uint32_t n;
203  in >> n;
204 
205  this->resize(n);
206 
207  if (n>0)
208  {
209  in.ReadBufferFixEndianness(&x[0],n);
210  in.ReadBufferFixEndianness(&y[0],n);
211  in.ReadBufferFixEndianness(&z[0],n);
212  in.ReadBufferFixEndianness(&pointWeight[0],n);
213  }
214 
215  if (version>=1)
216  {
217  if (version>=2)
218  in >> genericMapParams;
219  else
220  {
221  bool disableSaveAs3DObject;
222  in >> disableSaveAs3DObject;
223  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
224  }
225 
226  insertionOptions.readFromStream(in); // version 9: insert options are saved with its own method
227  }
228  else
229  {
230  insertionOptions = TInsertionOptions();
231  in >> insertionOptions.minDistBetweenLaserPoints
232  >> insertionOptions.addToExistingPointsMap
233  >> insertionOptions.also_interpolate
234  >> insertionOptions.disableDeletion
235  >> insertionOptions.fuseWithExisting
236  >> insertionOptions.isPlanarMap
237  >> insertionOptions.maxDistForInterpolatePoints;
238  {
239  bool disableSaveAs3DObject;
240  in >> disableSaveAs3DObject;
241  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
242  }
243  in >> insertionOptions.horizontalTolerance;
244  }
245 
246  likelihoodOptions.readFromStream(in); // Added in version 5
247 
248  } break;
249  default:
251 
252  };
253 
254 }
255 
256 /*---------------------------------------------------------------
257  Clear
258  ---------------------------------------------------------------*/
259 void CWeightedPointsMap::internal_clear()
260 {
261  // This swap() thing is the only way to really deallocate the memory.
265  vector_strong_clear(pointWeight);
266 
267  mark_as_modified();
268 }
269 
270 namespace mrpt {
271  namespace maps {
272  namespace detail {
274 
276  {
277  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only once before inserting points - this is the place to reserve memory in lric for extra working variables. */
279  MRPT_UNUSED_PARAM(me);
280  MRPT_UNUSED_PARAM(lric);
281  }
282  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once per range data */
283  inline static void internal_loadFromRangeScan2D_prepareOneRange(CWeightedPointsMap &me, const float gx,const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange2DInsertContext & lric ) {
284  MRPT_UNUSED_PARAM(me);
285  MRPT_UNUSED_PARAM(gx);
286  MRPT_UNUSED_PARAM(gy);
287  MRPT_UNUSED_PARAM(gz);
288  MRPT_UNUSED_PARAM(lric);
289  }
290  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after each "{x,y,z}.push_back(...);" */
292  MRPT_UNUSED_PARAM(lric);
293  me.pointWeight.push_back(1);
294  }
295 
296  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only once before inserting points - this is the place to reserve memory in lric for extra working variables. */
298  MRPT_UNUSED_PARAM(me);
299  MRPT_UNUSED_PARAM(lric);
300  }
301  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once per range data */
302  inline static void internal_loadFromRangeScan3D_prepareOneRange(CWeightedPointsMap &me, const float gx,const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange3DInsertContext & lric ) {
303  MRPT_UNUSED_PARAM(me);
304  MRPT_UNUSED_PARAM(gx);
305  MRPT_UNUSED_PARAM(gy);
306  MRPT_UNUSED_PARAM(gz);
307  MRPT_UNUSED_PARAM(lric);
308  }
309  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after each "{x,y,z}.push_back(...);" */
311  MRPT_UNUSED_PARAM(lric);
312  me.pointWeight.push_back(1);
313  }
314  /** Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once per range data, at the end */
316  MRPT_UNUSED_PARAM(me);
317  MRPT_UNUSED_PARAM(lric);
318  }
319  };
320  }
321  }
322 }
323 
324 
325 /** See CPointsMap::loadFromRangeScan() */
326 void CWeightedPointsMap::loadFromRangeScan(
327  const CObservation2DRangeScan &rangeScan,
328  const CPose3D *robotPose)
329 {
331 }
332 
333 /** See CPointsMap::loadFromRangeScan() */
334 void CWeightedPointsMap::loadFromRangeScan(
335  const CObservation3DRangeScan &rangeScan,
336  const CPose3D *robotPose)
337 {
339 }
340 
341 
342 // ================================ PLY files import & export virtual methods ================================
343 
344 /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
345 void CWeightedPointsMap::PLY_import_set_vertex_count(const size_t N)
346 {
347  this->setSize(N);
348 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble z
Definition: glext.h:3734
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix) MRPT_OVERRIDE
Load all params from a config file/source.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:4618
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.
std::vector< uint32_t > pointWeight
The points weights.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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...
This class allows loading and storing values and vectors of different types from a configuration text...
With this struct options are provided to the observation insertion process.
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: CStream.h:139
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
T length2length4N(T len)
Returns the smaller number >=len such that it&#39;s a multiple of 4.
Definition: bits.h:209
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
GLuint index
Definition: glext.h:3891
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
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...
int version
Definition: mrpt_jpeglib.h:898
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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.
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>&#39;s clear() method, but really forcing deallocating the memory...
Definition: bits.h:205
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...
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLuint in
Definition: glext.h:6301
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
GLenum GLint GLint y
Definition: glext.h:3516
GLenum GLint x
Definition: glext.h:3516
unsigned __int32 uint32_t
Definition: rptypes.h:49
size_t size() const
Returns the number of stored points in the map.
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()
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...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019