Main MRPT website > C++ reference
MRPT logo
CColouredPointsMap.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CColouredPointsMap_H
10 #define CColouredPointsMap_H
11 
18 
19 #include <mrpt/maps/link_pragmas.h>
20 
21 namespace mrpt
22 {
23  namespace slam
24  {
25  class CObservation3DRangeScan;
26 
27 
28  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CColouredPointsMap, CPointsMap,MAPS_IMPEXP )
29 
30  /** A map of 2D/3D points with individual colours (RGB).
31  * For different color schemes, see CColouredPointsMap::colorScheme
32  * Colors are defined in the range [0,1].
33  * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable
34  * \ingroup mrpt_maps_grp
35  */
36  class MAPS_IMPEXP CColouredPointsMap : public CPointsMap
37  {
38  // This must be added to any CSerializable derived class:
40 
41  public:
42  /** Destructor
43  */
44  virtual ~CColouredPointsMap();
45 
46  /** Default constructor
47  */
48  CColouredPointsMap();
49 
50  // --------------------------------------------
51  /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
52  @{ */
53 
54  /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
55  * This is useful for situations where it is approximately known the final size of the map. This method is more
56  * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
57  */
58  virtual void reserve(size_t newLength);
59 
60  /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
61  * and old contents are not changed.
62  * \sa reserve, setPoint, setPointFast, setSize
63  */
64  virtual void resize(size_t newLength);
65 
66  /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
67  * and leaving all points to default values.
68  * \sa reserve, setPoint, setPointFast, setSize
69  */
70  virtual void setSize(size_t newLength);
71 
72  /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
73  virtual void setPointFast(size_t index,float x, float y, float z)
74  {
75  this->x[index] = x;
76  this->y[index] = y;
77  this->z[index] = z;
78  }
79 
80  /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */
81  virtual void insertPointFast( float x, float y, float z = 0 );
82 
83  /** Virtual assignment operator, to be implemented in derived classes.
84  */
85  virtual void copyFrom(const CPointsMap &obj);
86 
87  /** Get all the data fields for one point as a vector: [X Y Z R G B]
88  * Unlike getPointAllFields(), this method does not check for index out of bounds
89  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
90  */
91  virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const {
92  point_data.resize(6);
93  point_data[0] = x[index];
94  point_data[1] = y[index];
95  point_data[2] = z[index];
96  point_data[3] = m_color_R[index];
97  point_data[4] = m_color_G[index];
98  point_data[5] = m_color_B[index];
99  }
101  /** Set all the data fields for one point as a vector: [X Y Z R G B]
102  * Unlike setPointAllFields(), this method does not check for index out of bounds
103  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
104  */
105  virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) {
106  ASSERTDEB_(point_data.size()==6)
107  x[index] = point_data[0];
108  y[index] = point_data[1];
109  z[index] = point_data[2];
110  m_color_R[index] = point_data[3];
111  m_color_G[index] = point_data[4];
112  m_color_B[index] = point_data[5];
113  }
114 
115  /** See CPointsMap::loadFromRangeScan() */
116  virtual void loadFromRangeScan(
117  const CObservation2DRangeScan &rangeScan,
118  const CPose3D *robotPose = NULL );
120  /** See CPointsMap::loadFromRangeScan() */
121  virtual void loadFromRangeScan(
122  const CObservation3DRangeScan &rangeScan,
123  const CPose3D *robotPose = NULL );
125  protected:
127  /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */
128  virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints);
129 
130 
131  // Friend methods:
132  template <class Derived> friend struct detail::loadFromRangeImpl;
133  template <class Derived> friend struct detail::pointmap_traits;
135  public:
138  /** @} */
139  // --------------------------------------------
140 
141  /** Save to a text file. In each line contains X Y Z (meters) R G B (range [0,1]) for each point in the map.
142  * Returns false if any error occured, true elsewere.
143  */
144  bool save3D_and_colour_to_text_file(const std::string &file) const;
146  /** Changes a given point from map. First index is 0.
147  * \exception Throws std::exception on index out of bound.
148  */
149  virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B);
151  // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "setPoint()"
152  /// \overload
153  inline void setPoint(size_t index,float x, float y, float z) {
154  ASSERT_BELOW_(index,this->size())
155  setPointFast(index,x,y,z);
156  mark_as_modified();
157  }
158  /// \overload
159  inline void setPoint(size_t index,CPoint2D &p) { setPoint(index,p.x(),p.y(),0); }
160  /// \overload
161  inline void setPoint(size_t index,CPoint3D &p) { setPoint(index,p.x(),p.y(),p.z()); }
162  /// \overload
163  inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
164 
166  /** Adds a new point given its coordinates and color (colors range is [0,1]) */
167  virtual void insertPoint( float x, float y, float z, float R, float G, float B );
168  // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "insertPoint()"
169  /// \overload of \a insertPoint()
170  inline void insertPoint( const CPoint3D &p ) { insertPoint(p.x(),p.y(),p.z()); }
171  /// \overload
172  inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
173  /// \overload
174  inline void insertPoint( float x, float y, float z) { insertPointFast(x,y,z); mark_as_modified(); }
176  /** Changes just the color of a given point from the map. First index is 0.
177  * \exception Throws std::exception on index out of bound.
178  */
179  void setPointColor(size_t index,float R, float G, float B);
180 
181  /** Like \c setPointColor but without checking for out-of-index erors */
182  inline void setPointColor_fast(size_t index,float R, float G, float B)
183  {
184  this->m_color_R[index]=R;
185  this->m_color_G[index]=G;
186  this->m_color_B[index]=B;
187  }
188 
189  /** Retrieves a point and its color (colors range is [0,1])
190  */
191  virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const;
192 
193  /** Retrieves a point */
194  unsigned long getPoint( size_t index, float &x, float &y, float &z) const;
195 
196  /** Retrieves a point color (colors range is [0,1]) */
197  void getPointColor( size_t index, float &R, float &G, float &B ) const;
198 
199  /** Like \c getPointColor but without checking for out-of-index erors */
200  inline void getPointColor_fast( size_t index, float &R, float &G, float &B ) const
201  {
202  R = m_color_R[index];
203  G = m_color_G[index];
204  B = m_color_B[index];
205  }
207  /** Returns true if the point map has a color field for each point */
208  virtual bool hasColorPoints() const { return true; }
209 
210  /** Override of the default 3D scene builder to account for the individual points' color.
211  * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
212  */
213  virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
214 
215  /** Colour a set of points from a CObservationImage and the global pose of the robot
216  */
217  bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose );
219  /** The choices for coloring schemes:
220  * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max.
221  * - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available.
222  * \sa TColourOptions
223  */
224  enum TColouringMethod
225  {
226  cmFromHeightRelativeToSensor = 0,
227  cmFromHeightRelativeToSensorJet = 0,
228  cmFromHeightRelativeToSensorGray = 1,
229  cmFromIntensityImage = 2
230  };
232  /** The definition of parameters for generating colors from laser scans */
234  {
235  /** Initilization of default parameters */
236  TColourOptions( );
237  virtual ~TColourOptions() {}
238  /** See utils::CLoadableOptions
239  */
240  void loadFromConfigFile(
241  const mrpt::utils::CConfigFileBase &source,
242  const std::string &section);
243 
244  /** See utils::CLoadableOptions
245  */
246  void dumpToTextStream(CStream &out) const;
247 
249  float z_min,z_max;
250  float d_max;
251  };
253  TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map.
255  void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value
256 
257  /** @name PCL library support
258  @{ */
259 
260  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format \return false on any error */
261  virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const;
262 
263  /** Loads a PCL point cloud (WITH RGB information) into this MRPT class (for clouds without RGB data, see CPointsMap::setFromPCLPointCloud() ).
264  * Usage example:
265  * \code
266  * pcl::PointCloud<pcl::PointXYZRGB> cloud;
267  * mrpt::slam::CColouredPointsMap pc;
268  *
269  * pc.setFromPCLPointCloudRGB(cloud);
270  * \endcode
271  * \sa CPointsMap::setFromPCLPointCloud()
272  */
273  template <class POINTCLOUD>
274  void setFromPCLPointCloudRGB(const POINTCLOUD &cloud)
275  {
276  const size_t N = cloud.points.size();
277  clear();
278  reserve(N);
279  const float f = 1.0f/255.0f;
280  for (size_t i=0;i<N;++i)
281  this->insertPoint(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z,cloud.points[i].r*f,cloud.points[i].g*f,cloud.points[i].b*f);
282  }
284  /** @} */
287  protected:
288  /** The color data */
289  std::vector<float> m_color_R,m_color_G,m_color_B;
291  /** Minimum distance from where the points have been seen */
292  //std::vector<float> m_min_dist;
294  /** Clear the map, erasing all the points.
295  */
296  virtual void internal_clear();
297 
298  /** @name Redefinition of PLY Import virtual methods from CPointsMap
299  @{ */
300  /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
301  * \param pt_color Will be NULL if the loaded file does not provide color info.
302  */
303  virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL);
305  /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
306  virtual void PLY_import_set_vertex_count(const size_t N);
307  /** @} */
308 
309  /** @name Redefinition of PLY Export virtual methods from CPointsMap
310  @{ */
311  /** In a base class, will be called after PLY_export_get_vertex_count() once for each exported point.
312  * \param pt_color Will be NULL if the loaded file does not provide color info.
313  */
314  virtual void PLY_export_get_vertex(
315  const size_t idx,
317  bool &pt_has_color,
318  mrpt::utils::TColorf &pt_color) const;
319  /** @} */
320 
321  }; // End of class def.
322 
323  } // End of namespace
324 
326  namespace utils
327  {
328  /** Specialization mrpt::utils::PointCloudAdapter<mrpt::slam::CColouredPointsMap> \ingroup mrpt_adapters_grp */
329  template <>
331  {
332  private:
334  public:
335  typedef float coords_t; //!< The type of each point XYZ coordinates
336  static const int HAS_RGB = 1; //!< Has any color RGB info?
337  static const int HAS_RGBf = 1; //!< Has native RGB info (as floats)?
338  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
339 
340  /** Constructor (accept a const ref for convenience) */
341  inline PointCloudAdapter(const mrpt::slam::CColouredPointsMap &obj) : m_obj(*const_cast<mrpt::slam::CColouredPointsMap*>(&obj)) { }
342  /** Get number of points */
343  inline size_t size() const { return m_obj.size(); }
344  /** Set number of points (to uninitialized values) */
345  inline void resize(const size_t N) { m_obj.resize(N); }
346 
347  /** Get XYZ coordinates of i'th point */
348  template <typename T>
349  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
350  m_obj.getPointFast(idx,x,y,z);
351  }
352  /** Set XYZ coordinates of i'th point */
353  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
354  m_obj.setPointFast(idx,x,y,z);
355  }
357  /** Get XYZ_RGBf coordinates of i'th point */
358  template <typename T>
359  inline void getPointXYZ_RGBf(const size_t idx, T &x,T &y, T &z, float &r,float &g,float &b) const {
360  m_obj.getPoint(idx,x,y,z,r,g,b);
361  }
362  /** Set XYZ_RGBf coordinates of i'th point */
363  inline 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) {
364  m_obj.setPoint(idx,x,y,z,r,g,b);
365  }
366 
367  /** Get XYZ_RGBu8 coordinates of i'th point */
368  template <typename T>
369  inline void getPointXYZ_RGBu8(const size_t idx, T &x,T &y, T &z, uint8_t &r,uint8_t &g,uint8_t &b) const {
370  float Rf,Gf,Bf;
371  m_obj.getPoint(idx,x,y,z,Rf,Gf,Bf);
372  r=Rf*255; g=Gf*255; b=Bf*255;
373  }
374  /** Set XYZ_RGBu8 coordinates of i'th point */
375  inline 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) {
376  m_obj.setPoint(idx,x,y,z,r/255.f,g/255.f,b/255.f);
377  }
378 
379  /** Get RGBf color of i'th point */
380  inline void getPointRGBf(const size_t idx, float &r,float &g,float &b) const { m_obj.getPointColor_fast(idx,r,g,b); }
381  /** Set XYZ_RGBf coordinates of i'th point */
382  inline void setPointRGBf(const size_t idx, const float r,const float g,const float b) { m_obj.setPointColor_fast(idx,r,g,b); }
383 
384  /** Get RGBu8 color of i'th point */
385  inline void getPointRGBu8(const size_t idx, uint8_t &r,uint8_t &g,uint8_t &b) const {
386  float R,G,B;
387  m_obj.getPointColor_fast(idx,R,G,B);
388  r=R*255; g=G*255; b=B*255;
389  }
390  /** Set RGBu8 coordinates of i'th point */
391  inline void setPointRGBu8(const size_t idx,const uint8_t r,const uint8_t g,const uint8_t b) {
392  m_obj.setPointColor_fast(idx,r/255.f,g/255.f,b/255.f);
393  }
394 
395  }; // end of PointCloudAdapter<mrpt::slam::CColouredPointsMap>
396 
397  }
398 
399 } // End of namespace
400 
401 #endif
void setPointRGBf(const size_t idx, const float r, const float g, const float b)
Set XYZ_RGBf coordinates of i&#39;th point.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:116
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:333
void setFromPCLPointCloudRGB(const POINTCLOUD &cloud)
Loads a PCL point cloud (WITH RGB information) into this MRPT class (for clouds without RGB data...
void insertPoint(const mrpt::math::TPoint3D &p)
The definition of parameters for generating colors from laser scans.
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.
virtual void getPointAllFieldsFast(const size_t index, std::vector< float > &point_data) const
Get all the data fields for one point as a vector: [X Y Z R G B] Unlike getPointAllFields(), this method does not check for index out of bounds.
float coords_t
The type of each point XYZ coordinates.
void getPointColor_fast(size_t index, float &R, float &G, float &B) const
Like getPointColor but without checking for out-of-index erors.
void getPointRGBu8(const size_t idx, uint8_t &r, uint8_t &g, uint8_t &b) const
Get RGBu8 color of i&#39;th point.
void setPoint(size_t index, CPoint3D &p)
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.
PointCloudAdapter(const mrpt::slam::CColouredPointsMap &obj)
Constructor (accept a const ref for convenience)
void setPoint(size_t index, CPoint2D &p)
A map of 2D/3D points with individual colours (RGB).
size_t size(const MATRIXLIKE &m, int dim)
virtual 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...
#define ASSERT_BELOW_(__A, __B)
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
void resize(const size_t N)
Set number of points (to uninitialized values)
TColouringMethod
The choices for coloring schemes:
double z
Z coordinate.
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 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.
This class allows loading and storing values and vectors of different types from a configuration text...
void insertPoint(float x, float y, float z)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Lightweight 3D point (float version).
virtual void resize(size_t newLength)
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
double x
X coordinate.
A class used to store a 2D point.
Definition: CPoint2D.h:35
A class used to store a 3D point.
Definition: CPoint3D.h:33
class BASE_IMPEXP CStream
void setPointColor_fast(size_t index, float R, float G, float B)
Like setPointColor but without checking for out-of-index erors.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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:372
A RGB color - floats in the range [0,1].
Definition: TColor.h:51
virtual void getPoint(size_t index, float &x, float &y, float &z, float &R, float &G, float &B) const
Retrieves a point and its color (colors range is [0,1])
virtual void setPoint(size_t index, float x, float y, float z, float R, float G, float B)
Changes a given point from map.
double y
Y coordinate.
An adapter to different kinds of point cloud object.
Definition: adapters.h:38
Lightweight 3D point.
void setPoint(size_t index, float x, float y)
#define ASSERTDEB_(f)
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo