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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019