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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020