Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
10 
11 #include <mrpt/maps/CMetricMap.h>
21 #include <mrpt/obs/obs_frwds.h>
23 #include <mrpt/img/color_maps.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 {
33 // Forward decls. needed to make its static methods friends of CPointsMap
34 namespace detail
35 {
36 template <class Derived>
38 template <class Derived>
40 } // namespace detail
41 
42 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser
43  * scans or other sensors.
44  * This is a virtual class, thus only a derived class can be instantiated by
45  * the user. The user most usually wants to use CSimplePointsMap.
46  *
47  * This class implements generic version of
48  * mrpt::maps::CMetric::insertObservation() accepting these types of sensory
49  * data:
50  * - mrpt::obs::CObservation2DRangeScan: 2D range scans
51  * - mrpt::obs::CObservation3DRangeScan: 3D range scans (Kinect, etc...)
52  * - mrpt::obs::CObservationRange: IRs, Sonars, etc.
53  * - mrpt::obs::CObservationVelodyneScan
54  *
55  * Loading and saving in the standard LAS LiDAR point cloud format is supported
56  * by installing `libLAS` and including the
57  * header `<mrpt/maps/CPointsMaps_liblas.h>` in your program. Since MRPT 1.5.0
58  * there is no need to build MRPT against libLAS to use this feature.
59  * See LAS functions in \ref mrpt_maps_liblas_grp.
60  *
61  * \sa CMetricMap, CPoint, CSerializable
62  * \ingroup mrpt_maps_grp
63  */
64 class CPointsMap : public CMetricMap,
65  public mrpt::math::KDTreeCapable<CPointsMap>,
68 {
70  // This must be added for declaration of MEX-related functions
72 
73  protected:
74  /** Helper struct used for \a internal_loadFromRangeScan2D_prepareOneRange()
75  */
77  {
79  const mrpt::obs::CObservation2DRangeScan& _rangeScan)
80  : HM(mrpt::math::UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
81  {
82  }
83  /** Homog matrix of the local sensor pose within the robot */
86  /** Extra variables to be used as desired by the derived class. */
87  std::vector<float> fVars;
88  std::vector<unsigned int> uVars;
89  std::vector<uint8_t> bVars;
90  };
91 
92  /** Helper struct used for \a internal_loadFromRangeScan3D_prepareOneRange()
93  */
95  {
97  const mrpt::obs::CObservation3DRangeScan& _rangeScan)
98  : HM(mrpt::math::UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
99  {
100  }
101  /** Homog matrix of the local sensor pose within the robot */
104  /** In \a internal_loadFromRangeScan3D_prepareOneRange, these are the
105  * local coordinates of the scan points being inserted right now. */
107  /** Extra variables to be used as desired by the derived class. */
108  std::vector<float> fVars;
109  std::vector<unsigned int> uVars;
110  std::vector<uint8_t> bVars;
111  };
112 
113  public:
114  /** Ctor */
115  CPointsMap();
116  /** Virtual destructor. */
117  virtual ~CPointsMap();
118 
119  // --------------------------------------------
120  /** @name Pure virtual interfaces to be implemented by any class derived
121  from CPointsMap
122  @{ */
123 
124  /** Reserves memory for a given number of points: the size of the map does
125  * not change, it only reserves the memory.
126  * This is useful for situations where it is approximately known the final
127  * size of the map. This method is more
128  * efficient than constantly increasing the size of the buffers. Refer to
129  * the STL C++ library's "reserve" methods.
130  */
131  virtual void reserve(size_t newLength) = 0;
132 
133  /** Resizes all point buffers so they can hold the given number of points:
134  * newly created points are set to default values,
135  * and old contents are not changed.
136  * \sa reserve, setPoint, setPointFast, setSize
137  */
138  virtual void resize(size_t newLength) = 0;
139 
140  /** Resizes all point buffers so they can hold the given number of points,
141  * *erasing* all previous contents
142  * and leaving all points to default values.
143  * \sa reserve, setPoint, setPointFast, setSize
144  */
145  virtual void setSize(size_t newLength) = 0;
146 
147  /** Changes the coordinates of the given point (0-based index), *without*
148  * checking for out-of-bounds and *without* calling mark_as_modified() \sa
149  * setPoint */
150  virtual void setPointFast(size_t index, float x, float y, float z) = 0;
151 
152  /** The virtual method for \a insertPoint() *without* calling
153  * mark_as_modified() */
154  virtual void insertPointFast(float x, float y, float z = 0) = 0;
155 
156  /** Virtual assignment operator, copies as much common data (XYZ, color,...)
157  * as possible from the source map into this one. */
158  virtual void copyFrom(const CPointsMap& obj) = 0;
159 
160  /** Get all the data fields for one point as a vector: depending on the
161  * implementation class this can be [X Y Z] or [X Y Z R G B], etc...
162  * Unlike getPointAllFields(), this method does not check for index out of
163  * bounds
164  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
165  */
166  virtual void getPointAllFieldsFast(
167  const size_t index, std::vector<float>& point_data) const = 0;
168 
169  /** Set all the data fields for one point as a vector: depending on the
170  * implementation class this can be [X Y Z] or [X Y Z R G B], etc...
171  * Unlike setPointAllFields(), this method does not check for index out of
172  * bounds
173  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
174  */
175  virtual void setPointAllFieldsFast(
176  const size_t index, const std::vector<float>& point_data) = 0;
177 
178  protected:
179  /** Auxiliary method called from within \a addFrom() automatically, to
180  * finish the copying of class-specific data */
181  virtual void addFrom_classSpecific(
182  const CPointsMap& anotherMap, const size_t nPreviousPoints) = 0;
183 
184  public:
185  /** @} */
186  // --------------------------------------------
187 
188  /** Returns the square distance from the 2D point (x0,y0) to the closest
189  * correspondence in the map.
190  */
192  float x0, float y0) const override;
193 
195  const mrpt::math::TPoint2D& p0) const
196  {
198  static_cast<float>(p0.x), static_cast<float>(p0.y));
199  }
200 
201  /** With this struct options are provided to the observation insertion
202  * process.
203  * \sa CObservation::insertIntoPointsMap
204  */
206  {
207  /** Initilization of default parameters */
209  void loadFromConfigFile(
211  const std::string& section) override; // See base docs
212  void dumpToTextStream(
213  std::ostream& out) const override; // See base docs
214 
215  /** The minimum distance between points (in 3D): If two points are too
216  * close, one of them is not inserted into the map. Default is 0.02
217  * meters. */
219  /** Applicable to "loadFromRangeScan" only! If set to false, the points
220  * from the scan are loaded, clearing all previous content. Default is
221  * false. */
223  /** If set to true, far points (<1m) are interpolated with samples at
224  * "minDistSqrBetweenLaserPoints" intervals (Default is false). */
226  /** If set to false (default=true) points in the same plane as the
227  * inserted scan and inside the free space, are erased: i.e. they don't
228  * exist yet. */
230  /** If set to true (default=false), inserted points are "fused" with
231  * previously existent ones. This shrink the size of the points map, but
232  * its slower. */
234  /** If set to true, only HORIZONTAL (in the XY plane) measurements will
235  * be inserted in the map (Default value is false, thus 3D maps are
236  * generated). \sa horizontalTolerance */
238  /** The tolerance in rads in pitch & roll for a laser scan to be
239  * considered horizontal, considered only when isPlanarMap=true
240  * (default=0). */
242  /** The maximum distance between two points to interpolate between them
243  * (ONLY when also_interpolate=true) */
245  /** Points with x,y,z coordinates set to zero will also be inserted */
247 
248  /** Binary dump to stream - for usage in derived classes' serialization
249  */
251  /** Binary dump to stream - for usage in derived classes' serialization
252  */
254  };
255 
256  /** The options used when inserting observations in the map */
258 
259  /** Options used when evaluating "computeObservationLikelihood" in the
260  * derived classes.
261  * \sa CObservation::computeObservationLikelihood
262  */
264  {
265  /** Initilization of default parameters
266  */
268  virtual ~TLikelihoodOptions() {}
269  void loadFromConfigFile(
271  const std::string& section) override; // See base docs
272  void dumpToTextStream(
273  std::ostream& out) const override; // See base docs
274 
275  /** Binary dump to stream - for usage in derived classes' serialization
276  */
278  /** Binary dump to stream - for usage in derived classes' serialization
279  */
281 
282  /** Sigma squared (variance, in meters) of the exponential used to model
283  * the likelihood (default= 0.5^2 meters) */
284  double sigma_dist;
285  /** Maximum distance in meters to consider for the numerator divided by
286  * "sigma_dist", so that each point has a minimum (but very small)
287  * likelihood to avoid underflows (default=1.0 meters) */
289  /** Speed up the likelihood computation by considering only one out of N
290  * rays (default=10) */
292  };
294 
295  /** Rendering options, used in getAs3DObject()
296  */
298  {
299  void loadFromConfigFile(
301  const std::string& section) override; // See base docs
302  void dumpToTextStream(
303  std::ostream& out) const override; // See base docs
304 
305  /** Binary dump to stream - used in derived classes' serialization */
307  /** Binary dump to stream - used in derived classes' serialization */
309 
310  float point_size{3.0f};
311  /** Color of points. Superseded by colormap if the latter is set. */
312  mrpt::img::TColorf color{.0f, .0f, 1.0f};
313  /** Colormap for points (index is "z" coordinates) */
315  };
317 
318  /** Adds all the points from \a anotherMap to this map, without fusing.
319  * This operation can be also invoked via the "+=" operator, for example:
320  * \code
321  * mrpt::maps::CSimplePointsMap m1, m2;
322  * ...
323  * m1.addFrom( m2 ); // Add all points of m2 to m1
324  * m1 += m2; // Exactly the same than above
325  * \endcode
326  * \note The method in CPointsMap is generic but derived classes may
327  * redefine this virtual method to another one more optimized.
328  */
329  virtual void addFrom(const CPointsMap& anotherMap);
330 
331  /** This operator is synonymous with \a addFrom. \sa addFrom */
332  inline void operator+=(const CPointsMap& anotherMap)
333  {
334  this->addFrom(anotherMap);
335  }
336 
337  /** Insert the contents of another map into this one with some geometric
338  * transformation, without fusing close points.
339  * \param otherMap The other map whose points are to be inserted into this
340  * one.
341  * \param otherPose The pose of the other map in the coordinates of THIS map
342  * \sa fuseWith, addFrom
343  */
344  void insertAnotherMap(
345  const CPointsMap* otherMap, const mrpt::poses::CPose3D& otherPose);
346 
347  // --------------------------------------------------
348  /** @name File input/output methods
349  @{ */
350 
351  /** Load from a text file. Each line should contain an "X Y" coordinate
352  * pair, separated by whitespaces.
353  * Returns false if any error occured, true elsewere.
354  */
355  inline bool load2D_from_text_file(const std::string& file)
356  {
357  return load2Dor3D_from_text_file(file, false);
358  }
359 
360  /** Load from a text file. Each line should contain an "X Y Z" coordinate
361  * tuple, separated by whitespaces.
362  * Returns false if any error occured, true elsewere.
363  */
364  inline bool load3D_from_text_file(const std::string& file)
365  {
366  return load2Dor3D_from_text_file(file, true);
367  }
368 
369  /** 2D or 3D generic implementation of \a load2D_from_text_file and
370  * load3D_from_text_file */
371  bool load2Dor3D_from_text_file(const std::string& file, const bool is_3D);
372 
373  /** Save to a text file. Each line will contain "X Y" point coordinates.
374  * Returns false if any error occured, true elsewere.
375  */
376  bool save2D_to_text_file(const std::string& file) const;
377 
378  /** Save to a text file. Each line will contain "X Y Z" point coordinates.
379  * Returns false if any error occured, true elsewere.
380  */
381  bool save3D_to_text_file(const std::string& file) const;
382 
383  /** This virtual method saves the map to a file "filNamePrefix"+<
384  * some_file_extension >, as an image or in any other applicable way (Notice
385  * that other methods to save the map may be implemented in classes
386  * implementing this virtual interface) */
388  const std::string& filNamePrefix) const override
389  {
390  std::string fil(filNamePrefix + std::string(".txt"));
391  save3D_to_text_file(fil);
392  }
393 
394  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
395  * (requires MRPT built against PCL) \return false on any error */
396  virtual bool savePCDFile(
397  const std::string& filename, bool save_as_binary) const;
398 
399  /** Load the point cloud from a PCL PCD file (requires MRPT built against
400  * PCL) \return false on any error */
401  virtual bool loadPCDFile(const std::string& filename);
402 
403  /** @} */ // End of: File input/output methods
404  // --------------------------------------------------
405 
406  /** Returns the number of stored points in the map.
407  */
408  inline size_t size() const { return m_x.size(); }
409  /** Access to a given point from map, as a 2D point. First index is 0.
410  * \return The return value is the weight of the point (the times it has
411  * been fused), or 1 if weights are not used.
412  * \exception Throws std::exception on index out of bound.
413  * \sa setPoint, getPointFast
414  */
415  unsigned long getPoint(size_t index, float& x, float& y, float& z) const;
416  /// \overload
417  unsigned long getPoint(size_t index, float& x, float& y) const;
418  /// \overload
419  unsigned long getPoint(size_t index, double& x, double& y, double& z) const;
420  /// \overload
421  unsigned long getPoint(size_t index, double& x, double& y) const;
422  /// \overload
423  inline unsigned long getPoint(size_t index, mrpt::math::TPoint2D& p) const
424  {
425  return getPoint(index, p.x, p.y);
426  }
427  /// \overload
428  inline unsigned long getPoint(size_t index, mrpt::math::TPoint3D& p) const
429  {
430  return getPoint(index, p.x, p.y, p.z);
431  }
432 
433  /** Access to a given point from map, and its colors, if the map defines
434  * them (othersise, R=G=B=1.0). First index is 0.
435  * \return The return value is the weight of the point (the times it has
436  * been fused)
437  * \exception Throws std::exception on index out of bound.
438  */
439  virtual void getPoint(
440  size_t index, float& x, float& y, float& z, float& R, float& G,
441  float& B) const
442  {
443  getPoint(index, x, y, z);
444  R = G = B = 1;
445  }
446 
447  /** Just like \a getPoint() but without checking out-of-bound index and
448  * without returning the point weight, just XYZ.
449  */
450  inline void getPointFast(size_t index, float& x, float& y, float& z) const
451  {
452  x = m_x[index];
453  y = m_y[index];
454  z = m_z[index];
455  }
456 
457  /** Returns true if the point map has a color field for each point */
458  virtual bool hasColorPoints() const { return false; }
459  /** Changes a given point from map, with Z defaulting to 0 if not provided.
460  * \exception Throws std::exception on index out of bound.
461  */
462  inline void setPoint(size_t index, float x, float y, float z)
463  {
464  ASSERT_BELOW_(index, this->size());
465  setPointFast(index, x, y, z);
467  }
468  /// \overload
469  inline void setPoint(size_t index, const mrpt::math::TPoint2D& p)
470  {
471  setPoint(index, p.x, p.y, 0);
472  }
473  /// \overload
474  inline void setPoint(size_t index, const mrpt::math::TPoint3D& p)
475  {
476  setPoint(index, p.x, p.y, p.z);
477  }
478  /// \overload
479  inline void setPoint(size_t index, float x, float y)
480  {
481  setPoint(index, x, y, 0);
482  }
483  /// overload (RGB data is ignored in classes without color information)
484  virtual void setPoint(
485  size_t index, float x, float y, float z, float R, float G, float B)
486  {
490  setPoint(index, x, y, z);
491  }
492 
493  /// Sets the point weight, which is ignored in all classes but those which
494  /// actually store that field (Note: No checks are done for out-of-bounds
495  /// index). \sa getPointWeight
496  virtual void setPointWeight(size_t index, unsigned long w)
497  {
500  }
501  /// Gets the point weight, which is ignored in all classes (defaults to 1)
502  /// but in those which actually store that field (Note: No checks are done
503  /// for out-of-bounds index). \sa setPointWeight
504  virtual unsigned int getPointWeight(size_t index) const
505  {
507  return 1;
508  }
509 
510  /** Provides a direct access to points buffer, or nullptr if there is no
511  * points in the map.
512  */
513  void getPointsBuffer(
514  size_t& outPointsCount, const float*& xs, const float*& ys,
515  const float*& zs) const;
516 
517  /** Provides a direct access to a read-only reference of the internal point
518  * buffer. \sa getAllPoints */
519  inline const mrpt::aligned_std_vector<float>& getPointsBufferRef_x() const { return m_x; }
520  /** Provides a direct access to a read-only reference of the internal point
521  * buffer. \sa getAllPoints */
522  inline const mrpt::aligned_std_vector<float>& getPointsBufferRef_y() const { return m_y; }
523  /** Provides a direct access to a read-only reference of the internal point
524  * buffer. \sa getAllPoints */
525  inline const mrpt::aligned_std_vector<float>& getPointsBufferRef_z() const { return m_z; }
526  /** Returns a copy of the 2D/3D points as a std::vector of float
527  * coordinates.
528  * If decimation is greater than 1, only 1 point out of that number will be
529  * saved in the output, effectively performing a subsampling of the points.
530  * \sa getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z
531  * \tparam VECTOR can be std::vector<float or double> or any row/column
532  * Eigen::Array or Eigen::Matrix (this includes mrpt::math::CVectorFloat and
533  * mrpt::math::CVectorDouble).
534  */
535  template <class VECTOR>
537  VECTOR& xs, VECTOR& ys, VECTOR& zs, size_t decimation = 1) const
538  {
539  MRPT_START
540  ASSERT_(decimation > 0);
541  const size_t Nout = m_x.size() / decimation;
542  xs.resize(Nout);
543  ys.resize(Nout);
544  zs.resize(Nout);
545  size_t idx_in, idx_out;
546  for (idx_in = 0, idx_out = 0; idx_out < Nout;
547  idx_in += decimation, ++idx_out)
548  {
549  xs[idx_out] = m_x[idx_in];
550  ys[idx_out] = m_y[idx_in];
551  zs[idx_out] = m_z[idx_in];
552  }
553  MRPT_END
554  }
555 
556  /** Gets all points as a STL-like container.
557  * \tparam CONTAINER Any STL-like container of mrpt::math::TPoint3D,
558  * mrpt::math::TPoint3Df or anything having members `x`,`y`,`z`.
559  * Note that this method is not efficient for large point clouds. Fastest
560  * methods are getPointsBuffer() or getPointsBufferRef_x(),
561  * getPointsBufferRef_y(), getPointsBufferRef_z()
562  */
563  template <class CONTAINER>
564  void getAllPoints(CONTAINER& ps, size_t decimation = 1) const
565  {
566  std::vector<float> dmy1, dmy2, dmy3;
567  getAllPoints(dmy1, dmy2, dmy3, decimation);
568  ps.resize(dmy1.size());
569  for (size_t i = 0; i < dmy1.size(); i++)
570  {
571  ps[i].x = dmy1[i];
572  ps[i].y = dmy2[i];
573  ps[i].z = dmy3[i];
574  }
575  }
576 
577  /** Returns a copy of the 2D/3D points as a std::vector of float
578  * coordinates.
579  * If decimation is greater than 1, only 1 point out of that number will be
580  * saved in the output, effectively performing a subsampling of the points.
581  * \sa setAllPoints
582  */
583  void getAllPoints(
584  std::vector<float>& xs, std::vector<float>& ys,
585  size_t decimation = 1) const;
586 
587  inline void getAllPoints(
588  std::vector<mrpt::math::TPoint2D>& ps, size_t decimation = 1) const
589  {
590  std::vector<float> dmy1, dmy2;
591  getAllPoints(dmy1, dmy2, decimation);
592  ps.resize(dmy1.size());
593  for (size_t i = 0; i < dmy1.size(); i++)
594  {
595  ps[i].x = static_cast<double>(dmy1[i]);
596  ps[i].y = static_cast<double>(dmy2[i]);
597  }
598  }
599 
600  /** Provides a way to insert (append) individual points into the map: the
601  * missing fields of child
602  * classes (color, weight, etc) are left to their default values
603  */
604  inline void insertPoint(float x, float y, float z = 0)
605  {
606  insertPointFast(x, y, z);
608  }
609  /// \overload
610  inline void insertPoint(const mrpt::math::TPoint3D& p)
611  {
612  insertPoint(p.x, p.y, p.z);
613  }
614  /// overload (RGB data is ignored in classes without color information)
615  virtual void insertPoint(
616  float x, float y, float z, float R, float G, float B)
617  {
621  insertPoint(x, y, z);
622  }
623 
624  /** Set all the points at once from vectors with X,Y and Z coordinates (if Z
625  * is not provided, it will be set to all zeros).
626  * \tparam VECTOR can be mrpt::math::CVectorFloat or std::vector<float> or
627  * any other column or row Eigen::Matrix.
628  */
629  template <typename VECTOR>
630  inline void setAllPointsTemplate(
631  const VECTOR& X, const VECTOR& Y, const VECTOR& Z = VECTOR())
632  {
633  const size_t N = X.size();
634  ASSERT_EQUAL_(X.size(), Y.size());
635  ASSERT_(Z.size() == 0 || Z.size() == X.size());
636  this->setSize(N);
637  const bool z_valid = !Z.empty();
638  if (z_valid)
639  for (size_t i = 0; i < N; i++)
640  {
641  this->setPointFast(i, X[i], Y[i], Z[i]);
642  }
643  else
644  for (size_t i = 0; i < N; i++)
645  {
646  this->setPointFast(i, X[i], Y[i], 0);
647  }
649  }
650 
651  /** Set all the points at once from vectors with X,Y and Z coordinates. \sa
652  * getAllPoints */
653  inline void setAllPoints(
654  const std::vector<float>& X, const std::vector<float>& Y,
655  const std::vector<float>& Z)
656  {
657  setAllPointsTemplate(X, Y, Z);
658  }
659 
660  /** Set all the points at once from vectors with X and Y coordinates (Z=0).
661  * \sa getAllPoints */
662  inline void setAllPoints(
663  const std::vector<float>& X, const std::vector<float>& Y)
664  {
665  setAllPointsTemplate(X, Y);
666  }
667 
668  /** Get all the data fields for one point as a vector: depending on the
669  * implementation class this can be [X Y Z] or [X Y Z R G B], etc...
670  * \sa getPointAllFieldsFast, setPointAllFields, setPointAllFieldsFast
671  */
673  const size_t index, std::vector<float>& point_data) const
674  {
675  ASSERT_BELOW_(index, this->size());
676  getPointAllFieldsFast(index, point_data);
677  }
678 
679  /** Set all the data fields for one point as a vector: depending on the
680  * implementation class this can be [X Y Z] or [X Y Z R G B], etc...
681  * Unlike setPointAllFields(), this method does not check for index out of
682  * bounds
683  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
684  */
686  const size_t index, const std::vector<float>& point_data)
687  {
688  ASSERT_BELOW_(index, this->size());
689  setPointAllFieldsFast(index, point_data);
690  }
691 
692  /** Delete points out of the given "z" axis range have been removed.
693  */
694  void clipOutOfRangeInZ(float zMin, float zMax);
695 
696  /** Delete points which are more far than "maxRange" away from the given
697  * "point".
698  */
699  void clipOutOfRange(const mrpt::math::TPoint2D& point, float maxRange);
700 
701  /** Remove from the map the points marked in a bool's array as "true".
702  * \exception std::exception If mask size is not equal to points count.
703  */
704  void applyDeletionMask(const std::vector<bool>& mask);
705 
706  // See docs in base class.
707  virtual void determineMatching2D(
708  const mrpt::maps::CMetricMap* otherMap,
709  const mrpt::poses::CPose2D& otherMapPose,
710  mrpt::tfest::TMatchingPairList& correspondences,
711  const TMatchingParams& params,
712  TMatchingExtraResults& extraResults) const override;
713 
714  // See docs in base class
715  virtual void determineMatching3D(
716  const mrpt::maps::CMetricMap* otherMap,
717  const mrpt::poses::CPose3D& otherMapPose,
718  mrpt::tfest::TMatchingPairList& correspondences,
719  const TMatchingParams& params,
720  TMatchingExtraResults& extraResults) const override;
721 
722  // See docs in base class
724  const mrpt::maps::CMetricMap* otherMap,
725  const mrpt::poses::CPose3D& otherMapPose,
726  const TMatchingRatioParams& params) const override;
727 
728  /** Computes the matchings between this and another 3D points map.
729  This method matches each point in the other map with the centroid of the
730  3 closest points in 3D
731  from this map (if the distance is below a defined threshold).
732  * \param otherMap [IN] The other map to compute the
733  matching with.
734  * \param otherMapPose [IN] The pose of the other map as seen
735  from "this".
736  * \param maxDistForCorrespondence [IN] Maximum 2D linear distance
737  between two points to be matched.
738  * \param correspondences [OUT] The detected matchings pairs.
739  * \param correspondencesRatio [OUT] The ratio [0,1] of points in
740  otherMap with at least one correspondence.
741  *
742  * \sa determineMatching3D
743  */
745  const mrpt::maps::CMetricMap* otherMap2,
746  const mrpt::poses::CPose3D& otherMapPose,
747  float maxDistForCorrespondence,
748  mrpt::tfest::TMatchingPairList& correspondences,
749  float& correspondencesRatio);
750 
751  /** Transform the range scan into a set of cartessian coordinated
752  * points. The options in "insertionOptions" are considered in this
753  *method.
754  * \param rangeScan The scan to be inserted into this map
755  * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of
756  *reference for the point cloud (i.e. the vehicle/robot pose in world
757  *coordinates).
758  *
759  * Only ranges marked as "valid=true" in the observation will be inserted
760  *
761  * \note Each derived class may enrich points in different ways (color,
762  *weight, etc..), so please refer to the description of the specific
763  * implementation of mrpt::maps::CPointsMap you are using.
764  * \note The actual generic implementation of this file lives in
765  *<src>/CPointsMap_crtp_common.h, but specific instantiations are generated
766  *at each derived class.
767  *
768  * \sa CObservation2DRangeScan, CObservation3DRangeScan
769  */
770  virtual void loadFromRangeScan(
771  const mrpt::obs::CObservation2DRangeScan& rangeScan,
772  const mrpt::poses::CPose3D* robotPose = nullptr) = 0;
773 
774  /** Overload of \a loadFromRangeScan() for 3D range scans (for example,
775  * Kinect observations).
776  *
777  * \param rangeScan The scan to be inserted into this map
778  * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of
779  * reference for the point cloud (i.e. the vehicle/robot pose in world
780  * coordinates).
781  *
782  * \note Each derived class may enrich points in different ways (color,
783  * weight, etc..), so please refer to the description of the specific
784  * implementation of mrpt::maps::CPointsMap you are using.
785  * \note The actual generic implementation of this file lives in
786  * <src>/CPointsMap_crtp_common.h, but specific instantiations are generated
787  * at each derived class.
788  * \sa loadFromVelodyneScan
789  */
790  virtual void loadFromRangeScan(
791  const mrpt::obs::CObservation3DRangeScan& rangeScan,
792  const mrpt::poses::CPose3D* robotPose = nullptr) = 0;
793 
794  /** Like \a loadFromRangeScan() for Velodyne 3D scans. Points are translated
795  * and rotated according to the \a sensorPose field in the observation and,
796  * if provided, to the \a robotPose parameter.
797  *
798  * \param scan The Raw LIDAR data to be inserted into this map. It MUST
799  * contain point cloud data, generated by calling to \a
800  * mrpt::obs::CObservationVelodyneScan::generatePointCloud() prior to
801  * insertion in this map.
802  * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of
803  * reference for the point cloud (i.e. the vehicle/robot pose in world
804  * coordinates).
805  * \sa loadFromRangeScan
806  */
809  const mrpt::poses::CPose3D* robotPose = nullptr);
810 
811  /** Insert the contents of another map into this one, fusing the previous
812  *content with the new one.
813  * This means that points very close to existing ones will be "fused",
814  *rather than "added". This prevents
815  * the unbounded increase in size of these class of maps.
816  * NOTICE that "otherMap" is neither translated nor rotated here, so if
817  *this is desired it must done
818  * before calling this method.
819  * \param otherMap The other map whose points are to be inserted into this
820  *one.
821  * \param minDistForFuse Minimum distance (in meters) between two points,
822  *each one in a map, to be considered the same one and be fused rather than
823  *added.
824  * \param notFusedPoints If a pointer is supplied, this list will contain at
825  *output a list with a "bool" value per point in "this" map. This will be
826  *false/true according to that point having been fused or not.
827  * \sa loadFromRangeScan, addFrom
828  */
829  void fuseWith(
830  CPointsMap* anotherMap, float minDistForFuse = 0.02f,
831  std::vector<bool>* notFusedPoints = nullptr);
832 
833  /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose
834  * compounding operator).
835  */
837 
838  /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose
839  * compounding operator).
840  */
842 
843  /** Copy all the points from "other" map to "this", replacing each point \f$
844  * p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
845  */
847  const CPointsMap& other, const mrpt::poses::CPose3D& b);
848 
849  /** Returns true if the map is empty/no observation has been inserted.
850  */
851  virtual bool isEmpty() const override;
852 
853  /** STL-like method to check whether the map is empty: */
854  inline bool empty() const { return isEmpty(); }
855  /** Returns a 3D object representing the map.
856  * The color of the points is controlled by renderOptions
857  */
858  virtual void getAs3DObject(
859  mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
860 
861  /** If the map is a simple points map or it's a multi-metric map that
862  * contains EXACTLY one simple points map, return it.
863  * Otherwise, return NULL
864  */
866  const override
867  {
868  return nullptr;
869  }
871  {
872  return nullptr;
873  }
874 
875  /** This method returns the largest distance from the origin to any of the
876  * points, such as a sphere centered at the origin with this radius cover
877  * ALL the points in the map (the results are buffered, such as, if the map
878  * is not modified, the second call will be much faster than the first one).
879  */
880  float getLargestDistanceFromOrigin() const;
881 
882  /** Like \a getLargestDistanceFromOrigin() but returns in \a output_is_valid
883  * = false if the distance was not already computed, skipping its
884  * computation then, unlike getLargestDistanceFromOrigin() */
885  float getLargestDistanceFromOriginNoRecompute(bool& output_is_valid) const
886  {
887  output_is_valid = m_largestDistanceFromOriginIsUpdated;
889  }
890 
891  /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there
892  * are no points.
893  * Results are cached unless the map is somehow modified to avoid repeated
894  * calculations.
895  */
896  void boundingBox(
897  float& min_x, float& max_x, float& min_y, float& max_y, float& min_z,
898  float& max_z) const;
899 
900  inline void boundingBox(
901  mrpt::math::TPoint3D& pMin, mrpt::math::TPoint3D& pMax) const
902  {
903  float dmy1, dmy2, dmy3, dmy4, dmy5, dmy6;
904  boundingBox(dmy1, dmy2, dmy3, dmy4, dmy5, dmy6);
905  pMin.x = dmy1;
906  pMin.y = dmy3;
907  pMin.z = dmy5;
908  pMax.x = dmy2;
909  pMax.y = dmy4;
910  pMax.z = dmy6;
911  }
912 
913  /** Extracts the points in the map within a cylinder in 3D defined the
914  * provided radius and zmin/zmax values.
915  */
916  void extractCylinder(
917  const mrpt::math::TPoint2D& center, const double radius,
918  const double zmin, const double zmax, CPointsMap* outMap);
919 
920  /** Extracts the points in the map within the area defined by two corners.
921  * The points are coloured according the R,G,B input data.
922  */
923  void extractPoints(
924  const mrpt::math::TPoint3D& corner1,
925  const mrpt::math::TPoint3D& corner2, CPointsMap* outMap,
926  const double& R = 1, const double& G = 1, const double& B = 1);
927 
928  /** @name Filter-by-height stuff
929  @{ */
930 
931  /** Enable/disable the filter-by-height functionality \sa
932  * setHeightFilterLevels \note Default upon construction is disabled. */
933  inline void enableFilterByHeight(bool enable = true)
934  {
935  m_heightfilter_enabled = enable;
936  }
937  /** Return whether filter-by-height is enabled \sa enableFilterByHeight */
938  inline bool isFilterByHeightEnabled() const
939  {
940  return m_heightfilter_enabled;
941  }
942 
943  /** Set the min/max Z levels for points to be actually inserted in the map
944  * (only if \a enableFilterByHeight() was called before). */
945  inline void setHeightFilterLevels(const double _z_min, const double _z_max)
946  {
947  m_heightfilter_z_min = _z_min;
948  m_heightfilter_z_max = _z_max;
949  }
950  /** Get the min/max Z levels for points to be actually inserted in the map
951  * \sa enableFilterByHeight, setHeightFilterLevels */
952  inline void getHeightFilterLevels(double& _z_min, double& _z_max) const
953  {
954  _z_min = m_heightfilter_z_min;
955  _z_max = m_heightfilter_z_max;
956  }
957 
958  /** @} */
959 
960  // See docs in base class
962  const mrpt::obs::CObservation* obs,
963  const mrpt::poses::CPose3D& takenFrom) override;
964 
965  /** @name PCL library support
966  @{ */
967 
968  /** Use to convert this MRPT point cloud object into a PCL point cloud
969  * object (PointCloud<PointXYZ>).
970  * Usage example:
971  * \code
972  * mrpt::maps::CPointsCloud pc;
973  * pcl::PointCloud<pcl::PointXYZ> cloud;
974  *
975  * pc.getPCLPointCloud(cloud);
976  * \endcode
977  * \sa setFromPCLPointCloud, CColouredPointsMap::getPCLPointCloudXYZRGB
978  * (for color data)
979  */
980  template <class POINTCLOUD>
981  void getPCLPointCloud(POINTCLOUD& cloud) const
982  {
983  const size_t nThis = this->size();
984  // Fill in the cloud data
985  cloud.width = nThis;
986  cloud.height = 1;
987  cloud.is_dense = false;
988  cloud.points.resize(cloud.width * cloud.height);
989  for (size_t i = 0; i < nThis; ++i)
990  {
991  cloud.points[i].x = m_x[i];
992  cloud.points[i].y = m_y[i];
993  cloud.points[i].z = m_z[i];
994  }
995  }
996 
997  /** Loads a PCL point cloud into this MRPT class (note: this method ignores
998  * potential RGB information, see
999  * CColouredPointsMap::setFromPCLPointCloudRGB() ).
1000  * Usage example:
1001  * \code
1002  * pcl::PointCloud<pcl::PointXYZ> cloud;
1003  * mrpt::maps::CPointsCloud pc;
1004  *
1005  * pc.setFromPCLPointCloud(cloud);
1006  * \endcode
1007  * \sa getPCLPointCloud, CColouredPointsMap::setFromPCLPointCloudRGB()
1008  */
1009  template <class POINTCLOUD>
1010  void setFromPCLPointCloud(const POINTCLOUD& cloud)
1011  {
1012  const size_t N = cloud.points.size();
1013  clear();
1014  reserve(N);
1015  for (size_t i = 0; i < N; ++i)
1016  this->insertPointFast(
1017  cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
1018  }
1019 
1020  /** @} */
1021 
1022  /** @name Methods that MUST be implemented by children classes of
1023  KDTreeCapable
1024  @{ */
1025 
1026  /// Must return the number of data points
1027  inline size_t kdtree_get_point_count() const { return this->size(); }
1028  /// Returns the dim'th component of the idx'th point in the class:
1029  inline float kdtree_get_pt(const size_t idx, int dim) const
1030  {
1031  if (dim == 0)
1032  return m_x[idx];
1033  else if (dim == 1)
1034  return m_y[idx];
1035  else if (dim == 2)
1036  return m_z[idx];
1037  else
1038  return 0;
1039  }
1040 
1041  /// Returns the distance between the vector "p1[0:size-1]" and the data
1042  /// point with index "idx_p2" stored in the class:
1043  inline float kdtree_distance(
1044  const float* p1, const size_t idx_p2, size_t size) const
1045  {
1046  if (size == 2)
1047  {
1048  const float d0 = p1[0] - m_x[idx_p2];
1049  const float d1 = p1[1] - m_y[idx_p2];
1050  return d0 * d0 + d1 * d1;
1051  }
1052  else
1053  {
1054  const float d0 = p1[0] - m_x[idx_p2];
1055  const float d1 = p1[1] - m_y[idx_p2];
1056  const float d2 = p1[2] - m_z[idx_p2];
1057  return d0 * d0 + d1 * d1 + d2 * d2;
1058  }
1059  }
1060 
1061  // Optional bounding-box computation: return false to default to a standard
1062  // bbox computation loop.
1063  // Return true if the BBOX was already computed by the class and returned
1064  // in "bb" so it can be avoided to redo it again.
1065  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
1066  // for point clouds)
1067  template <typename BBOX>
1068  bool kdtree_get_bbox(BBOX& bb) const
1069  {
1070  float min_z, max_z;
1071  this->boundingBox(
1072  bb[0].low, bb[0].high, bb[1].low, bb[1].high, min_z, max_z);
1073  if (bb.size() == 3)
1074  {
1075  bb[2].low = min_z;
1076  bb[2].high = max_z;
1077  }
1078  return true;
1079  }
1080  /** @} */
1081 
1082  /** Users normally don't need to call this. Called by this class or children
1083  * classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the
1084  * kd-tree cache, and such. */
1085  inline void mark_as_modified() const
1086  {
1088  m_boundingBoxIsUpdated = false;
1090  }
1091 
1092  protected:
1093  /** The point coordinates */
1095 
1096  /** Cache of sin/cos values for the latest 2D scan geometries. */
1098 
1099  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
1100  * \sa getLargestDistanceFromOrigin
1101  */
1103 
1104  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
1105  * \sa getLargestDistanceFromOrigin
1106  */
1108 
1111  m_bb_max_z;
1112 
1113  /** This is a common version of CMetricMap::insertObservation() for point
1114  * maps (actually, CMetricMap::internal_insertObservation),
1115  * so derived classes don't need to worry implementing that method unless
1116  * something special is really necesary.
1117  * See mrpt::maps::CPointsMap for the enumeration of types of observations
1118  * which are accepted. */
1120  const mrpt::obs::CObservation* obs,
1121  const mrpt::poses::CPose3D* robotPose) override;
1122 
1123  /** Helper method for ::copyFrom() */
1124  void base_copyFrom(const CPointsMap& obj);
1125 
1126  /** @name PLY Import virtual methods to implement in base classes
1127  @{ */
1128  /** In a base class, reserve memory to prepare subsequent calls to
1129  * PLY_import_set_face */
1130  virtual void PLY_import_set_face_count(const size_t N) override
1131  {
1132  MRPT_UNUSED_PARAM(N);
1133  }
1134 
1135  /** In a base class, will be called after PLY_import_set_vertex_count() once
1136  * for each loaded point.
1137  * \param pt_color Will be nullptr if the loaded file does not provide
1138  * color info.
1139  */
1140  virtual void PLY_import_set_vertex(
1141  const size_t idx, const mrpt::math::TPoint3Df& pt,
1142  const mrpt::img::TColorf* pt_color = nullptr) override;
1143  /** @} */
1144 
1145  /** @name PLY Export virtual methods to implement in base classes
1146  @{ */
1147  size_t PLY_export_get_vertex_count() const override;
1148  size_t PLY_export_get_face_count() const override { return 0; }
1149  virtual void PLY_export_get_vertex(
1150  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
1151  mrpt::img::TColorf& pt_color) const override;
1152  /** @} */
1153 
1154  /** The minimum and maximum height for a certain laser scan to be inserted
1155  * into this map
1156  * \sa m_heightfilter_enabled */
1158 
1159  /** Whether or not (default=not) filter the input points by height
1160  * \sa m_heightfilter_z_min, m_heightfilter_z_max */
1162 
1163  // Friend methods:
1164  template <class Derived>
1166  template <class Derived>
1168 
1169 }; // End of class def.
1170 
1171 } // namespace maps
1172 
1173 namespace opengl
1174 {
1175 /** Specialization mrpt::opengl::PointCloudAdapter<mrpt::maps::CPointsMap>
1176  * \ingroup mrpt_adapters_grp*/
1177 template <>
1179  : public detail::PointCloudAdapterHelperNoRGB<mrpt::maps::CPointsMap, float>
1180 {
1181  private:
1183 
1184  public:
1185  /** The type of each point XYZ coordinates */
1186  using coords_t = float;
1187  /** Has any color RGB info? */
1188  static const int HAS_RGB = 0;
1189  /** Has native RGB info (as floats)? */
1190  static const int HAS_RGBf = 0;
1191  /** Has native RGB info (as uint8_t)? */
1192  static const int HAS_RGBu8 = 0;
1193 
1194  /** Constructor (accept a const ref for convenience) */
1196  : m_obj(*const_cast<mrpt::maps::CPointsMap*>(&obj))
1197  {
1198  }
1199  /** Get number of points */
1200  inline size_t size() const { return m_obj.size(); }
1201  /** Set number of points (to uninitialized values) */
1202  inline void resize(const size_t N) { m_obj.resize(N); }
1203  /** Get XYZ coordinates of i'th point */
1204  template <typename T>
1205  inline void getPointXYZ(const size_t idx, T& x, T& y, T& z) const
1206  {
1207  m_obj.getPointFast(idx, x, y, z);
1208  }
1209  /** Set XYZ coordinates of i'th point */
1210  inline void setPointXYZ(
1211  const size_t idx, const coords_t x, const coords_t y, const coords_t z)
1212  {
1213  m_obj.setPointFast(idx, x, y, z);
1214  }
1215 }; // end of PointCloudAdapter<mrpt::maps::CPointsMap>
1216 } // namespace opengl
1217 } // namespace mrpt
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
virtual void copyFrom(const CPointsMap &obj)=0
Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source ...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CPointsMap.cpp:752
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - used in derived classes&#39; serialization.
Definition: CPointsMap.cpp:698
TLikelihoodOptions()
Initilization of default parameters.
Definition: CPointsMap.cpp:668
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:673
void setFromPCLPointCloud(const POINTCLOUD &cloud)
Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information...
Definition: CPointsMap.h:1010
void insertAnotherMap(const CPointsMap *otherMap, const mrpt::poses::CPose3D &otherPose)
Insert the contents of another map into this one with some geometric transformation, without fusing close points.
Parameters for CMetricMap::compute3DMatchingRatio()
bool save2D_to_text_file(const std::string &file) const
Save to a text file.
Definition: CPointsMap.cpp:83
void setAllPointsTemplate(const VECTOR &X, const VECTOR &Y, const VECTOR &Z=VECTOR())
Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros).
Definition: CPointsMap.h:630
GLenum GLint GLuint mask
Definition: glext.h:4050
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:237
bool empty() const
STL-like method to check whether the map is empty:
Definition: CPointsMap.h:854
mrpt::img::TColormap colormap
Colormap for points (index is "z" coordinates)
Definition: CPointsMap.h:314
virtual void addFrom(const CPointsMap &anotherMap)
Adds all the points from anotherMap to this map, without fusing.
TColormap
Different colormaps for use in mrpt::img::colormap()
Definition: color_maps.h:31
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
Definition: CPointsMap.h:450
#define MRPT_START
Definition: exceptions.h:262
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr)=0
Transform the range scan into a set of cartessian coordinated points.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CPointsMap.cpp:790
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose) override
This is a common version of CMetricMap::insertObservation() for point maps (actually, CMetricMap::internal_insertObservation), so derived classes don&#39;t need to worry implementing that method unless something special is really necesary.
GLdouble GLdouble z
Definition: glext.h:3872
bool m_heightfilter_enabled
Whether or not (default=not) filter the input points by height.
Definition: CPointsMap.h:1161
void operator+=(const CPointsMap &anotherMap)
This operator is synonymous with addFrom.
Definition: CPointsMap.h:332
size_t PLY_export_get_face_count() const override
In a base class, return the number of faces.
Definition: CPointsMap.h:1148
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:574
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
double x
X,Y coordinates.
const mrpt::obs::CObservation2DRangeScan & rangeScan
Definition: CPointsMap.h:85
const mrpt::obs::CObservation3DRangeScan & rangeScan
Definition: CPointsMap.h:103
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y)
Set all the points at once from vectors with X and Y coordinates (Z=0).
Definition: CPointsMap.h:662
virtual void setPointAllFieldsFast(const size_t index, const std::vector< float > &point_data)=0
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
void resize(const size_t N)
Set number of points (to uninitialized values)
Definition: CPointsMap.h:1202
const double G
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:803
virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() override
Definition: CPointsMap.h:870
void base_copyFrom(const CPointsMap &obj)
Helper method for ::copyFrom()
const mrpt::aligned_std_vector< float > & getPointsBufferRef_y() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:522
virtual void setPointFast(size_t index, float x, float y, float z)=0
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
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 loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=nullptr)
Like loadFromRangeScan() for Velodyne 3D scans.
void getAllPoints(std::vector< mrpt::math::TPoint2D > &ps, size_t decimation=1) const
Definition: CPointsMap.h:587
size_t kdtree_get_point_count() const
Must return the number of data points.
Definition: CPointsMap.h:1027
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:604
void getAllPoints(CONTAINER &ps, size_t decimation=1) const
Gets all points as a STL-like container.
Definition: CPointsMap.h:564
virtual void getPointAllFieldsFast(const size_t index, std::vector< float > &point_data) const =0
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
virtual unsigned int getPointWeight(size_t index) const
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
Definition: CPointsMap.h:504
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
TInsertionOptions()
Initilization of default parameters.
Definition: CPointsMap.cpp:622
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CPointsMap.cpp:765
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
float kdtree_distance(const float *p1, const size_t idx_p2, size_t size) const
Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored ...
Definition: CPointsMap.h:1043
virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::img::TColorf *pt_color=nullptr) override
In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D &p0) const
Definition: CPointsMap.h:194
size_t PLY_export_get_vertex_count() const override
In a base class, return the number of vertices.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CPointsMap.cpp:743
bool save3D_to_text_file(const std::string &file) const
Save to a text file.
Definition: CPointsMap.cpp:100
virtual void getPoint(size_t index, float &x, float &y, float &z, float &R, float &G, float &B) const
Access to a given point from map, and its colors, if the map defines them (othersise, R=G=B=1.0).
Definition: CPointsMap.h:439
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
Rendering options, used in getAs3DObject()
Definition: CPointsMap.h:297
float maxDistForInterpolatePoints
The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)...
Definition: CPointsMap.h:244
virtual void PLY_import_set_face_count(const size_t N) override
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face.
Definition: CPointsMap.h:1130
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
bool kdtree_get_bbox(BBOX &bb) const
Definition: CPointsMap.h:1068
unsigned long getPoint(size_t index, mrpt::math::TPoint2D &p) const
Definition: CPointsMap.h:423
GLuint color
Definition: glext.h:8300
A generic adaptor class for providing Nearest Neighbor (NN) lookup via the nanoflann library...
Definition: KDTreeCapable.h:79
void setPointAllFields(const size_t index, const std::vector< float > &point_data)
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
Definition: CPointsMap.h:685
bool isFilterByHeightEnabled() const
Return whether filter-by-height is enabled.
Definition: CPointsMap.h:938
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:205
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
A numeric matrix of compile-time fixed size.
bool insertInvalidPoints
Points with x,y,z coordinates set to zero will also be inserted.
Definition: CPointsMap.h:246
TLaserRange3DInsertContext(const mrpt::obs::CObservation3DRangeScan &_rangeScan)
Definition: CPointsMap.h:96
This class allows loading and storing values and vectors of different types from a configuration text...
float kdtree_get_pt(const size_t idx, int dim) const
Returns the dim&#39;th component of the idx&#39;th point in the class:
Definition: CPointsMap.h:1029
An adapter to different kinds of point cloud object.
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Lightweight 3D point (float version).
const mrpt::aligned_std_vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:525
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
double sigma_dist
Sigma squared (variance, in meters) of the exponential used to model the likelihood (default= 0...
Definition: CPointsMap.h:284
GLuint index
Definition: glext.h:4054
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class...
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y, const std::vector< float > &Z)
Set all the points at once from vectors with X,Y and Z coordinates.
Definition: CPointsMap.h:653
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Definition: CPointsMap.h:536
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:102
PointCloudAdapter(const mrpt::maps::CPointsMap &obj)
Constructor (accept a const ref for convenience)
Definition: CPointsMap.h:1195
void setPoint(size_t index, const mrpt::math::TPoint3D &p)
Definition: CPointsMap.h:474
void getHeightFilterLevels(double &_z_min, double &_z_max) const
Get the min/max Z levels for points to be actually inserted in the map.
Definition: CPointsMap.h:952
A list of TMatchingPair.
Definition: TMatchingPair.h:83
void setPoint(size_t index, float x, float y)
Definition: CPointsMap.h:479
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:1107
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - used in derived classes&#39; serialization.
Definition: CPointsMap.cpp:706
void extractCylinder(const mrpt::math::TPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap)
Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax valu...
float scan_x
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
Definition: CPointsMap.h:106
void fuseWith(CPointsMap *anotherMap, float minDistForFuse=0.02f, std::vector< bool > *notFusedPoints=nullptr)
Insert the contents of another map into this one, fusing the previous content with the new one...
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:636
GLubyte GLubyte b
Definition: glext.h:6279
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const override
If the map is a simple points map or it&#39;s a multi-metric map that contains EXACTLY one simple points ...
Definition: CPointsMap.h:865
TLaserRange2DInsertContext(const mrpt::obs::CObservation2DRangeScan &_rangeScan)
Definition: CPointsMap.h:78
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:87
virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints)=0
Auxiliary method called from within addFrom() automatically, to finish the copying of class-specific ...
virtual void setPointWeight(size_t index, unsigned long w)
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
Definition: CPointsMap.h:496
double x
X,Y,Z coordinates.
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
Definition: CPointsMap.h:1205
virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const
Save the point cloud as a PCL PCD file, in either ASCII or binary format (requires MRPT built against...
A virtual base class that implements the capability of exporting 3D point clouds and faces to a file ...
bool load2D_from_text_file(const std::string &file)
Load from a text file.
Definition: CPointsMap.h:355
void getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
Definition: CPointsMap.h:981
virtual bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CPointsMap.cpp:618
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
Definition: CPointsMap.cpp:853
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CPointsMap.cpp:782
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:1102
GLsizei const GLchar ** string
Definition: glext.h:4101
void compute3DDistanceToMesh(const mrpt::maps::CMetricMap *otherMap2, const mrpt::poses::CPose3D &otherMapPose, float maxDistForCorrespondence, mrpt::tfest::TMatchingPairList &correspondences, float &correspondencesRatio)
Computes the matchings between this and another 3D points map.
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:108
void clipOutOfRange(const mrpt::math::TPoint2D &point, float maxRange)
Delete points which are more far than "maxRange" away from the given "point".
Definition: CPointsMap.cpp:284
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)
Definition: CPointsMap.h:484
GLclampd zmax
Definition: glext.h:7918
void boundingBox(float &min_x, float &max_x, float &min_y, float &max_y, float &min_z, float &max_z) const
Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
Definition: CPointsMap.cpp:951
float coords_t
The type of each point XYZ coordinates.
Definition: CPointsMap.h:1186
void setHeightFilterLevels(const double _z_min, const double _z_max)
Set the min/max Z levels for points to be actually inserted in the map (only if enableFilterByHeight(...
Definition: CPointsMap.h:945
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
Definition: CPointsMap.h:225
void clipOutOfRangeInZ(float zMin, float zMax)
Delete points out of the given "z" axis range have been removed.
Definition: CPointsMap.cpp:266
mrpt::aligned_std_vector< float > m_z
Definition: CPointsMap.h:1094
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CPointsMap.cpp:724
mrpt::obs::CSinCosLookUpTableFor2DScans m_scans_sincos_cache
Cache of sin/cos values for the latest 2D scan geometries.
Definition: CPointsMap.h:1097
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:211
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
virtual void resize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
void mark_as_modified() const
Users normally don&#39;t need to call this.
Definition: CPointsMap.h:1085
bool load2Dor3D_from_text_file(const std::string &file, const bool is_3D)
2D or 3D generic implementation of load2D_from_text_file and load3D_from_text_file ...
Definition: CPointsMap.cpp:117
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded...
Definition: CPointsMap.h:222
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones...
Definition: CPointsMap.h:233
mrpt::aligned_std_vector< float > m_y
Definition: CPointsMap.h:1094
void getPointAllFields(const size_t index, std::vector< float > &point_data) const
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
Definition: CPointsMap.h:672
TLikelihoodOptions likelihoodOptions
Definition: CPointsMap.h:293
bool load3D_from_text_file(const std::string &file)
Load from a text file.
Definition: CPointsMap.h:364
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
Definition: CPointsMap.cpp:245
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
Definition: CPointsMap.h:462
TRenderOptions renderOptions
Definition: CPointsMap.h:316
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
Options used when evaluating "computeObservationLikelihood" in the derived classes.
Definition: CPointsMap.h:263
unsigned long getPoint(size_t index, mrpt::math::TPoint3D &p) const
Definition: CPointsMap.h:428
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
Definition: CPointsMap.cpp:832
void kdtree_mark_as_outdated() const
To be called by child classes when KD tree data changes.
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)
Definition: CPointsMap.h:615
void setPoint(size_t index, const mrpt::math::TPoint2D &p)
Definition: CPointsMap.h:469
#define MRPT_END
Definition: exceptions.h:266
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
GLuint in
Definition: glext.h:7274
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
Definition: CPointsMap.h:241
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CPointsMap.cpp:304
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
virtual bool loadPCDFile(const std::string &filename)
Load the point cloud from a PCL PCD file (requires MRPT built against PCL)
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:648
virtual ~CPointsMap()
Virtual destructor.
Definition: CPointsMap.cpp:77
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:94
GLenum GLint GLint y
Definition: glext.h:3538
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:257
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
Definition: CPointsMap.h:288
void enableFilterByHeight(bool enable=true)
Enable/disable the filter-by-height functionality.
Definition: CPointsMap.h:933
GLsizeiptr size
Definition: glext.h:3923
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Definition: CPointsMap.h:387
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
virtual void setSize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points, erasing all previous contents ...
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
GLenum GLint x
Definition: glext.h:3538
virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const override
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map...
Definition: CPointsMap.cpp:908
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10) ...
Definition: CPointsMap.h:291
Lightweight 3D point.
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)
Extracts the points in the map within the area defined by two corners.
void setPointXYZ(const size_t idx, const coords_t x, const coords_t y, const coords_t z)
Set XYZ coordinates of i&#39;th point.
Definition: CPointsMap.h:1210
const mrpt::aligned_std_vector< float > & getPointsBufferRef_x() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:519
Parameters for the determination of matchings between point clouds, etc.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:84
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::aligned_std_vector< float > m_x
The point coordinates.
Definition: CPointsMap.h:1094
Lightweight 2D point.
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const
Like getLargestDistanceFromOrigin() but returns in output_is_valid = false if the distance was not al...
Definition: CPointsMap.h:885
bool disableDeletion
If set to false (default=true) points in the same plane as the inserted scan and inside the free spac...
Definition: CPointsMap.h:229
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
void boundingBox(mrpt::math::TPoint3D &pMin, mrpt::math::TPoint3D &pMax) const
Definition: CPointsMap.h:900
virtual void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::img::TColorf &pt_color) const override
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point...
void insertPoint(const mrpt::math::TPoint3D &p)
Definition: CPointsMap.h:610
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
Definition: CPointsMap.h:458
A virtual base class that implements the capability of importing 3D point clouds and faces from a fil...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:76
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool&#39;s array as "true".
double m_heightfilter_z_min
The minimum and maximum height for a certain laser scan to be inserted into this map.
Definition: CPointsMap.h:1157
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:218
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:681



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019