MRPT  1.9.9
CPointsMapXYZI.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/maps/CPointsMap.h>
12 #include <mrpt/math/CMatrixF.h>
14 #include <mrpt/obs/obs_frwds.h>
17 
18 namespace mrpt
19 {
20 namespace maps
21 {
22 /** A map of 3D points with reflectance/intensity (float).
23  * \sa mrpt::maps::CPointsMap, mrpt::maps::CMetricMap
24  * \ingroup mrpt_maps_grp
25  */
26 class CPointsMapXYZI : public CPointsMap
27 {
29 
30  public:
31  CPointsMapXYZI() = default;
32 
36  {
37  impl_copyFrom(o);
38  return *this;
39  }
41  {
42  impl_copyFrom(o);
43  return *this;
44  }
45 
46  /** @name Pure virtual interfaces to be implemented by any class derived
47  from CPointsMap
48  @{ */
49 
50  void reserve(size_t newLength) override; // See base class docs
51  void resize(size_t newLength) override; // See base class docs
52  void setSize(size_t newLength) override; // See base class docs
53 
54  /** The virtual method for \a insertPoint() *without* calling
55  * mark_as_modified() */
56  void insertPointFast(float x, float y, float z = 0) override;
57 
58  /** Get all the data fields for one point as a vector: [X Y Z I]
59  * Unlike getPointAllFields(), this method does not check for index out of
60  * bounds
61  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
62  */
64  const size_t index, std::vector<float>& point_data) const override
65  {
66  point_data.resize(4);
67  point_data[0] = m_x[index];
68  point_data[1] = m_y[index];
69  point_data[2] = m_z[index];
70  point_data[3] = m_intensity[index];
71  }
72 
73  /** Set all the data fields for one point as a vector: [X Y Z I]
74  * Unlike setPointAllFields(), this method does not check for index out of
75  * bounds
76  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
77  */
79  const size_t index, const std::vector<float>& point_data) override
80  {
81  ASSERT_(point_data.size() == 4);
82  m_x[index] = point_data[0];
83  m_y[index] = point_data[1];
84  m_z[index] = point_data[2];
85  m_intensity[index] = point_data[3];
86  }
87 
88  /** Loads from a Kitti dataset Velodyne scan binary file.
89  * The file can be gz compressed (only enabled if the filename ends in ".gz"
90  * to prevent spurious false autodetection of gzip files).
91  * \return true on success */
92  bool loadFromKittiVelodyneFile(const std::string& filename);
93 
94  bool saveToKittiVelodyneFile(const std::string& filename) const;
95 
96  /** See CPointsMap::loadFromRangeScan() */
97  void loadFromRangeScan(
98  const mrpt::obs::CObservation2DRangeScan& rangeScan,
99  const mrpt::poses::CPose3D* robotPose = nullptr) override;
100  /** See CPointsMap::loadFromRangeScan() */
101  void loadFromRangeScan(
102  const mrpt::obs::CObservation3DRangeScan& rangeScan,
103  const mrpt::poses::CPose3D* robotPose = nullptr) override;
104 
105  protected:
106  // See base class
107  void impl_copyFrom(const CPointsMap& obj) override;
108  // See base class
110  const CPointsMap& anotherMap, const size_t nPreviousPoints) override;
111 
112  // Friend methods:
113  template <class Derived>
115  template <class Derived>
116  friend struct detail::pointmap_traits;
117 
118  public:
119  /** @} */
120  // --------------------------------------------
121 
122  /** Save to a text file. In each line contains X Y Z (meters) I (intensity)
123  * Returns false if any error occured, true elsewere.
124  */
125  bool saveXYZI_to_text_file(const std::string& file) const;
126 
127  /** Loads from a text file, each line having "X Y Z I", I in [0,1].
128  * Returns false if any error occured, true elsewere. */
129  bool loadXYZI_from_text_file(const std::string& file);
130 
131  /** Changes a given point from map. First index is 0.
132  * \exception Throws std::exception on index out of bound.
133  */
134  void setPointRGB(
135  size_t index, float x, float y, float z, float R_intensity,
136  float G_ignored, float B_ignored) override;
137 
138  /** Adds a new point given its coordinates and color (colors range is [0,1])
139  */
140  void insertPointRGB(
141  float x, float y, float z, float R_intensity, float G_ignored,
142  float B_ignored) override;
143 
144  /** Changes the intensity of a given point from the map. First index is 0.
145  * \exception Throws std::exception on index out of bound.
146  */
147  void setPointIntensity(size_t index, float intensity);
148 
149  /** Like \c setPointColor but without checking for out-of-index erors */
150  inline void setPointColor_fast(size_t index, float R, float G, float B)
151  {
152  m_intensity[index] = R;
153  }
154 
155  /** Retrieves a point and its color (colors range is [0,1])
156  */
157  void getPointRGB(
158  size_t index, float& x, float& y, float& z, float& R_intensity,
159  float& G_intensity, float& B_intensity) const override;
160 
161  /** Retrieves a point intensity (range [0,1]) */
162  float getPointIntensity(size_t index) const;
163 
164  /** Like \c getPointColor but without checking for out-of-index erors */
165  inline float getPointIntensity_fast(size_t index) const
166  {
167  return m_intensity[index];
168  }
169 
170  /** Returns true if the point map has a color field for each point */
171  bool hasColorPoints() const override { return true; }
172 
173  /** Override of the default 3D scene builder to account for the individual
174  * points' color.
175  */
176  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
177 
178  /** @name PCL library support
179  @{ */
180 
181  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
182  * \note This method requires user code to include PCL before MRPT headers.
183  * \return false on any error */
184 #if defined(PCL_LINEAR_VERSION)
185  inline bool savePCDFile(
186  const std::string& filename, bool save_as_binary) const
187  {
188  pcl::PointCloud<pcl::PointXYZI> cloud;
189 
190  const size_t nThis = this->size();
191 
192  // Fill in the cloud data
193  cloud.width = nThis;
194  cloud.height = 1;
195  cloud.is_dense = false;
196  cloud.points.resize(cloud.width * cloud.height);
197 
198  for (size_t i = 0; i < nThis; ++i)
199  {
200  cloud.points[i].x = m_x[i];
201  cloud.points[i].y = m_y[i];
202  cloud.points[i].z = m_z[i];
203  cloud.points[i].intensity = this->m_intensity[i];
204  }
205 
206  return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
207  }
208 #endif
209 
210  /** Loads a PCL point cloud (WITH XYZI information) into this MRPT class.
211  * Usage example:
212  * \code
213  * pcl::PointCloud<pcl::PointXYZI> cloud;
214  * mrpt::maps::CPointsMapXYZI pc;
215  *
216  * pc.setFromPCLPointCloudXYZI(cloud);
217  * \endcode
218  * \sa CPointsMap::setFromPCLPointCloud()
219  */
220  template <class POINTCLOUD>
221  void setFromPCLPointCloudXYZI(const POINTCLOUD& cloud)
222  {
223  const size_t N = cloud.points.size();
224  clear();
225  reserve(N);
226  for (size_t i = 0; i < N; ++i)
227  this->insertPoint(
228  cloud.points[i].x, cloud.points[i].y, cloud.points[i].z,
229  cloud.points[i].intensity);
230  }
231 
232  /** Like CPointsMap::getPCLPointCloud() but for PointCloud<PointXYZI> */
233  template <class POINTCLOUD>
234  void getPCLPointCloudXYZI(POINTCLOUD& cloud) const
235  {
236  const size_t nThis = this->size();
237  this->getPCLPointCloud(cloud); // 1st: xyz data
238  // 2nd: I data
239  for (size_t i = 0; i < nThis; ++i)
240  cloud.points[i].intensity = m_intensity[i];
241  }
242  /** @} */
243 
244  protected:
245  /** The intensity/reflectance data */
247 
248  /** Clear the map, erasing all the points */
249  void internal_clear() override;
250 
251  /** @name Redefinition of PLY Import virtual methods from CPointsMap
252  @{ */
254  const size_t idx, const mrpt::math::TPoint3Df& pt,
255  const mrpt::img::TColorf* pt_color = nullptr) override;
256 
257  void PLY_import_set_vertex_count(const size_t N) override;
258  /** @} */
259 
260  /** @name Redefinition of PLY Export virtual methods from CPointsMap
261  @{ */
263  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
264  mrpt::img::TColorf& pt_color) const override;
265  /** @} */
266 
268  mrpt::maps::CPointsMap::TInsertionOptions insertionOpts;
269  mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts;
271 
272 }; // End of class def.
273 
274 } // namespace maps
275 
276 #include <mrpt/opengl/pointcloud_adapters.h>
277 namespace opengl
278 {
279 /** Specialization
280  * mrpt::opengl::PointCloudAdapter<mrpt::maps::CPointsMapXYZI> \ingroup
281  * mrpt_adapters_grp */
282 template <>
284 {
285  private:
287 
288  public:
289  /** The type of each point XYZ coordinates */
290  using coords_t = float;
291  /** Has any color RGB info? */
292  static constexpr bool HAS_RGB = true;
293  /** Has native RGB info (as floats)? */
294  static constexpr bool HAS_RGBf = true;
295  /** Has native RGB info (as uint8_t)? */
296  static constexpr bool HAS_RGBu8 = false;
297 
298  /** Constructor (accept a const ref for convenience) */
300  : m_obj(*const_cast<mrpt::maps::CPointsMapXYZI*>(&obj))
301  {
302  }
303  /** Get number of points */
304  inline size_t size() const { return m_obj.size(); }
305  /** Set number of points (to uninitialized values) */
306  inline void resize(const size_t N) { m_obj.resize(N); }
307  /** Does nothing as of now */
308  inline void setDimensions(const size_t& height, const size_t& width) {}
309  /** Get XYZ coordinates of i'th point */
310  template <typename T>
311  inline void getPointXYZ(const size_t idx, T& x, T& y, T& z) const
312  {
313  m_obj.getPointFast(idx, x, y, z);
314  }
315  /** Set XYZ coordinates of i'th point */
316  inline void setPointXYZ(
317  const size_t idx, const coords_t x, const coords_t y, const coords_t z)
318  {
319  m_obj.setPointFast(idx, x, y, z);
320  }
321 
322  /** Get XYZ_RGBf coordinates of i'th point */
323  template <typename T>
324  inline void getPointXYZ_RGBf(
325  const size_t idx, T& x, T& y, T& z, float& r, float& g, float& b) const
326  {
327  m_obj.getPointRGB(idx, x, y, z, r, g, b);
328  }
329  /** Set XYZ_RGBf coordinates of i'th point */
330  inline void setPointXYZ_RGBf(
331  const size_t idx, const coords_t x, const coords_t y, const coords_t z,
332  const float r, const float g, const float b)
333  {
334  m_obj.setPointRGB(idx, x, y, z, r, g, b);
335  }
336 
337  /** Get XYZ_RGBu8 coordinates of i'th point */
338  template <typename T>
339  inline void getPointXYZ_RGBu8(
340  const size_t idx, T& x, T& y, T& z, uint8_t& r, uint8_t& g,
341  uint8_t& b) const
342  {
343  float I, Gignrd, Bignrd;
344  m_obj.getPoint(idx, x, y, z, I, Gignrd, Bignrd);
345  r = g = b = I * 255;
346  }
347  /** Set XYZ_RGBu8 coordinates of i'th point */
348  inline void setPointXYZ_RGBu8(
349  const size_t idx, const coords_t x, const coords_t y, const coords_t z,
350  const uint8_t r, const uint8_t g, const uint8_t b)
351  {
352  m_obj.setPointRGB(idx, x, y, z, r / 255.f, g / 255.f, b / 255.f);
353  }
354 
355  /** Get RGBf color of i'th point */
356  inline void getPointRGBf(
357  const size_t idx, float& r, float& g, float& b) const
358  {
359  r = g = b = m_obj.getPointIntensity_fast(idx);
360  }
361  /** Set XYZ_RGBf coordinates of i'th point */
362  inline void setPointRGBf(
363  const size_t idx, const float r, const float g, const float b)
364  {
365  m_obj.setPointColor_fast(idx, r, g, b);
366  }
367 
368  /** Get RGBu8 color of i'th point */
369  inline void getPointRGBu8(
370  const size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
371  {
372  float i = m_obj.getPointIntensity_fast(idx);
373  r = g = b = i * 255;
374  }
375  /** Set RGBu8 coordinates of i'th point */
376  inline void setPointRGBu8(
377  const size_t idx, const uint8_t r, const uint8_t g, const uint8_t b)
378  {
379  m_obj.setPointColor_fast(idx, r / 255.f, g / 255.f, b / 255.f);
380  }
381 
382 }; // end of PointCloudAdapter<mrpt::maps::CPointsMapXYZI>
383 } // namespace opengl
384 } // namespace mrpt
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
void setPointRGB(size_t index, float x, float y, float z, float R_intensity, float G_ignored, float B_ignored) override
Changes a given point from map.
PointCloudAdapter(const mrpt::maps::CPointsMapXYZI &obj)
Constructor (accept a const ref for convenience)
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:194
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
Definition: CPointsMap.h:481
void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::img::TColorf &pt_color) const override
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point...
void setSize(size_t newLength) override
Resizes all point buffers so they can hold the given number of points, erasing all previous contents ...
GLdouble GLdouble z
Definition: glext.h:3879
CPointsMap & operator=(const CPointsMap &o)
Definition: CPointsMap.h:120
bool loadXYZI_from_text_file(const std::string &file)
Loads from a text file, each line having "X Y Z I", I in [0,1].
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...
CPointsMapXYZI(const CPointsMap &o)
bool hasColorPoints() const override
Returns true if the point map has a color field for each point.
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 I] Unlike setPointAllFields(), this method does not check for index out of bounds.
const double G
mrpt::aligned_std_vector< float > m_intensity
The intensity/reflectance data.
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.
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
void setFromPCLPointCloudXYZI(const POINTCLOUD &cloud)
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
void resize(const size_t N)
Set number of points (to uninitialized values)
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:644
void getPCLPointCloudXYZI(POINTCLOUD &cloud) const
Like CPointsMap::getPCLPointCloud() but for PointCloud<PointXYZI>
void setPointIntensity(size_t index, float intensity)
Changes the intensity of a given point from the map.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
void internal_clear() override
Clear the map, erasing all the points.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
GLenum GLsizei width
Definition: glext.h:3535
void setPointRGBf(const size_t idx, const float r, const float g, const float b)
Set XYZ_RGBf coordinates of i&#39;th point.
bool loadFromKittiVelodyneFile(const std::string &filename)
Loads from a Kitti dataset Velodyne scan binary file.
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 ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:221
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
void setPointXYZ_RGBu8(const size_t idx, const coords_t x, const coords_t y, const coords_t z, const uint8_t r, const uint8_t g, const uint8_t b)
Set XYZ_RGBu8 coordinates of i&#39;th point.
void impl_copyFrom(const CPointsMap &obj) override
Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source ...
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.
An adapter to different kinds of point cloud object.
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...
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
void setDimensions(const size_t &height, const size_t &width)
Does nothing as of now.
float getPointIntensity_fast(size_t index) const
Like getPointColor but without checking for out-of-index erors.
GLuint index
Definition: glext.h:4068
void getPointRGB(size_t index, float &x, float &y, float &z, float &R_intensity, float &G_intensity, float &B_intensity) const override
Retrieves a point and its color (colors range is [0,1])
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
void getPointXYZ_RGBu8(const size_t idx, T &x, T &y, T &z, uint8_t &r, uint8_t &g, uint8_t &b) const
Get XYZ_RGBu8 coordinates of i&#39;th point.
void getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
Definition: CPointsMap.h:1010
GLsizei const GLchar ** string
Definition: glext.h:4116
void setPointFast(size_t index, float x, float y, float z)
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
Definition: CPointsMap.h:161
void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
void getPointXYZ_RGBf(const size_t idx, T &x, T &y, T &z, float &r, float &g, float &b) const
Get XYZ_RGBf coordinates of i&#39;th point.
void setPointRGBu8(const size_t idx, const uint8_t r, const uint8_t g, const uint8_t b)
Set RGBu8 coordinates of i&#39;th point.
void insertPointRGB(float x, float y, float z, float R_intensity, float G_ignored, float B_ignored) override
Adds a new point given its coordinates and color (colors range is [0,1])
A map of 3D points with reflectance/intensity (float).
CPointsMapXYZI & operator=(const CPointsMapXYZI &o)
mrpt::aligned_std_vector< float > m_z
Definition: CPointsMap.h:1123
float getPointIntensity(size_t index) const
Retrieves a point intensity (range [0,1])
void setPointXYZ_RGBf(const size_t idx, const coords_t x, const coords_t y, const coords_t z, const float r, const float g, const float b)
Set XYZ_RGBf coordinates of i&#39;th point.
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...
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
mrpt::aligned_std_vector< float > m_y
Definition: CPointsMap.h:1123
bool saveToKittiVelodyneFile(const std::string &filename) const
const float R
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Override of the default 3D scene builder to account for the individual points&#39; color.
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:279
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
void getPointRGBf(const size_t idx, float &r, float &g, float &b) const
Get RGBf color of i&#39;th point.
float coords_t
The type of each point XYZ coordinates.
GLenum GLint GLint y
Definition: glext.h:3542
CPointsMapXYZI & operator=(const CPointsMap &o)
void getPointRGBu8(const size_t idx, uint8_t &r, uint8_t &g, uint8_t &b) const
Get RGBu8 color of i&#39;th point.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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 I] Unlike getPointAllFields(), this method does not check for index out of bounds.
GLenum GLint x
Definition: glext.h:3542
void insertPointFast(float x, float y, float z=0) override
The virtual method for insertPoint() without calling mark_as_modified()
CPointsMapXYZI(const CPointsMapXYZI &o)
GLenum GLsizei GLsizei height
Definition: glext.h:3558
mrpt::aligned_std_vector< float > m_x
The point coordinates.
Definition: CPointsMap.h:1123
#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:441
void setPointColor_fast(size_t index, float R, float G, float B)
Like setPointColor but without checking for out-of-index erors.
bool saveXYZI_to_text_file(const std::string &file) const
Save to a text file.
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019