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



Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mar mar 31 20:00:11 CEST 2020