Main MRPT website > C++ reference
MRPT logo
CPointsMap.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, 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 #ifndef CPOINTSMAP_H
10 #define CPOINTSMAP_H
11 
12 #include <mrpt/slam/CMetricMap.h>
14 #include <mrpt/math/CMatrix.h>
19 #include <mrpt/poses/CPoint2D.h>
22 
23 #include <mrpt/maps/link_pragmas.h>
24 #include <mrpt/utils/adapters.h>
25 
26 namespace mrpt
27 {
28 /** \ingroup mrpt_maps_grp */
29 namespace slam
30 {
31  // Fordward declarations:
32  class CSimplePointsMap;
33  class CObservation2DRangeScan;
34  class CObservation3DRangeScan;
35 
36  using namespace mrpt::poses;
37  using namespace mrpt::math;
38 
40 
41  // Forward decls. needed to make its static methods friends of CPointsMap
42  namespace detail {
43  template <class Derived> struct loadFromRangeImpl;
44  template <class Derived> struct pointmap_traits;
45  }
46 
47 
48  /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
49  * This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap.
50  *
51  * This class implements generic version of mrpt::slam::CMetric::insertObservation() accepting these types of sensory data:
52  * - mrpt::slam::CObservation2DRangeScan: 2D range scans
53  * - mrpt::slam::CObservation3DRangeScan: 3D range scans (Kinect, etc...)
54  * - mrpt::slam::CObservationRange: IRs, Sonars, etc.
55  *
56  * If built against liblas, this class also provides method for loading and saving in the standard LAS LiDAR point cloud format: saveLASFile(), loadLASFile()
57  *
58  * \sa CMetricMap, CPoint, mrpt::utils::CSerializable
59  * \ingroup mrpt_maps_grp
60  */
62  public CMetricMap,
63  public mrpt::utils::KDTreeCapable<CPointsMap>,
66  {
67  // This must be added to any CSerializable derived class:
69 
70  protected:
71  /** Helper struct used for \a internal_loadFromRangeScan2D_prepareOneRange() */
73  TLaserRange2DInsertContext(const CObservation2DRangeScan &_rangeScan) : HM(UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
74  { }
75  CMatrixDouble44 HM; //!< Homog matrix of the local sensor pose within the robot
77  std::vector<float> fVars; //!< Extra variables to be used as desired by the derived class.
78  std::vector<unsigned int> uVars;
79  std::vector<uint8_t> bVars;
80  };
81 
82  /** Helper struct used for \a internal_loadFromRangeScan3D_prepareOneRange() */
84  TLaserRange3DInsertContext(const CObservation3DRangeScan &_rangeScan) : HM(UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
85  { }
86  CMatrixDouble44 HM; //!< Homog matrix of the local sensor pose within the robot
88  float scan_x, scan_y,scan_z; //!< In \a internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points being inserted right now.
89  std::vector<float> fVars; //!< Extra variables to be used as desired by the derived class.
90  std::vector<unsigned int> uVars;
91  std::vector<uint8_t> bVars;
92  };
93 
94  public:
95  CPointsMap(); //!< Ctor
96  virtual ~CPointsMap(); //!< Virtual destructor.
97 
98 
99  // --------------------------------------------
100  /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
101  @{ */
102 
103  /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
104  * This is useful for situations where it is approximately known the final size of the map. This method is more
105  * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
106  */
107  virtual void reserve(size_t newLength) = 0;
108 
109  /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
110  * and old contents are not changed.
111  * \sa reserve, setPoint, setPointFast, setSize
112  */
113  virtual void resize(size_t newLength) = 0;
114 
115  /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
116  * and leaving all points to default values.
117  * \sa reserve, setPoint, setPointFast, setSize
118  */
119  virtual void setSize(size_t newLength) = 0;
120 
121  /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
122  virtual void setPointFast(size_t index,float x, float y, float z)=0;
123 
124  /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */
125  virtual void insertPointFast( float x, float y, float z = 0 ) = 0;
126 
127  /** Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source map into this one. */
128  virtual void copyFrom(const CPointsMap &obj) = 0;
129 
130  /** Get all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
131  * Unlike getPointAllFields(), this method does not check for index out of bounds
132  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
133  */
134  virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const = 0;
135 
136  /** Set all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
137  * Unlike setPointAllFields(), this method does not check for index out of bounds
138  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
139  */
140  virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) = 0;
141 
142  protected:
143 
144  /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */
145  virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints) = 0;
146 
147  public:
148 
149  /** @} */
150  // --------------------------------------------
151 
152 
153  /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
154  */
155  virtual float squareDistanceToClosestCorrespondence(
156  float x0,
157  float y0 ) const MRPT_OVERRIDE;
158 
159  inline float squareDistanceToClosestCorrespondenceT(const TPoint2D &p0) const {
160  return squareDistanceToClosestCorrespondence(static_cast<float>(p0.x),static_cast<float>(p0.y));
161  }
162 
163 
164  /** With this struct options are provided to the observation insertion process.
165  * \sa CObservation::insertIntoPointsMap
166  */
168  {
169  /** Initilization of default parameters */
171  /** See utils::CLoadableOptions */
172  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section);
173  /** See utils::CLoadableOptions */
174  void dumpToTextStream(CStream &out) const;
175 
176  float minDistBetweenLaserPoints; //!< The minimum distance between points (in 3D): If two points are too close, one of them is not inserted into the map. Default is 0.02 meters.
177  bool addToExistingPointsMap; //!< Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded, clearing all previous content. Default is false.
178  bool also_interpolate; //!< If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" intervals (Default is false).
179  bool disableDeletion; //!< If set to false (default=true) points in the same plane as the inserted scan and inside the free space, are erased: i.e. they don't exist yet.
180  bool fuseWithExisting; //!< If set to true (default=false), inserted points are "fused" with previously existent ones. This shrink the size of the points map, but its slower.
181  bool isPlanarMap; //!< If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default value is false, thus 3D maps are generated). \sa horizontalTolerance
182  float horizontalTolerance; //!< The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
183  float maxDistForInterpolatePoints; //!< The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)
184  bool insertInvalidPoints; //!< Points with x,y,z coordinates set to zero will also be inserted
185 
186  void writeToStream(CStream &out) const; //!< Binary dump to stream - for usage in derived classes' serialization
187  void readFromStream(CStream &in); //!< Binary dump to stream - for usage in derived classes' serialization
188  };
189 
190  TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map
191 
192  /** Options used when evaluating "computeObservationLikelihood" in the derived classes.
193  * \sa CObservation::computeObservationLikelihood
194  */
196  {
197  /** Initilization of default parameters
198  */
200  virtual ~TLikelihoodOptions() {}
201 
202  /** See utils::CLoadableOptions */
203  void loadFromConfigFile(
204  const mrpt::utils::CConfigFileBase &source,
205  const std::string &section);
206 
207  /** See utils::CLoadableOptions */
208  void dumpToTextStream(CStream &out) const;
209 
210  void writeToStream(CStream &out) const; //!< Binary dump to stream - for usage in derived classes' serialization
211  void readFromStream(CStream &in); //!< Binary dump to stream - for usage in derived classes' serialization
212 
213  double sigma_dist; //!< Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0.5meters)
214  double max_corr_distance; //!< Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point has a minimum (but very small) likelihood to avoid underflows (default=1.0 meters)
215  uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=10)
216  };
217 
219 
220 
221  /** Adds all the points from \a anotherMap to this map, without fusing.
222  * This operation can be also invoked via the "+=" operator, for example:
223  * \code
224  * CSimplePointsMap m1, m2;
225  * ...
226  * m1.addFrom( m2 ); // Add all points of m2 to m1
227  * m1 += m2; // Exactly the same than above
228  * \endcode
229  * \note The method in CPointsMap is generic but derived classes may redefine this virtual method to another one more optimized.
230  */
231  virtual void addFrom(const CPointsMap &anotherMap);
232 
233  /** This operator is synonymous with \a addFrom. \sa addFrom */
234  inline void operator +=(const CPointsMap &anotherMap)
235  {
236  this->addFrom(anotherMap);
237  }
238 
239  /** Insert the contents of another map into this one with some geometric transformation, without fusing close points.
240  * \param otherMap The other map whose points are to be inserted into this one.
241  * \param otherPose The pose of the other map in the coordinates of THIS map
242  * \sa fuseWith, addFrom
243  */
244  void insertAnotherMap(
245  const CPointsMap *otherMap,
246  const CPose3D &otherPose);
247 
248  // --------------------------------------------------
249  /** @name File input/output methods
250  @{ */
251 
252  /** Load from a text file. Each line should contain an "X Y" coordinate pair, separated by whitespaces.
253  * Returns false if any error occured, true elsewere.
254  */
255  inline bool load2D_from_text_file(const std::string &file) { return load2Dor3D_from_text_file(file,false); }
256 
257  /** Load from a text file. Each line should contain an "X Y Z" coordinate tuple, separated by whitespaces.
258  * Returns false if any error occured, true elsewere.
259  */
260  inline bool load3D_from_text_file(const std::string &file) { return load2Dor3D_from_text_file(file,true); }
261 
262  /** 2D or 3D generic implementation of \a load2D_from_text_file and load3D_from_text_file */
263  bool load2Dor3D_from_text_file(const std::string &file, const bool is_3D);
264 
265  /** Save to a text file. Each line will contain "X Y" point coordinates.
266  * Returns false if any error occured, true elsewere.
267  */
268  bool save2D_to_text_file(const std::string &file) const;
269 
270  /** Save to a text file. Each line will contain "X Y Z" point coordinates.
271  * Returns false if any error occured, true elsewere.
272  */
273  bool save3D_to_text_file(const std::string &file)const;
274 
275  /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
276  */
278  const std::string &filNamePrefix
279  )const
280  {
281  std::string fil( filNamePrefix + std::string(".txt") );
282  save3D_to_text_file( fil );
283  }
284 
285  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format (requires MRPT built against PCL) \return false on any error */
286  virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const;
287 
288  /** Load the point cloud from a PCL PCD file (requires MRPT built against PCL) \return false on any error */
289  virtual bool loadPCDFile(const std::string &filename);
290 
291 
292  /** Optional settings for saveLASFile() */
294  {
295  // None.
296  };
297 
298  /** Optional settings for loadLASFile() */
300  {
301  // None.
302  };
303 
304  /** Extra information gathered from the LAS file header */
306  {
307  std::string FileSignature;
308  std::string SystemIdentifier;
309  std::string SoftwareIdentifier;
310  std::string project_guid;
311  std::string spatial_reference_proj4; //!< Proj.4 string describing the Spatial Reference System.
312  uint16_t creation_year;//!< Creation date (Year number)
313  uint16_t creation_DOY; //!< Creation day of year
314 
315  LAS_HeaderInfo() : creation_year(0),creation_DOY(0)
316  {}
317  };
318 
319  /** Save the point cloud as an ASPRS LAS binary file (requires MRPT built against liblas). Refer to http://www.liblas.org/
320  * \return false on any error */
321  virtual bool saveLASFile(const std::string &filename, const LAS_WriteParams & params = LAS_WriteParams() ) const;
322 
323  /** Load the point cloud from an ASPRS LAS binary file (requires MRPT built against liblas). Refer to http://www.liblas.org/
324  * \note Color (RGB) information will be taken into account if using the derived class mrpt::slam::CColouredPointsMap
325  * \return false on any error */
326  virtual bool loadLASFile(const std::string &filename, LAS_HeaderInfo &out_headerInfo, const LAS_LoadParams &params = LAS_LoadParams() );
327 
328  /** @} */ // End of: File input/output methods
329  // --------------------------------------------------
330 
331  /** Returns the number of stored points in the map.
332  */
333  inline size_t size() const { return x.size(); }
334 
335  /** Returns the number of stored points in the map (DEPRECATED, use "size()" instead better)
336  */
337  inline size_t getPointsCount() const { return size(); }
338 
339  /** Access to a given point from map, as a 2D point. First index is 0.
340  * \return The return value is the weight of the point (the times it has been fused), or 1 if weights are not used.
341  * \exception Throws std::exception on index out of bound.
342  * \sa setPoint, getPointFast
343  */
344  unsigned long getPoint(size_t index,float &x,float &y,float &z) const;
345  /// \overload
346  unsigned long getPoint(size_t index,float &x,float &y) const;
347  /// \overload
348  unsigned long getPoint(size_t index,double &x,double &y,double &z) const;
349  /// \overload
350  unsigned long getPoint(size_t index,double &x,double &y) const;
351  /// \overload
352  inline unsigned long getPoint(size_t index,CPoint2D &p) const { return getPoint(index,p.x(),p.y()); }
353  /// \overload
354  inline unsigned long getPoint(size_t index,CPoint3D &p) const { return getPoint(index,p.x(),p.y(),p.z()); }
355  /// \overload
356  inline unsigned long getPoint(size_t index,mrpt::math::TPoint2D &p) const { return getPoint(index,p.x,p.y); }
357  /// \overload
358  inline unsigned long getPoint(size_t index,mrpt::math::TPoint3D &p) const { return getPoint(index,p.x,p.y,p.z); }
359 
360  /** Access to a given point from map, and its colors, if the map defines them (othersise, R=G=B=1.0). First index is 0.
361  * \return The return value is the weight of the point (the times it has been fused)
362  * \exception Throws std::exception on index out of bound.
363  */
364  virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const
365  {
366  getPoint(index,x,y,z);
367  R=G=B=1;
368  }
369 
370  /** Just like \a getPoint() but without checking out-of-bound index and without returning the point weight, just XYZ.
371  */
372  inline void getPointFast(size_t index,float &x,float &y,float &z) const { x=this->x[index]; y=this->y[index]; z=this->z[index]; }
373 
374  /** Returns true if the point map has a color field for each point */
375  virtual bool hasColorPoints() const { return false; }
376 
377  /** Changes a given point from map, with Z defaulting to 0 if not provided.
378  * \exception Throws std::exception on index out of bound.
379  */
380  inline void setPoint(size_t index,float x, float y, float z) {
381  ASSERT_BELOW_(index,this->size())
382  setPointFast(index,x,y,z);
383  mark_as_modified();
384  }
385  /// \overload
386  inline void setPoint(size_t index,CPoint2D &p) { setPoint(index,p.x(),p.y(),0); }
387  /// \overload
388  inline void setPoint(size_t index,CPoint3D &p) { setPoint(index,p.x(),p.y(),p.z()); }
389  /// \overload
390  inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
391  /// \overload (RGB data is ignored in classes without color information)
392  virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B) { setPoint(index,x,y,z); }
393 
394  /// Sets the point weight, which is ignored in all classes but those which actually store that field (Note: No checks are done for out-of-bounds index). \sa getPointWeight
395  virtual void setPointWeight(size_t index,unsigned long w) { }
396  /// Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually store that field (Note: No checks are done for out-of-bounds index). \sa setPointWeight
397  virtual unsigned int getPointWeight(size_t index) const { return 1; }
398 
399 
400  /** Provides a direct access to points buffer, or NULL if there is no points in the map.
401  */
402  void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const;
403 
404  /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
405  inline const std::vector<float> & getPointsBufferRef_x() const { return x; }
406  /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
407  inline const std::vector<float> & getPointsBufferRef_y() const { return y; }
408  /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
409  inline const std::vector<float> & getPointsBufferRef_z() const { return z; }
410 
411  /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
412  * If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.
413  * \sa getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z
414  * \tparam VECTOR can be std::vector<float or double> or any row/column Eigen::Array or Eigen::Matrix (this includes mrpt::vector_float and mrpt::vector_double).
415  */
416  template <class VECTOR>
417  void getAllPoints( VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation = 1 ) const
418  {
419  MRPT_START
420  ASSERT_(decimation>0)
421  const size_t Nout = x.size() / decimation;
422  xs.resize(Nout);
423  ys.resize(Nout);
424  zs.resize(Nout);
425  size_t idx_in, idx_out;
426  for (idx_in=0,idx_out=0;idx_out<Nout;idx_in+=decimation,++idx_out)
427  {
428  xs[idx_out]=x[idx_in];
429  ys[idx_out]=y[idx_in];
430  zs[idx_out]=z[idx_in];
431  }
432  MRPT_END
433  }
434 
435  inline void getAllPoints(std::vector<TPoint3D> &ps,size_t decimation=1) const {
436  std::vector<float> dmy1,dmy2,dmy3;
437  getAllPoints(dmy1,dmy2,dmy3,decimation);
438  ps.resize(dmy1.size());
439  for (size_t i=0;i<dmy1.size();i++) {
440  ps[i].x=static_cast<double>(dmy1[i]);
441  ps[i].y=static_cast<double>(dmy2[i]);
442  ps[i].z=static_cast<double>(dmy3[i]);
443  }
444  }
445 
446  /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
447  * If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.
448  * \sa setAllPoints
449  */
450  void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const;
451 
452  inline void getAllPoints(std::vector<TPoint2D> &ps,size_t decimation=1) const {
453  std::vector<float> dmy1,dmy2;
454  getAllPoints(dmy1,dmy2,decimation);
455  ps.resize(dmy1.size());
456  for (size_t i=0;i<dmy1.size();i++) {
457  ps[i].x=static_cast<double>(dmy1[i]);
458  ps[i].y=static_cast<double>(dmy2[i]);
459  }
460  }
461 
462  /** Provides a way to insert (append) individual points into the map: the missing fields of child
463  * classes (color, weight, etc) are left to their default values
464  */
465  inline void insertPoint( float x, float y, float z=0 ) { insertPointFast(x,y,z); mark_as_modified(); }
466  /// \overload of \a insertPoint()
467  inline void insertPoint( const CPoint3D &p ) { insertPoint(p.x(),p.y(),p.z()); }
468  /// \overload
469  inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
470  /// \overload (RGB data is ignored in classes without color information)
471  virtual void insertPoint( float x, float y, float z, float R, float G, float B ) { insertPoint(x,y,z); }
472 
473  /** Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros).
474  * \tparam VECTOR can be mrpt::vector_float or std::vector<float> or any other column or row Eigen::Matrix.
475  */
476  template <typename VECTOR>
477  inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR())
478  {
479  const size_t N = X.size();
480  ASSERT_EQUAL_(X.size(),Y.size())
481  ASSERT_(Z.size()==0 || Z.size()==X.size())
482  this->setSize(N);
483  const bool z_valid = !Z.empty();
484  if (z_valid) for (size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],Z[i]); }
485  else for (size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],0); }
486  mark_as_modified();
487  }
488 
489  /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */
490  inline void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z) {
491  setAllPointsTemplate(X,Y,Z);
492  }
493 
494  /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */
495  inline void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y) {
496  setAllPointsTemplate(X,Y);
497  }
498 
499  /** Get all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
500  * \sa getPointAllFieldsFast, setPointAllFields, setPointAllFieldsFast
501  */
502  void getPointAllFields( const size_t index, std::vector<float> & point_data ) const {
503  ASSERT_BELOW_(index,this->size())
504  getPointAllFieldsFast(index,point_data);
505  }
506 
507  /** Set all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
508  * Unlike setPointAllFields(), this method does not check for index out of bounds
509  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
510  */
511  void setPointAllFields( const size_t index, const std::vector<float> & point_data ){
512  ASSERT_BELOW_(index,this->size())
513  setPointAllFieldsFast(index,point_data);
514  }
515 
516 
517  /** Delete points out of the given "z" axis range have been removed.
518  */
519  void clipOutOfRangeInZ(float zMin, float zMax);
520 
521  /** Delete points which are more far than "maxRange" away from the given "point".
522  */
523  void clipOutOfRange(const CPoint2D &point, float maxRange);
524 
525  /** Remove from the map the points marked in a bool's array as "true".
526  * \exception std::exception If mask size is not equal to points count.
527  */
528  void applyDeletionMask( const std::vector<bool> &mask );
529 
530  // See docs in base class.
531  virtual void determineMatching2D(
532  const CMetricMap * otherMap,
533  const CPose2D & otherMapPose,
534  TMatchingPairList & correspondences,
535  const TMatchingParams & params,
536  TMatchingExtraResults & extraResults ) const;
537 
538  // See docs in base class
539  virtual void determineMatching3D(
540  const CMetricMap * otherMap,
541  const CPose3D & otherMapPose,
542  TMatchingPairList & correspondences,
543  const TMatchingParams & params,
544  TMatchingExtraResults & extraResults ) const;
545 
546  // See docs in base class
547  float compute3DMatchingRatio(
548  const CMetricMap *otherMap,
549  const CPose3D &otherMapPose,
550  float maxDistForCorr = 0.10f,
551  float maxMahaDistForCorr = 2.0f
552  ) const;
553 
554 
555  /** Computes the matchings between this and another 3D points map.
556  This method matches each point in the other map with the centroid of the 3 closest points in 3D
557  from this map (if the distance is below a defined threshold).
558 
559  * \param otherMap [IN] The other map to compute the matching with.
560  * \param otherMapPose [IN] The pose of the other map as seen from "this".
561  * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched.
562  * \param correspondences [OUT] The detected matchings pairs.
563  * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
564  *
565  * \sa determineMatching3D
566  */
567  void compute3DDistanceToMesh(
568  const CMetricMap *otherMap2,
569  const CPose3D &otherMapPose,
570  float maxDistForCorrespondence,
571  TMatchingPairList &correspondences,
572  float &correspondencesRatio );
573 
574  /** Transform the range scan into a set of cartessian coordinated
575  * points. The options in "insertionOptions" are considered in this method.
576  * \param rangeScan The scan to be inserted into this map
577  * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class.
578  *
579  * Only ranges marked as "valid=true" in the observation will be inserted
580  *
581  * \note Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific
582  * implementation of mrpt::slam::CPointsMap you are using.
583  * \note The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.
584  *
585  * \sa CObservation2DRangeScan, CObservation3DRangeScan
586  */
587  virtual void loadFromRangeScan(
588  const CObservation2DRangeScan &rangeScan,
589  const CPose3D *robotPose = NULL ) = 0;
590 
591  /** Overload of \a loadFromRangeScan() for 3D range scans (for example, Kinect observations).
592  *
593  * \param rangeScan The scan to be inserted into this map
594  * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class.
595  *
596  * \note Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific
597  * implementation of mrpt::slam::CPointsMap you are using.
598  * \note The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.
599  */
600  virtual void loadFromRangeScan(
601  const CObservation3DRangeScan &rangeScan,
602  const CPose3D *robotPose = NULL ) = 0;
603 
604  /** Insert the contents of another map into this one, fusing the previous content with the new one.
605  * This means that points very close to existing ones will be "fused", rather than "added". This prevents
606  * the unbounded increase in size of these class of maps.
607  * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
608  * before calling this method.
609  * \param otherMap The other map whose points are to be inserted into this one.
610  * \param minDistForFuse Minimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added.
611  * \param notFusedPoints If a pointer is supplied, this list will contain at output a list with a "bool" value per point in "this" map. This will be false/true according to that point having been fused or not.
612  * \sa loadFromRangeScan, addFrom
613  */
614  void fuseWith(
615  CPointsMap *anotherMap,
616  float minDistForFuse = 0.02f,
617  std::vector<bool> *notFusedPoints = NULL);
618 
619  /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
620  */
621  void changeCoordinatesReference(const CPose2D &b);
622 
623  /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
624  */
625  void changeCoordinatesReference(const CPose3D &b);
626 
627  /** Copy all the points from "other" map to "this", replacing each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
628  */
629  void changeCoordinatesReference(const CPointsMap &other, const CPose3D &b);
630 
631  /** Returns true if the map is empty/no observation has been inserted.
632  */
633  virtual bool isEmpty() const;
634 
635  /** STL-like method to check whether the map is empty: */
636  inline bool empty() const { return isEmpty(); }
637 
638  /** Returns a 3D object representing the map.
639  * The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B
640  * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
641  */
642  virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
643 
644  /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
645  * Otherwise, return NULL
646  */
647  virtual const CSimplePointsMap * getAsSimplePointsMap() const { return NULL; }
648  virtual CSimplePointsMap * getAsSimplePointsMap() { return NULL; }
649 
650 
651  /** This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one). */
652  float getLargestDistanceFromOrigin() const;
653 
654  /** Like \a getLargestDistanceFromOrigin() but returns in \a output_is_valid = false if the distance was not already computed, skipping its computation then, unlike getLargestDistanceFromOrigin() */
655  float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const {
656  output_is_valid = m_largestDistanceFromOriginIsUpdated;
657  return m_largestDistanceFromOrigin;
658  }
659 
660  /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
661  * Results are cached unless the map is somehow modified to avoid repeated calculations.
662  */
663  void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const;
664 
665  inline void boundingBox(TPoint3D &pMin,TPoint3D &pMax) const {
666  float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6;
667  boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6);
668  pMin.x=dmy1;
669  pMin.y=dmy3;
670  pMin.z=dmy5;
671  pMax.x=dmy2;
672  pMax.y=dmy4;
673  pMax.z=dmy6;
674  }
675 
676  /** Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax values.
677  */
678  void extractCylinder( const CPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap );
679 
680  /** Extracts the points in the map within the area defined by two corners.
681  * The points are coloured according the R,G,B input data.
682  */
683  void extractPoints( const TPoint3D &corner1, const TPoint3D &corner2, CPointsMap *outMap, const double &R = 1, const double &G = 1, const double &B = 1 );
684 
685  /** @name Filter-by-height stuff
686  @{ */
687 
688  /** Enable/disable the filter-by-height functionality \sa setHeightFilterLevels \note Default upon construction is disabled. */
689  inline void enableFilterByHeight(bool enable=true) { m_heightfilter_enabled=enable; }
690  /** Return whether filter-by-height is enabled \sa enableFilterByHeight */
691  inline bool isFilterByHeightEnabled() const { return m_heightfilter_enabled; }
692 
693  /** Set the min/max Z levels for points to be actually inserted in the map (only if \a enableFilterByHeight() was called before). */
694  inline void setHeightFilterLevels(const double _z_min, const double _z_max) { m_heightfilter_z_min=_z_min; m_heightfilter_z_max=_z_max; }
695  /** Get the min/max Z levels for points to be actually inserted in the map \sa enableFilterByHeight, setHeightFilterLevels */
696  inline void getHeightFilterLevels(double &_z_min, double &_z_max) const { _z_min=m_heightfilter_z_min; _z_max=m_heightfilter_z_max; }
697 
698  /** @} */
699 
700  /** The color [0,1] of points when extracted from getAs3DObject (default=blue) */
701  static float COLOR_3DSCENE_R;
702  static float COLOR_3DSCENE_G;
703  static float COLOR_3DSCENE_B;
704 
705 
706  // See docs in base class
707  virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
708 
709  /** @name PCL library support
710  @{ */
711 
712 
713  /** Use to convert this MRPT point cloud object into a PCL point cloud object.
714  * Usage example:
715  * \code
716  * mrpt::slam::CPointsCloud pc;
717  * pcl::PointCloud<pcl::PointXYZ> cloud;
718  *
719  * pc.getPCLPointCloud(cloud);
720  * \endcode
721  * \sa setFromPCLPointCloud
722  */
723  template <class POINTCLOUD>
724  void getPCLPointCloud(POINTCLOUD &cloud) const
725  {
726  const size_t nThis = this->size();
727  // Fill in the cloud data
728  cloud.width = nThis;
729  cloud.height = 1;
730  cloud.is_dense = false;
731  cloud.points.resize(cloud.width * cloud.height);
732  for (size_t i = 0; i < nThis; ++i) {
733  cloud.points[i].x =this->x[i];
734  cloud.points[i].y =this->y[i];
735  cloud.points[i].z =this->z[i];
736  }
737  }
738 
739  /** Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information, see CColouredPointsMap::setFromPCLPointCloudRGB() ).
740  * Usage example:
741  * \code
742  * pcl::PointCloud<pcl::PointXYZ> cloud;
743  * mrpt::slam::CPointsCloud pc;
744  *
745  * pc.setFromPCLPointCloud(cloud);
746  * \endcode
747  * \sa getPCLPointCloud, CColouredPointsMap::setFromPCLPointCloudRGB()
748  */
749  template <class POINTCLOUD>
750  void setFromPCLPointCloud(const POINTCLOUD &cloud)
751  {
752  const size_t N = cloud.points.size();
753  clear();
754  reserve(N);
755  for (size_t i=0;i<N;++i)
756  this->insertPointFast(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z);
757  }
758 
759  /** @} */
760 
761  /** @name Methods that MUST be implemented by children classes of KDTreeCapable
762  @{ */
763 
764  /// Must return the number of data points
765  inline size_t kdtree_get_point_count() const { return this->size(); }
766 
767  /// Returns the dim'th component of the idx'th point in the class:
768  inline float kdtree_get_pt(const size_t idx, int dim) const {
769  if (dim==0) return this->x[idx];
770  else if (dim==1) return this->y[idx];
771  else if (dim==2) return this->z[idx]; else return 0;
772  }
773 
774  /// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
775  inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t size) const
776  {
777  if (size==2)
778  {
779  const float d0 = p1[0]-x[idx_p2];
780  const float d1 = p1[1]-y[idx_p2];
781  return d0*d0+d1*d1;
782  }
783  else
784  {
785  const float d0 = p1[0]-x[idx_p2];
786  const float d1 = p1[1]-y[idx_p2];
787  const float d2 = p1[2]-z[idx_p2];
788  return d0*d0+d1*d1+d2*d2;
789  }
790  }
791 
792  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
793  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
794  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
795  template <typename BBOX>
796  bool kdtree_get_bbox(BBOX &bb) const
797  {
798  float min_z,max_z;
799  this->boundingBox(
800  bb[0].low, bb[0].high,
801  bb[1].low, bb[1].high,
802  min_z,max_z);
803  if (bb.size()==3) {
804  bb[2].low = min_z; bb[2].high = max_z;
805  }
806  return true;
807  }
808 
809 
810  /** @} */
811 
812  protected:
813  std::vector<float> x,y,z; //!< The point coordinates
814 
815  CSinCosLookUpTableFor2DScans m_scans_sincos_cache; //!< Cache of sin/cos values for the latest 2D scan geometries.
816 
817  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
818  * \sa getLargestDistanceFromOrigin
819  */
821 
822  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
823  * \sa getLargestDistanceFromOrigin
824  */
826 
828  mutable float m_bb_min_x,m_bb_max_x, m_bb_min_y,m_bb_max_y, m_bb_min_z,m_bb_max_z;
829 
830 
831  /** Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and such. */
832  inline void mark_as_modified() const
833  {
834  m_largestDistanceFromOriginIsUpdated=false;
835  m_boundingBoxIsUpdated = false;
836  kdtree_mark_as_outdated();
837  }
838 
839  /** This is a common version of CMetricMap::insertObservation() for point maps (actually, CMetricMap::internal_insertObservation),
840  * so derived classes don't need to worry implementing that method unless something special is really necesary.
841  * See mrpt::slam::CPointsMap for the enumeration of types of observations which are accepted.
842  */
843  bool internal_insertObservation(
844  const CObservation *obs,
845  const CPose3D *robotPose);
846 
847  /** Helper method for ::copyFrom() */
848  void base_copyFrom(const CPointsMap &obj);
849 
850 
851  /** @name PLY Import virtual methods to implement in base classes
852  @{ */
853  /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face */
854  virtual void PLY_import_set_face_count(const size_t N) { }
855 
856  /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
857  * \param pt_color Will be NULL if the loaded file does not provide color info.
858  */
859  virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL);
860  /** @} */
861 
862  /** @name PLY Export virtual methods to implement in base classes
863  @{ */
864 
865  /** In a base class, return the number of vertices */
866  virtual size_t PLY_export_get_vertex_count() const;
867 
868  /** In a base class, return the number of faces */
869  virtual size_t PLY_export_get_face_count() const { return 0; }
870 
871  /** In a base class, will be called after PLY_export_get_vertex_count() once for each exported point.
872  * \param pt_color Will be NULL if the loaded file does not provide color info.
873  */
874  virtual void PLY_export_get_vertex(
875  const size_t idx,
877  bool &pt_has_color,
878  mrpt::utils::TColorf &pt_color) const;
879 
880  /** @} */
881 
882  /** The minimum and maximum height for a certain laser scan to be inserted into this map
883  * \sa m_heightfilter_enabled */
884  double m_heightfilter_z_min, m_heightfilter_z_max;
885 
886  /** Whether or not (default=not) filter the input points by height
887  * \sa m_heightfilter_z_min, m_heightfilter_z_max */
889 
890 
891  // Friend methods:
892  template <class Derived> friend struct detail::loadFromRangeImpl;
893  template <class Derived> friend struct detail::pointmap_traits;
894 
895 
896  }; // End of class def.
897 
898  } // End of namespace
899 
900  namespace global_settings
901  {
902  /** The size of points when exporting with getAs3DObject() (default=3.0)
903  * Affects to:
904  * - mrpt::slam::CPointsMap and all its children classes.
905  */
907  }
908 
909  namespace utils
910  {
911  /** Specialization mrpt::utils::PointCloudAdapter<mrpt::slam::CPointsMap> \ingroup mrpt_adapters_grp*/
912  template <>
913  class PointCloudAdapter<mrpt::slam::CPointsMap> : public detail::PointCloudAdapterHelperNoRGB<mrpt::slam::CPointsMap,float>
914  {
915  private:
917  public:
918  typedef float coords_t; //!< The type of each point XYZ coordinates
919  static const int HAS_RGB = 0; //!< Has any color RGB info?
920  static const int HAS_RGBf = 0; //!< Has native RGB info (as floats)?
921  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
922 
923  /** Constructor (accept a const ref for convenience) */
924  inline PointCloudAdapter(const mrpt::slam::CPointsMap &obj) : m_obj(*const_cast<mrpt::slam::CPointsMap*>(&obj)) { }
925  /** Get number of points */
926  inline size_t size() const { return m_obj.size(); }
927  /** Set number of points (to uninitialized values) */
928  inline void resize(const size_t N) { m_obj.resize(N); }
929 
930  /** Get XYZ coordinates of i'th point */
931  template <typename T>
932  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
933  m_obj.getPointFast(idx,x,y,z);
934  }
935  /** Set XYZ coordinates of i'th point */
936  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
937  m_obj.setPointFast(idx,x,y,z);
938  }
939  }; // end of PointCloudAdapter<mrpt::slam::CPointsMap>
940 
941  }
942 
943 } // End of namespace
944 
945 #endif
#define ASSERT_EQUAL_(__A, __B)
virtual CSimplePointsMap * getAsSimplePointsMap()
Definition: CPointsMap.h:648
void setPoint(size_t index, CPoint3D &p)
Definition: CPointsMap.h:388
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:181
void getPointAllFields(const size_t index, std::vector< float > &point_data) const
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
Definition: CPointsMap.h:502
void getAllPoints(std::vector< TPoint3D > &ps, size_t decimation=1) const
Definition: CPointsMap.h:435
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:116
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:333
MAPS_IMPEXP float POINTSMAPS_3DOBJECT_POINTSIZE
The size of points when exporting with getAs3DObject() (default=3.0) Affects to:
virtual void setPointWeight(size_t index, unsigned long w)
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
Definition: CPointsMap.h:395
virtual void PLY_import_set_face_count(const size_t N)
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face.
Definition: CPointsMap.h:854
double y
Y coordinate.
size_t getPointsCount() const
Returns the number of stored points in the map (DEPRECATED, use "size()" instead better) ...
Definition: CPointsMap.h:337
double x
X coordinate.
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:825
void setHeightFilterLevels(const double _z_min, const double _z_max)
Set the min/max Z levels for points to be actually inserted in the map (only if enableFilterByHeight(...
Definition: CPointsMap.h:694
bool load2D_from_text_file(const std::string &file)
Load from a text file.
Definition: CPointsMap.h:255
PointCloudAdapter(const mrpt::slam::CPointsMap &obj)
Constructor (accept a const ref for convenience)
Definition: CPointsMap.h:924
virtual void setPointFast(size_t index, float x, float y, float z)=0
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
Definition: CPointsMap.h:214
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
Definition: adapters.h:48
bool disableDeletion
If set to false (default=true) points in the same plane as the inserted scan and inside the free spac...
Definition: CPointsMap.h:179
bool isFilterByHeightEnabled() const
Return whether filter-by-height is enabled.
Definition: CPointsMap.h:691
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y)
Set all the points at once from vectors with X and Y coordinates (Z=0).
Definition: CPointsMap.h:495
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded...
Definition: CPointsMap.h:177
#define ASSERT_BELOW_(__A, __B)
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y, const std::vector< float > &Z)
Set all the points at once from vectors with X,Y and Z coordinates.
Definition: CPointsMap.h:490
Options used when evaluating "computeObservationLikelihood" in the derived classes.
Definition: CPointsMap.h:195
const std::vector< float > & getPointsBufferRef_y() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:407
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
Definition: CPointsMap.h:375
size_t kdtree_get_point_count() const
Must return the number of data points.
Definition: CPointsMap.h:765
float scan_z
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
Definition: CPointsMap.h:88
bool insertInvalidPoints
Points with x,y,z coordinates set to zero will also be inserted.
Definition: CPointsMap.h:184
bool m_heightfilter_enabled
Whether or not (default=not) filter the input points by height.
Definition: CPointsMap.h:888
TLikelihoodOptions likelihoodOptions
Definition: CPointsMap.h:218
void setPointAllFields(const size_t index, const std::vector< float > &point_data)
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
Definition: CPointsMap.h:511
void enableFilterByHeight(bool enable=true)
Enable/disable the filter-by-height functionality.
Definition: CPointsMap.h:689
CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:86
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:167
void resize(const size_t N)
Set number of points (to uninitialized values)
Definition: CPointsMap.h:928
Optional settings for loadLASFile()
Definition: CPointsMap.h:299
virtual void setPoint(size_t index, float x, float y, float z, float R, float G, float B)
Definition: CPointsMap.h:392
double z
Z coordinate.
float kdtree_distance(const float *p1, const size_t idx_p2, size_t size) const
Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored ...
Definition: CPointsMap.h:775
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:190
size_t size() const
Get number of points.
Definition: CPointsMap.h:926
static float COLOR_3DSCENE_G
Definition: CPointsMap.h:702
unsigned long getPoint(size_t index, mrpt::math::TPoint2D &p) const
Definition: CPointsMap.h:356
void setFromPCLPointCloud(const POINTCLOUD &cloud)
Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information...
Definition: CPointsMap.h:750
float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const
Like getLargestDistanceFromOrigin() but returns in output_is_valid = false if the distance was not al...
Definition: CPointsMap.h:655
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
Definition: CPointsMap.h:182
unsigned long getPoint(size_t index, CPoint2D &p) const
Definition: CPointsMap.h:352
This class allows loading and storing values and vectors of different types from a configuration text...
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
TLaserRange3DInsertContext(const CObservation3DRangeScan &_rangeScan)
Definition: CPointsMap.h:84
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:90
void setPoint(size_t index, CPoint2D &p)
Definition: CPointsMap.h:386
virtual void insertPoint(float x, float y, float z, float R, float G, float B)
Definition: CPointsMap.h:471
static float COLOR_3DSCENE_R
The color [0,1] of points when extracted from getAs3DObject (default=blue)
Definition: CPointsMap.h:701
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:36
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:465
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Lightweight 3D point (float version).
#define MRPT_END
uint16_t creation_year
Creation date (Year number)
Definition: CPointsMap.h:312
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10) ...
Definition: CPointsMap.h:215
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:820
Optional settings for saveLASFile()
Definition: CPointsMap.h:293
const CObservation3DRangeScan & rangeScan
Definition: CPointsMap.h:87
CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:75
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:64
TLaserRange2DInsertContext(const CObservation2DRangeScan &_rangeScan)
Definition: CPointsMap.h:73
A list of TMatchingPair.
Definition: TMatchingPair.h:60
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:61
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:72
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...
bool load3D_from_text_file(const std::string &file)
Load from a text file.
Definition: CPointsMap.h:260
unsigned long getPoint(size_t index, mrpt::math::TPoint3D &p) const
Definition: CPointsMap.h:358
const std::vector< float > & getPointsBufferRef_x() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:405
double x
X coordinate.
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:83
Parameters for the determination of matchings between point clouds, etc.
Definition: CMetricMap.h:39
A virtual base class that implements the capability of importing 3D point clouds and faces from a fil...
A class used to store a 2D point.
Definition: CPoint2D.h:35
A class used to store a 3D point.
Definition: CPoint3D.h:33
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double m_heightfilter_z_min
The minimum and maximum height for a certain laser scan to be inserted into this map.
Definition: CPointsMap.h:884
virtual const CSimplePointsMap * getAsSimplePointsMap() const
If the map is a simple points map or it&#39;s a multi-metric map that contains EXACTLY one simple points ...
Definition: CPointsMap.h:647
void getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object.
Definition: CPointsMap.h:724
virtual void resize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:77
virtual unsigned int getPointWeight(size_t index) const
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
Definition: CPointsMap.h:397
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Definition: CPointsMap.h:277
void insertPoint(const mrpt::math::TPoint3D &p)
Definition: CPointsMap.h:469
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Definition: CPointsMap.h:417
void setPoint(size_t index, float x, float y)
Definition: CPointsMap.h:390
static float COLOR_3DSCENE_B
Definition: CPointsMap.h:703
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:46
A class used to store a 2D pose.
Definition: CPose2D.h:35
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:71
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
Definition: CPointsMap.h:372
virtual size_t PLY_export_get_face_count() const
In a base class, return the number of faces.
Definition: CPointsMap.h:869
float coords_t
The type of each point XYZ coordinates.
Definition: CPointsMap.h:918
const std::vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:409
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones...
Definition: CPointsMap.h:180
double sigma_dist
Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0...
Definition: CPointsMap.h:213
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:176
void setAllPointsTemplate(const VECTOR &X, const VECTOR &Y, const VECTOR &Z=VECTOR())
Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros).
Definition: CPointsMap.h:477
uint16_t creation_DOY
Creation day of year.
Definition: CPointsMap.h:313
void getHeightFilterLevels(double &_z_min, double &_z_max) const
Get the min/max Z levels for points to be actually inserted in the map.
Definition: CPointsMap.h:696
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
Definition: CPointsMap.h:932
void insertPoint(const CPoint3D &p)
Definition: CPointsMap.h:467
#define ASSERT_(f)
A RGB color - floats in the range [0,1].
Definition: TColor.h:51
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Definition: CMetricMap.h:62
void boundingBox(TPoint3D &pMin, TPoint3D &pMax) const
Definition: CPointsMap.h:665
void mark_as_modified() const
Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and suc...
Definition: CPointsMap.h:832
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
Definition: CPointsMap.h:178
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:61
float maxDistForInterpolatePoints
The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)...
Definition: CPointsMap.h:183
float kdtree_get_pt(const size_t idx, int dim) const
Returns the dim&#39;th component of the idx&#39;th point in the class:
Definition: CPointsMap.h:768
double y
Y coordinate.
void setPointXYZ(const size_t idx, const coords_t x, const coords_t y, const coords_t z)
Set XYZ coordinates of i&#39;th point.
Definition: CPointsMap.h:936
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement (e...
An adapter to different kinds of point cloud object.
Definition: adapters.h:38
Extra information gathered from the LAS file header.
Definition: CPointsMap.h:305
float squareDistanceToClosestCorrespondenceT(const TPoint2D &p0) const
Definition: CPointsMap.h:159
Lightweight 3D point.
unsigned long getPoint(size_t index, CPoint3D &p) const
Definition: CPointsMap.h:354
bool kdtree_get_bbox(BBOX &bb) const
Definition: CPointsMap.h:796
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
Lightweight 2D point.
A virtual base class that implements the capability of exporting 3D point clouds and faces to a file ...
std::string spatial_reference_proj4
Proj.4 string describing the Spatial Reference System.
Definition: CPointsMap.h:311
bool empty() const
STL-like method to check whether the map is empty:
Definition: CPointsMap.h:636
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:89
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
Definition: CPointsMap.h:380
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
CSinCosLookUpTableFor2DScans m_scans_sincos_cache
Cache of sin/cos values for the latest 2D scan geometries.
Definition: CPointsMap.h:815
virtual void getPoint(size_t index, float &x, float &y, float &z, float &R, float &G, float &B) const
Access to a given point from map, and its colors, if the map defines them (othersise, R=G=B=1.0).
Definition: CPointsMap.h:364
const CObservation2DRangeScan & rangeScan
Definition: CPointsMap.h:76
std::vector< float > z
The point coordinates.
Definition: CPointsMap.h:813
void getAllPoints(std::vector< TPoint2D > &ps, size_t decimation=1) const
Definition: CPointsMap.h:452



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo