MRPT  2.0.1
CWeightedPointsMap.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 
11 #include <mrpt/maps/CPointsMap.h>
12 #include <mrpt/math/CMatrixF.h>
16 
17 namespace mrpt
18 {
19 namespace maps
20 {
21 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser
22  * scans.
23  * This class stores the coordinates (x,y,z) and a "weight", or counter of
24  * how many times that point has been seen, used only if points fusion is
25  * enabled in the options structure.
26  * \sa CMetricMap, CPoint, mrpt::serialization::CSerializable, CSimplePointsMap
27  * \ingroup mrpt_maps_grp
28  */
30 {
32 
33  public:
34  /** Default constructor */
35  CWeightedPointsMap() = default;
38  {
39  impl_copyFrom(o);
40  }
42  {
43  impl_copyFrom(o);
44  return *this;
45  }
47  {
48  impl_copyFrom(o);
49  return *this;
50  }
51 
52  // --------------------------------------------
53  /** @name Pure virtual interfaces to be implemented by any class derived
54  from CPointsMap
55  @{ */
56  void reserve(size_t newLength) override; // See base class docs
57  void resize(size_t newLength) override; // See base class docs
58  void setSize(size_t newLength) override; // See base class docs
59 
60  /** The virtual method for \a insertPoint() *without* calling
61  * mark_as_modified() */
62  void insertPointFast(float x, float y, float z = 0) override;
63 
64  /** Get all the data fields for one point as a vector: [X Y Z WEIGHT]
65  * Unlike getPointAllFields(), this method does not check for index out of
66  * bounds
67  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
68  */
70  const size_t index, std::vector<float>& point_data) const override
71  {
72  point_data.resize(4);
73  point_data[0] = m_x[index];
74  point_data[1] = m_y[index];
75  point_data[2] = m_z[index];
76  point_data[3] = pointWeight[index];
77  }
78 
79  /** Set all the data fields for one point as a vector: [X Y Z WEIGHT]
80  * Unlike setPointAllFields(), this method does not check for index out of
81  * bounds
82  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
83  */
85  const size_t index, const std::vector<float>& point_data) override
86  {
87  ASSERTDEB_(point_data.size() == 4);
88  m_x[index] = point_data[0];
89  m_y[index] = point_data[1];
90  m_z[index] = point_data[2];
91  pointWeight[index] = point_data[3];
92  }
93 
94  /** See CPointsMap::loadFromRangeScan() */
95  void loadFromRangeScan(
96  const mrpt::obs::CObservation2DRangeScan& rangeScan,
97  const mrpt::poses::CPose3D* robotPose = nullptr) override;
98 
99  /** See CPointsMap::loadFromRangeScan() */
100  void loadFromRangeScan(
101  const mrpt::obs::CObservation3DRangeScan& rangeScan,
102  const mrpt::poses::CPose3D* robotPose = nullptr) override;
103 
104  protected:
105  void impl_copyFrom(const CPointsMap& obj) override;
107  const CPointsMap& anotherMap, const size_t nPreviousPoints) override;
108 
109  // Friend methods:
110  template <class Derived>
112  template <class Derived>
113  friend struct detail::pointmap_traits;
114 
115  public:
116  /** @} */
117  // --------------------------------------------
118 
119  /// Sets the point weight, which is ignored in all classes but those which
120  /// actually store that field (Note: No checks are done for out-of-bounds
121  /// index). \sa getPointWeight
122  void setPointWeight(size_t index, unsigned long w) override
123  {
124  pointWeight[index] = w;
125  }
126  /// Gets the point weight, which is ignored in all classes (defaults to 1)
127  /// but in those which actually store that field (Note: No checks are done
128  /// for out-of-bounds index). \sa setPointWeight
129  unsigned int getPointWeight(size_t index) const override
130  {
131  return pointWeight[index];
132  }
133 
134  protected:
135  /** The points weights */
137 
138  /** Clear the map, erasing all the points.
139  */
140  void internal_clear() override;
141 
142  protected:
143  /** @name PLY Import virtual methods to implement in base classes
144  @{ */
145  /** In a base class, reserve memory to prepare subsequent calls to
146  * PLY_import_set_vertex */
147  void PLY_import_set_vertex_count(const size_t N) override;
148  /** @} */
149 
151  /** Observations insertion options */
152  mrpt::maps::CPointsMap::TInsertionOptions insertionOpts;
153  /** Probabilistic observation likelihood options */
154  mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts;
156 }; // End of class def.
157 } // namespace maps
158 
159 namespace opengl
160 {
161 /** Specialization
162  * mrpt::opengl::PointCloudAdapter<mrpt::maps::CWeightedPointsMap> \ingroup
163  * mrpt_adapters_grp*/
164 template <>
166 {
167  private:
169 
170  public:
171  /** The type of each point XYZ coordinates */
172  using coords_t = float;
173  /** Has any color RGB info? */
174  static constexpr bool HAS_RGB = false;
175  /** Has native RGB info (as floats)? */
176  static constexpr bool HAS_RGBf = false;
177  /** Has native RGB info (as uint8_t)? */
178  static constexpr bool HAS_RGBu8 = false;
179 
180  /** Constructor (accept a const ref for convenience) */
182  : m_obj(*const_cast<mrpt::maps::CWeightedPointsMap*>(&obj))
183  {
184  }
185  /** Get number of points */
186  inline size_t size() const { return m_obj.size(); }
187  /** Set number of points (to uninitialized values) */
188  inline void resize(const size_t N) { m_obj.resize(N); }
189  /** Does nothing as of now */
190  inline void setDimensions(size_t height, size_t width) {}
191  /** Get XYZ coordinates of i'th point */
192  template <typename T>
193  inline void getPointXYZ(const size_t idx, T& x, T& y, T& z) const
194  {
195  m_obj.getPointFast(idx, x, y, z);
196  }
197  /** Set XYZ coordinates of i'th point */
198  inline void setPointXYZ(
199  const size_t idx, const coords_t x, const coords_t y, const coords_t z)
200  {
201  m_obj.setPointFast(idx, x, y, z);
202  }
203 }; // end of PointCloudAdapter<mrpt::maps::CPointsMap>
204 } // namespace opengl
205 } // namespace mrpt
void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints) override
Auxiliary method called from within addFrom() automatically, to finish the copying of class-specific ...
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
Definition: CPointsMap.h:480
void setPointWeight(size_t index, unsigned long w) override
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
void setDimensions(size_t height, size_t width)
Does nothing as of now.
CWeightedPointsMap(const CPointsMap &o)
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.
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
void PLY_import_set_vertex_count(const size_t N) override
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex.
void impl_copyFrom(const CPointsMap &obj) override
Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source ...
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
CWeightedPointsMap & operator=(const CWeightedPointsMap &o)
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
void insertPointFast(float x, float y, float z=0) override
The virtual method for insertPoint() without calling mark_as_modified()
PointCloudAdapter(const mrpt::maps::CWeightedPointsMap &obj)
Constructor (accept a const ref for convenience)
CWeightedPointsMap & operator=(const CPointsMap &o)
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:220
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
An adapter to different kinds of point cloud object.
unsigned int getPointWeight(size_t index) const override
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
void setPointAllFieldsFast(const size_t index, const std::vector< float > &point_data) override
Set all the data fields for one point as a vector: [X Y Z WEIGHT] Unlike setPointAllFields(), this method does not check for index out of bounds.
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 resize(size_t newLength) override
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
void getPointAllFieldsFast(const size_t index, std::vector< float > &point_data) const override
Get all the data fields for one point as a vector: [X Y Z WEIGHT] Unlike getPointAllFields(), this method does not check for index out of bounds.
void resize(const size_t N)
Set number of points (to uninitialized values)
mrpt::aligned_std_vector< float > m_z
Definition: CPointsMap.h:1116
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...
mrpt::aligned_std_vector< float > m_y
Definition: CPointsMap.h:1116
CWeightedPointsMap(const CWeightedPointsMap &o)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Options used when evaluating "computeObservationLikelihood" in the derived classes.
Definition: CPointsMap.h:278
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
mrpt::aligned_std_vector< uint32_t > pointWeight
The points weights.
CWeightedPointsMap()=default
Default constructor.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void internal_clear() override
Clear the map, erasing all the points.
float coords_t
The type of each point XYZ coordinates.
mrpt::aligned_std_vector< float > m_x
The point coordinates.
Definition: CPointsMap.h:1116
#define MAP_DEFINITION_END(_CLASS_NAME_)
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:440
void setSize(size_t newLength) override
Resizes all point buffers so they can hold the given number of points, erasing all previous contents ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020