Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CPOINTSMAP_H
10 #define CPOINTSMAP_H
11 
12 #include <mrpt/maps/CMetricMap.h>
21 #include <mrpt/obs/obs_frwds.h>
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  * - mrpt::obs::CObservationVelodyneScan
50  *
51  * Loading and saving in the standard LAS LiDAR point cloud format is supported by installing `libLAS` and including the
52  * header `<mrpt/maps/CPointsMaps_liblas.h>` in your program. Since MRPT 1.5.0 there is no need to build MRPT against libLAS to use this feature.
53  * See LAS functions in \ref mrpt_maps_liblas_grp.
54  *
55  * \sa CMetricMap, CPoint, mrpt::utils::CSerializable
56  * \ingroup mrpt_maps_grp
57  */
59  public CMetricMap,
60  public mrpt::math::KDTreeCapable<CPointsMap>,
63  {
64  // This must be added to any CSerializable derived class:
66  // This must be added for declaration of MEX-related functions
68 
69  protected:
70  /** Helper struct used for \a internal_loadFromRangeScan2D_prepareOneRange() */
72  TLaserRange2DInsertContext(const mrpt::obs::CObservation2DRangeScan &_rangeScan) : HM(mrpt::math::UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
73  { }
74  mrpt::math::CMatrixDouble44 HM; //!< Homog matrix of the local sensor pose within the robot
76  std::vector<float> fVars; //!< Extra variables to be used as desired by the derived class.
77  std::vector<unsigned int> uVars;
78  std::vector<uint8_t> bVars;
79  };
80 
81  /** Helper struct used for \a internal_loadFromRangeScan3D_prepareOneRange() */
83  TLaserRange3DInsertContext(const mrpt::obs::CObservation3DRangeScan &_rangeScan) : HM(mrpt::math::UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
84  { }
85  mrpt::math::CMatrixDouble44 HM; //!< Homog matrix of the local sensor pose within the robot
87  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.
88  std::vector<float> fVars; //!< Extra variables to be used as desired by the derived class.
89  std::vector<unsigned int> uVars;
90  std::vector<uint8_t> bVars;
91  };
92 
93  public:
94  CPointsMap(); //!< Ctor
95  virtual ~CPointsMap(); //!< Virtual destructor.
96 
97  // --------------------------------------------
98  /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
99  @{ */
100 
101  /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
102  * This is useful for situations where it is approximately known the final size of the map. This method is more
103  * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
104  */
105  virtual void reserve(size_t newLength) = 0;
106 
107  /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
108  * and old contents are not changed.
109  * \sa reserve, setPoint, setPointFast, setSize
110  */
111  virtual void resize(size_t newLength) = 0;
112 
113  /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
114  * and leaving all points to default values.
115  * \sa reserve, setPoint, setPointFast, setSize
116  */
117  virtual void setSize(size_t newLength) = 0;
118 
119  /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
120  virtual void setPointFast(size_t index,float x, float y, float z)=0;
121 
122  /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */
123  virtual void insertPointFast( float x, float y, float z = 0 ) = 0;
124 
125  /** Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source map into this one. */
126  virtual void copyFrom(const CPointsMap &obj) = 0;
127 
128  /** 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...
129  * Unlike getPointAllFields(), this method does not check for index out of bounds
130  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
131  */
132  virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const = 0;
133 
134  /** 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...
135  * Unlike setPointAllFields(), this method does not check for index out of bounds
136  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
137  */
138  virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) = 0;
139 
140  protected:
141 
142  /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */
143  virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints) = 0;
144 
145  public:
146 
147  /** @} */
148  // --------------------------------------------
149 
150 
151  /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
152  */
153  virtual float squareDistanceToClosestCorrespondence(
154  float x0,
155  float y0 ) const MRPT_OVERRIDE;
156 
158  return squareDistanceToClosestCorrespondence(static_cast<float>(p0.x),static_cast<float>(p0.y));
159  }
160 
161 
162  /** With this struct options are provided to the observation insertion process.
163  * \sa CObservation::insertIntoPointsMap
164  */
166  {
167  /** Initilization of default parameters */
169  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
170  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
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  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
198  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
199 
200  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream - for usage in derived classes' serialization
201  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream - for usage in derived classes' serialization
202 
203  double sigma_dist; //!< Sigma squared (variance, in meters) of the exponential used to model the likelihood (default= 0.5^2 meters)
204  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)
205  uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=10)
206  };
207 
209 
210 
211  /** Adds all the points from \a anotherMap to this map, without fusing.
212  * This operation can be also invoked via the "+=" operator, for example:
213  * \code
214  * mrpt::maps::CSimplePointsMap m1, m2;
215  * ...
216  * m1.addFrom( m2 ); // Add all points of m2 to m1
217  * m1 += m2; // Exactly the same than above
218  * \endcode
219  * \note The method in CPointsMap is generic but derived classes may redefine this virtual method to another one more optimized.
220  */
221  virtual void addFrom(const CPointsMap &anotherMap);
222 
223  /** This operator is synonymous with \a addFrom. \sa addFrom */
224  inline void operator +=(const CPointsMap &anotherMap)
225  {
226  this->addFrom(anotherMap);
227  }
228 
229  /** Insert the contents of another map into this one with some geometric transformation, without fusing close points.
230  * \param otherMap The other map whose points are to be inserted into this one.
231  * \param otherPose The pose of the other map in the coordinates of THIS map
232  * \sa fuseWith, addFrom
233  */
234  void insertAnotherMap(
235  const CPointsMap *otherMap,
236  const mrpt::poses::CPose3D &otherPose);
237 
238  // --------------------------------------------------
239  /** @name File input/output methods
240  @{ */
241 
242  /** Load from a text file. Each line should contain an "X Y" coordinate pair, separated by whitespaces.
243  * Returns false if any error occured, true elsewere.
244  */
245  inline bool load2D_from_text_file(const std::string &file) { return load2Dor3D_from_text_file(file,false); }
246 
247  /** Load from a text file. Each line should contain an "X Y Z" coordinate tuple, separated by whitespaces.
248  * Returns false if any error occured, true elsewere.
249  */
250  inline bool load3D_from_text_file(const std::string &file) { return load2Dor3D_from_text_file(file,true); }
251 
252  /** 2D or 3D generic implementation of \a load2D_from_text_file and load3D_from_text_file */
253  bool load2Dor3D_from_text_file(const std::string &file, const bool is_3D);
254 
255  /** Save to a text file. Each line will contain "X Y" point coordinates.
256  * Returns false if any error occured, true elsewere.
257  */
258  bool save2D_to_text_file(const std::string &file) const;
259 
260  /** Save to a text file. Each line will contain "X Y Z" point coordinates.
261  * Returns false if any error occured, true elsewere.
262  */
263  bool save3D_to_text_file(const std::string &file)const;
264 
265  /** 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) */
267  {
268  std::string fil( filNamePrefix + std::string(".txt") );
269  save3D_to_text_file( fil );
270  }
271 
272  /** 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 */
273  virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const;
274 
275  /** Load the point cloud from a PCL PCD file (requires MRPT built against PCL) \return false on any error */
276  virtual bool loadPCDFile(const std::string &filename);
277 
278  /** @} */ // End of: File input/output methods
279  // --------------------------------------------------
280 
281  /** Returns the number of stored points in the map.
282  */
283  inline size_t size() const { return x.size(); }
284 
285  /** Access to a given point from map, as a 2D point. First index is 0.
286  * \return The return value is the weight of the point (the times it has been fused), or 1 if weights are not used.
287  * \exception Throws std::exception on index out of bound.
288  * \sa setPoint, getPointFast
289  */
290  unsigned long getPoint(size_t index,float &x,float &y,float &z) const;
291  /// \overload
292  unsigned long getPoint(size_t index,float &x,float &y) const;
293  /// \overload
294  unsigned long getPoint(size_t index,double &x,double &y,double &z) const;
295  /// \overload
296  unsigned long getPoint(size_t index,double &x,double &y) const;
297  /// \overload
298  inline unsigned long getPoint(size_t index,mrpt::math::TPoint2D &p) const { return getPoint(index,p.x,p.y); }
299  /// \overload
300  inline unsigned long getPoint(size_t index,mrpt::math::TPoint3D &p) const { return getPoint(index,p.x,p.y,p.z); }
301 
302  /** 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.
303  * \return The return value is the weight of the point (the times it has been fused)
304  * \exception Throws std::exception on index out of bound.
305  */
306  virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const
307  {
308  getPoint(index,x,y,z);
309  R=G=B=1;
310  }
311 
312  /** Just like \a getPoint() but without checking out-of-bound index and without returning the point weight, just XYZ.
313  */
314  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]; }
315 
316  /** Returns true if the point map has a color field for each point */
317  virtual bool hasColorPoints() const { return false; }
318 
319  /** Changes a given point from map, with Z defaulting to 0 if not provided.
320  * \exception Throws std::exception on index out of bound.
321  */
322  inline void setPoint(size_t index,float x, float y, float z) {
323  ASSERT_BELOW_(index,this->size())
324  setPointFast(index,x,y,z);
325  mark_as_modified();
326  }
327  /// \overload
328  inline void setPoint(size_t index, const mrpt::math::TPoint2D &p) { setPoint(index,p.x,p.y,0); }
329  /// \overload
330  inline void setPoint(size_t index, const mrpt::math::TPoint3D &p) { setPoint(index,p.x,p.y,p.z); }
331  /// \overload
332  inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
333  /// overload (RGB data is ignored in classes without color information)
334  virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B)
335  {
337  setPoint(index,x,y,z);
338  }
339 
340  /// 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
341  virtual void setPointWeight(size_t index,unsigned long w)
342  {
344  }
345  /// 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
346  virtual unsigned int getPointWeight(size_t index) const { MRPT_UNUSED_PARAM(index); return 1; }
347 
348 
349  /** Provides a direct access to points buffer, or NULL if there is no points in the map.
350  */
351  void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const;
352 
353  /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
354  inline const std::vector<float> & getPointsBufferRef_x() const { return x; }
355  /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
356  inline const std::vector<float> & getPointsBufferRef_y() const { return y; }
357  /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
358  inline const std::vector<float> & getPointsBufferRef_z() const { return z; }
359 
360  /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
361  * 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.
362  * \sa getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z
363  * \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).
364  */
365  template <class VECTOR>
366  void getAllPoints( VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation = 1 ) const
367  {
368  MRPT_START
369  ASSERT_(decimation>0)
370  const size_t Nout = x.size() / decimation;
371  xs.resize(Nout);
372  ys.resize(Nout);
373  zs.resize(Nout);
374  size_t idx_in, idx_out;
375  for (idx_in=0,idx_out=0;idx_out<Nout;idx_in+=decimation,++idx_out)
376  {
377  xs[idx_out]=x[idx_in];
378  ys[idx_out]=y[idx_in];
379  zs[idx_out]=z[idx_in];
380  }
381  MRPT_END
382  }
383 
384  /** Gets all points as a STL-like container.
385  * \tparam CONTAINER Any STL-like container of mrpt::math::TPoint3D, mrpt::math::TPoint3Df or anything having members `x`,`y`,`z`.
386  * Note that this method is not efficient for large point clouds. Fastest methods are getPointsBuffer() or getPointsBufferRef_x(), getPointsBufferRef_y(), getPointsBufferRef_z()
387  */
388  template <class CONTAINER>
389  void getAllPoints(CONTAINER &ps,size_t decimation=1) const {
390  std::vector<float> dmy1,dmy2,dmy3;
391  getAllPoints(dmy1,dmy2,dmy3,decimation);
392  ps.resize(dmy1.size());
393  for (size_t i=0;i<dmy1.size();i++) {
394  ps[i].x=dmy1[i];
395  ps[i].y=dmy2[i];
396  ps[i].z=dmy3[i];
397  }
398  }
399 
400  /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
401  * 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.
402  * \sa setAllPoints
403  */
404  void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const;
405 
406  inline void getAllPoints(std::vector<mrpt::math::TPoint2D> &ps,size_t decimation=1) const {
407  std::vector<float> dmy1,dmy2;
408  getAllPoints(dmy1,dmy2,decimation);
409  ps.resize(dmy1.size());
410  for (size_t i=0;i<dmy1.size();i++) {
411  ps[i].x=static_cast<double>(dmy1[i]);
412  ps[i].y=static_cast<double>(dmy2[i]);
413  }
414  }
415 
416  /** Provides a way to insert (append) individual points into the map: the missing fields of child
417  * classes (color, weight, etc) are left to their default values
418  */
419  inline void insertPoint( float x, float y, float z=0 ) { insertPointFast(x,y,z); mark_as_modified(); }
420  /// \overload
421  inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
422  /// overload (RGB data is ignored in classes without color information)
423  virtual void insertPoint( float x, float y, float z, float R, float G, float B )
424  {
426  insertPoint(x,y,z);
427  }
428 
429  /** 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).
430  * \tparam VECTOR can be mrpt::math::CVectorFloat or std::vector<float> or any other column or row Eigen::Matrix.
431  */
432  template <typename VECTOR>
433  inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR())
434  {
435  const size_t N = X.size();
436  ASSERT_EQUAL_(X.size(),Y.size())
437  ASSERT_(Z.size()==0 || Z.size()==X.size())
438  this->setSize(N);
439  const bool z_valid = !Z.empty();
440  if (z_valid) for (size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],Z[i]); }
441  else for (size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],0); }
442  mark_as_modified();
443  }
444 
445  /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */
446  inline void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z) {
447  setAllPointsTemplate(X,Y,Z);
448  }
449 
450  /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */
451  inline void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y) {
452  setAllPointsTemplate(X,Y);
453  }
454 
455  /** 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...
456  * \sa getPointAllFieldsFast, setPointAllFields, setPointAllFieldsFast
457  */
458  void getPointAllFields( const size_t index, std::vector<float> & point_data ) const {
459  ASSERT_BELOW_(index,this->size())
460  getPointAllFieldsFast(index,point_data);
461  }
462 
463  /** 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...
464  * Unlike setPointAllFields(), this method does not check for index out of bounds
465  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
466  */
467  void setPointAllFields( const size_t index, const std::vector<float> & point_data ){
468  ASSERT_BELOW_(index,this->size())
469  setPointAllFieldsFast(index,point_data);
470  }
471 
472 
473  /** Delete points out of the given "z" axis range have been removed.
474  */
475  void clipOutOfRangeInZ(float zMin, float zMax);
476 
477  /** Delete points which are more far than "maxRange" away from the given "point".
478  */
479  void clipOutOfRange(const mrpt::math::TPoint2D &point, float maxRange);
480 
481  /** Remove from the map the points marked in a bool's array as "true".
482  * \exception std::exception If mask size is not equal to points count.
483  */
484  void applyDeletionMask( const std::vector<bool> &mask );
485 
486  // See docs in base class.
487  virtual void determineMatching2D(
488  const mrpt::maps::CMetricMap * otherMap,
489  const mrpt::poses::CPose2D & otherMapPose,
490  mrpt::utils::TMatchingPairList & correspondences,
491  const TMatchingParams & params,
492  TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
493 
494  // See docs in base class
495  virtual void determineMatching3D(
496  const mrpt::maps::CMetricMap * otherMap,
497  const mrpt::poses::CPose3D & otherMapPose,
498  mrpt::utils::TMatchingPairList & correspondences,
499  const TMatchingParams & params,
500  TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
501 
502  // See docs in base class
503  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
504 
505  /** Computes the matchings between this and another 3D points map.
506  This method matches each point in the other map with the centroid of the 3 closest points in 3D
507  from this map (if the distance is below a defined threshold).
508  * \param otherMap [IN] The other map to compute the matching with.
509  * \param otherMapPose [IN] The pose of the other map as seen from "this".
510  * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched.
511  * \param correspondences [OUT] The detected matchings pairs.
512  * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
513  *
514  * \sa determineMatching3D
515  */
516  void compute3DDistanceToMesh(
517  const mrpt::maps::CMetricMap *otherMap2,
518  const mrpt::poses::CPose3D &otherMapPose,
519  float maxDistForCorrespondence,
520  mrpt::utils::TMatchingPairList &correspondences,
521  float &correspondencesRatio );
522 
523  /** Transform the range scan into a set of cartessian coordinated
524  * points. The options in "insertionOptions" are considered in this method.
525  * \param rangeScan The scan to be inserted into this map
526  * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).
527  *
528  * Only ranges marked as "valid=true" in the observation will be inserted
529  *
530  * \note Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific
531  * implementation of mrpt::maps::CPointsMap you are using.
532  * \note The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.
533  *
534  * \sa CObservation2DRangeScan, CObservation3DRangeScan
535  */
536  virtual void loadFromRangeScan(
537  const mrpt::obs::CObservation2DRangeScan &rangeScan,
538  const mrpt::poses::CPose3D *robotPose = NULL ) = 0;
539 
540  /** Overload of \a loadFromRangeScan() for 3D range scans (for example, Kinect observations).
541  *
542  * \param rangeScan The scan to be inserted into this map
543  * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).
544  *
545  * \note Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific
546  * implementation of mrpt::maps::CPointsMap you are using.
547  * \note The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.
548  * \sa loadFromVelodyneScan
549  */
550  virtual void loadFromRangeScan(
551  const mrpt::obs::CObservation3DRangeScan &rangeScan,
552  const mrpt::poses::CPose3D *robotPose = NULL ) = 0;
553 
554  /** Like \a loadFromRangeScan() for Velodyne 3D scans. Points are translated and rotated according to the \a sensorPose field in the observation and, if provided, to the \a robotPose parameter.
555  *
556  * \param scan The Raw LIDAR data to be inserted into this map. It MUST contain point cloud data, generated by calling to \a mrpt::obs::CObservationVelodyneScan::generatePointCloud() prior to insertion in this map.
557  * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).
558  * \sa loadFromRangeScan
559  */
560  void loadFromVelodyneScan(
562  const mrpt::poses::CPose3D *robotPose = NULL );
563 
564  /** Insert the contents of another map into this one, fusing the previous content with the new one.
565  * This means that points very close to existing ones will be "fused", rather than "added". This prevents
566  * the unbounded increase in size of these class of maps.
567  * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
568  * before calling this method.
569  * \param otherMap The other map whose points are to be inserted into this one.
570  * \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.
571  * \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.
572  * \sa loadFromRangeScan, addFrom
573  */
574  void fuseWith(
575  CPointsMap *anotherMap,
576  float minDistForFuse = 0.02f,
577  std::vector<bool> *notFusedPoints = NULL);
578 
579  /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
580  */
581  void changeCoordinatesReference(const mrpt::poses::CPose2D &b);
582 
583  /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
584  */
585  void changeCoordinatesReference(const mrpt::poses::CPose3D &b);
586 
587  /** 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).
588  */
589  void changeCoordinatesReference(const CPointsMap &other, const mrpt::poses::CPose3D &b);
590 
591  /** Returns true if the map is empty/no observation has been inserted.
592  */
593  virtual bool isEmpty() const MRPT_OVERRIDE;
594 
595  /** STL-like method to check whether the map is empty: */
596  inline bool empty() const { return isEmpty(); }
597 
598  /** Returns a 3D object representing the map.
599  * The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B
600  * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
601  */
602  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const MRPT_OVERRIDE;
603 
604  /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
605  * Otherwise, return NULL
606  */
607  virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const MRPT_OVERRIDE { return NULL; }
609 
610 
611  /** 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). */
612  float getLargestDistanceFromOrigin() const;
613 
614  /** Like \a getLargestDistanceFromOrigin() but returns in \a output_is_valid = false if the distance was not already computed, skipping its computation then, unlike getLargestDistanceFromOrigin() */
615  float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const {
616  output_is_valid = m_largestDistanceFromOriginIsUpdated;
617  return m_largestDistanceFromOrigin;
618  }
619 
620  /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
621  * Results are cached unless the map is somehow modified to avoid repeated calculations.
622  */
623  void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const;
624 
625  inline void boundingBox(mrpt::math::TPoint3D &pMin,mrpt::math::TPoint3D &pMax) const {
626  float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6;
627  boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6);
628  pMin.x=dmy1;
629  pMin.y=dmy3;
630  pMin.z=dmy5;
631  pMax.x=dmy2;
632  pMax.y=dmy4;
633  pMax.z=dmy6;
634  }
635 
636  /** Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax values.
637  */
638  void extractCylinder( const mrpt::math::TPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap );
639 
640  /** Extracts the points in the map within the area defined by two corners.
641  * The points are coloured according the R,G,B input data.
642  */
643  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 );
644 
645  /** @name Filter-by-height stuff
646  @{ */
647 
648  /** Enable/disable the filter-by-height functionality \sa setHeightFilterLevels \note Default upon construction is disabled. */
649  inline void enableFilterByHeight(bool enable=true) { m_heightfilter_enabled=enable; }
650  /** Return whether filter-by-height is enabled \sa enableFilterByHeight */
651  inline bool isFilterByHeightEnabled() const { return m_heightfilter_enabled; }
652 
653  /** Set the min/max Z levels for points to be actually inserted in the map (only if \a enableFilterByHeight() was called before). */
654  inline void setHeightFilterLevels(const double _z_min, const double _z_max) { m_heightfilter_z_min=_z_min; m_heightfilter_z_max=_z_max; }
655  /** Get the min/max Z levels for points to be actually inserted in the map \sa enableFilterByHeight, setHeightFilterLevels */
656  inline void getHeightFilterLevels(double &_z_min, double &_z_max) const { _z_min=m_heightfilter_z_min; _z_max=m_heightfilter_z_max; }
657 
658  /** @} */
659 
660  /** The color [0,1] of points when extracted from getAs3DObject (default=blue) */
661  static float COLOR_3DSCENE_R;
662  static float COLOR_3DSCENE_G;
663  static float COLOR_3DSCENE_B;
664 
665 
666  // See docs in base class
667  virtual double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
668 
669  /** @name PCL library support
670  @{ */
671 
672 
673  /** Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
674  * Usage example:
675  * \code
676  * mrpt::maps::CPointsCloud pc;
677  * pcl::PointCloud<pcl::PointXYZ> cloud;
678  *
679  * pc.getPCLPointCloud(cloud);
680  * \endcode
681  * \sa setFromPCLPointCloud, CColouredPointsMap::getPCLPointCloudXYZRGB (for color data)
682  */
683  template <class POINTCLOUD>
684  void getPCLPointCloud(POINTCLOUD &cloud) const
685  {
686  const size_t nThis = this->size();
687  // Fill in the cloud data
688  cloud.width = nThis;
689  cloud.height = 1;
690  cloud.is_dense = false;
691  cloud.points.resize(cloud.width * cloud.height);
692  for (size_t i = 0; i < nThis; ++i) {
693  cloud.points[i].x =this->x[i];
694  cloud.points[i].y =this->y[i];
695  cloud.points[i].z =this->z[i];
696  }
697  }
698 
699  /** Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information, see CColouredPointsMap::setFromPCLPointCloudRGB() ).
700  * Usage example:
701  * \code
702  * pcl::PointCloud<pcl::PointXYZ> cloud;
703  * mrpt::maps::CPointsCloud pc;
704  *
705  * pc.setFromPCLPointCloud(cloud);
706  * \endcode
707  * \sa getPCLPointCloud, CColouredPointsMap::setFromPCLPointCloudRGB()
708  */
709  template <class POINTCLOUD>
710  void setFromPCLPointCloud(const POINTCLOUD &cloud)
711  {
712  const size_t N = cloud.points.size();
713  clear();
714  reserve(N);
715  for (size_t i=0;i<N;++i)
716  this->insertPointFast(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z);
717  }
718 
719  /** @} */
720 
721  /** @name Methods that MUST be implemented by children classes of KDTreeCapable
722  @{ */
723 
724  /// Must return the number of data points
725  inline size_t kdtree_get_point_count() const { return this->size(); }
726 
727  /// Returns the dim'th component of the idx'th point in the class:
728  inline float kdtree_get_pt(const size_t idx, int dim) const {
729  if (dim==0) return this->x[idx];
730  else if (dim==1) return this->y[idx];
731  else if (dim==2) return this->z[idx]; else return 0;
732  }
733 
734  /// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
735  inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t size) const
736  {
737  if (size==2)
738  {
739  const float d0 = p1[0]-x[idx_p2];
740  const float d1 = p1[1]-y[idx_p2];
741  return d0*d0+d1*d1;
742  }
743  else
744  {
745  const float d0 = p1[0]-x[idx_p2];
746  const float d1 = p1[1]-y[idx_p2];
747  const float d2 = p1[2]-z[idx_p2];
748  return d0*d0+d1*d1+d2*d2;
749  }
750  }
751 
752  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
753  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
754  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
755  template <typename BBOX>
756  bool kdtree_get_bbox(BBOX &bb) const
757  {
758  float min_z,max_z;
759  this->boundingBox(
760  bb[0].low, bb[0].high,
761  bb[1].low, bb[1].high,
762  min_z,max_z);
763  if (bb.size()==3) {
764  bb[2].low = min_z; bb[2].high = max_z;
765  }
766  return true;
767  }
768  /** @} */
769 
770  /** Users normally don't need to call this. Called by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the kd-tree cache, and such. */
771  inline void mark_as_modified() const
772  {
773  m_largestDistanceFromOriginIsUpdated=false;
774  m_boundingBoxIsUpdated = false;
775  kdtree_mark_as_outdated();
776  }
777 
778  protected:
779  std::vector<float> x,y,z; //!< The point coordinates
780 
781  mrpt::obs::CSinCosLookUpTableFor2DScans m_scans_sincos_cache; //!< Cache of sin/cos values for the latest 2D scan geometries.
782 
783  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
784  * \sa getLargestDistanceFromOrigin
785  */
787 
788  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
789  * \sa getLargestDistanceFromOrigin
790  */
792 
794  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;
795 
796  /** This is a common version of CMetricMap::insertObservation() for point maps (actually, CMetricMap::internal_insertObservation),
797  * so derived classes don't need to worry implementing that method unless something special is really necesary.
798  * See mrpt::maps::CPointsMap for the enumeration of types of observations which are accepted. */
799  bool internal_insertObservation(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose3D *robotPose) MRPT_OVERRIDE;
800 
801  /** Helper method for ::copyFrom() */
802  void base_copyFrom(const CPointsMap &obj);
803 
804 
805  /** @name PLY Import virtual methods to implement in base classes
806  @{ */
807  /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face */
808  virtual void PLY_import_set_face_count(const size_t N) MRPT_OVERRIDE { MRPT_UNUSED_PARAM(N); }
809 
810  /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
811  * \param pt_color Will be NULL if the loaded file does not provide color info.
812  */
813  virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL) MRPT_OVERRIDE;
814  /** @} */
815 
816  /** @name PLY Export virtual methods to implement in base classes
817  @{ */
818  size_t PLY_export_get_vertex_count() const MRPT_OVERRIDE;
819  size_t PLY_export_get_face_count() const MRPT_OVERRIDE { return 0; }
820  virtual void PLY_export_get_vertex(const size_t idx,mrpt::math::TPoint3Df &pt,bool &pt_has_color,mrpt::utils::TColorf &pt_color) const MRPT_OVERRIDE;
821  /** @} */
822 
823  /** The minimum and maximum height for a certain laser scan to be inserted into this map
824  * \sa m_heightfilter_enabled */
825  double m_heightfilter_z_min, m_heightfilter_z_max;
826 
827  /** Whether or not (default=not) filter the input points by height
828  * \sa m_heightfilter_z_min, m_heightfilter_z_max */
830 
831 
832  // Friend methods:
833  template <class Derived> friend struct detail::loadFromRangeImpl;
834  template <class Derived> friend struct detail::pointmap_traits;
835 
836  }; // End of class def.
838 
839  } // End of namespace
840 
841  namespace global_settings
842  {
843  /** The size of points when exporting with getAs3DObject() (default=3.0)
844  * Affects to:
845  * - mrpt::maps::CPointsMap and all its children classes.
846  */
848  }
849 
850  namespace utils
851  {
852  /** Specialization mrpt::utils::PointCloudAdapter<mrpt::maps::CPointsMap> \ingroup mrpt_adapters_grp*/
853  template <>
854  class PointCloudAdapter<mrpt::maps::CPointsMap> : public detail::PointCloudAdapterHelperNoRGB<mrpt::maps::CPointsMap,float>
855  {
856  private:
858  public:
859  typedef float coords_t; //!< The type of each point XYZ coordinates
860  static const int HAS_RGB = 0; //!< Has any color RGB info?
861  static const int HAS_RGBf = 0; //!< Has native RGB info (as floats)?
862  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
863 
864  /** Constructor (accept a const ref for convenience) */
865  inline PointCloudAdapter(const mrpt::maps::CPointsMap &obj) : m_obj(*const_cast<mrpt::maps::CPointsMap*>(&obj)) { }
866  /** Get number of points */
867  inline size_t size() const { return m_obj.size(); }
868  /** Set number of points (to uninitialized values) */
869  inline void resize(const size_t N) { m_obj.resize(N); }
870 
871  /** Get XYZ coordinates of i'th point */
872  template <typename T>
873  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
874  m_obj.getPointFast(idx,x,y,z);
875  }
876  /** Set XYZ coordinates of i'th point */
877  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
878  m_obj.setPointFast(idx,x,y,z);
879  }
880  }; // end of PointCloudAdapter<mrpt::maps::CPointsMap>
881 
882  }
883 
884 } // End of namespace
885 
886 #endif
#define ASSERT_EQUAL_(__A, __B)
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void setFromPCLPointCloud(const POINTCLOUD &cloud)
Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information...
Parameters for CMetricMap::compute3DMatchingRatio()
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.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
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
static float COLOR_3DSCENE_B
bool m_heightfilter_enabled
Whether or not (default=not) filter the input points by height.
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
static float COLOR_3DSCENE_R
The color [0,1] of points when extracted from getAs3DObject (default=blue)
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)
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
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.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
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.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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...
float maxDistForInterpolatePoints
The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)...
GLuint in
Definition: glew.h:7146
float scan_z
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
void mark_as_modified() const
Users normally don't need to call this.
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
void getAllPoints(CONTAINER &ps, size_t decimation=1) const
Gets all points as a STL-like container.
#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 squared (variance, 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...
EIGEN_STRONG_INLINE bool empty() const
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.
virtual void PLY_import_set_face_count(const size_t N) MRPT_OVERRIDE
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
void setPoint(size_t index, const mrpt::math::TPoint3D &p)
void setPoint(size_t index, float x, float y)
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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...
MAPS_IMPEXP float POINTSMAPS_3DOBJECT_POINTSIZE
The size of points when exporting with getAs3DObject() (default=3.0) Affects to:
Definition: CPointsMap.cpp:54
GLhandleARB obj
Definition: glew.h:3276
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...
GLfloat GLfloat p
Definition: glew.h:10113
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
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)
overload (RGB data is ignored in classes without color information)
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...
GLsizeiptr size
Definition: glew.h:1586
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<PointXYZ>).
#define MRPT_START
GLdouble GLdouble z
Definition: glew.h:1464
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded...
GLclampd zmax
Definition: glew.h:5006
GLsizei const GLcharARB ** string
Definition: glew.h:3293
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones...
GLuint index
Definition: glew.h:1721
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.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
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.
static float COLOR_3DSCENE_G
virtual void insertPoint(float x, float y, float z, float R, float G, float B)
overload (RGB data is ignored in classes without color information)
void setPoint(size_t index, const mrpt::math::TPoint2D &p)
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:80
GLfloat * params
Definition: glew.h:1436
PointCloudAdapter(const mrpt::maps::CPointsMap &obj)
Constructor (accept a const ref for convenience)
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
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 ...
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const MRPT_OVERRIDE
If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points ...
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
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() MRPT_OVERRIDE
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.
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Lightweight 2D point.
GLenum GLint GLuint mask
Definition: glew.h:1752
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 insertPoint(const mrpt::math::TPoint3D &p)
float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D &p0) const
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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)
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...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018