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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013