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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cfb2df9d3 Fri Nov 15 06:57:14 2019 +0100 at vie nov 15 07:00:11 CET 2019