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



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Thu Dec 10 00:07:55 UTC 2015