Main MRPT website > C++ reference
MRPT logo
CColouredPointsMap.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef CColouredPointsMap_H
36 #define CColouredPointsMap_H
37 
44 
45 #include <mrpt/maps/link_pragmas.h>
46 
47 namespace mrpt
48 {
49  namespace slam
50  {
51  class CObservation3DRangeScan;
52 
53 
54  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CColouredPointsMap, CPointsMap,MAPS_IMPEXP )
55 
56  /** A map of 2D/3D points with individual colours (RGB).
57  * For different color schemes, see CColouredPointsMap::colorScheme
58  * Colors are defined in the range [0,1].
59  * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable
60  * \ingroup mrpt_maps_grp
61  */
62  class MAPS_IMPEXP CColouredPointsMap : public CPointsMap
63  {
64  // This must be added to any CSerializable derived class:
66 
67  public:
68  /** Destructor
69  */
70  virtual ~CColouredPointsMap();
71 
72  /** Default constructor
73  */
74  CColouredPointsMap();
75 
76  // --------------------------------------------
77  /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
78  @{ */
79 
80  /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
81  * This is useful for situations where it is approximately known the final size of the map. This method is more
82  * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
83  */
84  virtual void reserve(size_t newLength);
85 
86  /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
87  * and old contents are not changed.
88  * \sa reserve, setPoint, setPointFast, setSize
89  */
90  virtual void resize(size_t newLength);
91 
92  /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
93  * and leaving all points to default values.
94  * \sa reserve, setPoint, setPointFast, setSize
95  */
96  virtual void setSize(size_t newLength);
97 
98  /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
99  virtual void setPointFast(size_t index,float x, float y, float z)
100  {
101  this->x[index] = x;
102  this->y[index] = y;
103  this->z[index] = z;
104  }
106  /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */
107  virtual void insertPointFast( float x, float y, float z = 0 );
108 
109  /** Virtual assignment operator, to be implemented in derived classes.
110  */
111  virtual void copyFrom(const CPointsMap &obj);
113  /** Get all the data fields for one point as a vector: [X Y Z R G B]
114  * Unlike getPointAllFields(), this method does not check for index out of bounds
115  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
116  */
117  virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const {
118  point_data.resize(6);
119  point_data[0] = x[index];
120  point_data[1] = y[index];
121  point_data[2] = z[index];
122  point_data[3] = m_color_R[index];
123  point_data[4] = m_color_G[index];
124  point_data[5] = m_color_B[index];
125  }
127  /** Set all the data fields for one point as a vector: [X Y Z R G B]
128  * Unlike setPointAllFields(), this method does not check for index out of bounds
129  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
130  */
131  virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) {
132  ASSERTDEB_(point_data.size()==6)
133  x[index] = point_data[0];
134  y[index] = point_data[1];
135  z[index] = point_data[2];
136  m_color_R[index] = point_data[3];
137  m_color_G[index] = point_data[4];
138  m_color_B[index] = point_data[5];
139  }
140 
141  /** See CPointsMap::loadFromRangeScan() */
142  virtual void loadFromRangeScan(
143  const CObservation2DRangeScan &rangeScan,
144  const CPose3D *robotPose = NULL );
146  /** See CPointsMap::loadFromRangeScan() */
147  virtual void loadFromRangeScan(
148  const CObservation3DRangeScan &rangeScan,
149  const CPose3D *robotPose = NULL );
151  protected:
153  /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */
154  virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints);
155 
156 
157  // Friend methods:
158  template <class Derived> friend struct detail::loadFromRangeImpl;
159  template <class Derived> friend struct detail::pointmap_traits;
161  public:
164  /** @} */
165  // --------------------------------------------
166 
167  /** 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.
168  * Returns false if any error occured, true elsewere.
169  */
170  bool save3D_and_colour_to_text_file(const std::string &file) const;
172  /** Changes a given point from map. First index is 0.
173  * \exception Throws std::exception on index out of bound.
174  */
175  virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B);
177  // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "setPoint()"
178  /// \overload
179  inline void setPoint(size_t index,float x, float y, float z) {
180  ASSERT_BELOW_(index,this->size())
181  setPointFast(index,x,y,z);
182  mark_as_modified();
183  }
184  /// \overload
185  inline void setPoint(size_t index,CPoint2D &p) { setPoint(index,p.x(),p.y(),0); }
186  /// \overload
187  inline void setPoint(size_t index,CPoint3D &p) { setPoint(index,p.x(),p.y(),p.z()); }
188  /// \overload
189  inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
190 
192  /** Adds a new point given its coordinates and color (colors range is [0,1]) */
193  virtual void insertPoint( float x, float y, float z, float R, float G, float B );
194  // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "insertPoint()"
195  /// \overload of \a insertPoint()
196  inline void insertPoint( const CPoint3D &p ) { insertPoint(p.x(),p.y(),p.z()); }
197  /// \overload
198  inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
199  /// \overload
200  inline void insertPoint( float x, float y, float z) { insertPointFast(x,y,z); mark_as_modified(); }
201 
202  /** Changes just the color of a given point from the map. First index is 0.
203  * \exception Throws std::exception on index out of bound.
204  */
205  void setPointColor(size_t index,float R, float G, float B);
206 
207  /** Like \c setPointColor but without checking for out-of-index erors */
208  inline void setPointColor_fast(size_t index,float R, float G, float B)
209  {
210  this->m_color_R[index]=R;
211  this->m_color_G[index]=G;
212  this->m_color_B[index]=B;
213  }
214 
215  /** Retrieves a point and its color (colors range is [0,1])
216  */
217  virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const;
219  /** Retrieves a point */
220  unsigned long getPoint( size_t index, float &x, float &y, float &z) const;
221 
222  /** Retrieves a point color (colors range is [0,1]) */
223  void getPointColor( size_t index, float &R, float &G, float &B ) const;
225  /** Like \c getPointColor but without checking for out-of-index erors */
226  inline void getPointColor_fast( size_t index, float &R, float &G, float &B ) const
227  {
228  R = m_color_R[index];
229  G = m_color_G[index];
230  B = m_color_B[index];
231  }
233  /** Returns true if the point map has a color field for each point */
234  virtual bool hasColorPoints() const { return true; }
235 
236  /** Override of the default 3D scene builder to account for the individual points' color.
237  * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
238  */
239  virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
240 
241  /** Colour a set of points from a CObservationImage and the global pose of the robot
242  */
243  bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose );
244 
245  /** The choices for coloring schemes:
246  * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max.
247  * - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available.
248  * \sa TColourOptions
249  */
250  enum TColouringMethod
251  {
252  cmFromHeightRelativeToSensor = 0,
253  cmFromHeightRelativeToSensorJet = 0,
254  cmFromHeightRelativeToSensorGray = 1,
255  cmFromIntensityImage = 2
256  };
258  /** The definition of parameters for generating colors from laser scans */
260  {
261  /** Initilization of default parameters */
262  TColourOptions( );
263  virtual ~TColourOptions() {}
264  /** See utils::CLoadableOptions
265  */
266  void loadFromConfigFile(
267  const mrpt::utils::CConfigFileBase &source,
268  const std::string &section);
269 
270  /** See utils::CLoadableOptions
271  */
272  void dumpToTextStream(CStream &out) const;
275  float z_min,z_max;
276  float d_max;
277  };
278 
279  TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map.
280 
281  void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value
282 
283  /** @name PCL library support
284  @{ */
285 
286  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format \return false on any error */
287  virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const;
289  /** Loads a PCL point cloud (WITH RGB information) into this MRPT class (for clouds without RGB data, see CPointsMap::setFromPCLPointCloud() ).
290  * Usage example:
291  * \code
292  * pcl::PointCloud<pcl::PointXYZRGB> cloud;
293  * mrpt::slam::CColouredPointsMap pc;
294  *
295  * pc.setFromPCLPointCloudRGB(cloud);
296  * \endcode
297  * \sa CPointsMap::setFromPCLPointCloud()
298  */
299  template <class POINTCLOUD>
300  void setFromPCLPointCloudRGB(const POINTCLOUD &cloud)
301  {
302  const size_t N = cloud.points.size();
303  clear();
304  reserve(N);
305  const float f = 1.0f/255.0f;
306  for (size_t i=0;i<N;++i)
307  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);
308  }
310  /** @} */
313  protected:
314  /** The color data */
315  std::vector<float> m_color_R,m_color_G,m_color_B;
317  /** Minimum distance from where the points have been seen */
318  //std::vector<float> m_min_dist;
320  /** Clear the map, erasing all the points.
321  */
322  virtual void internal_clear();
323 
324  /** @name Redefinition of PLY Import virtual methods from CPointsMap
325  @{ */
326  /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
327  * \param pt_color Will be NULL if the loaded file does not provide color info.
328  */
329  virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL);
331  /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
332  virtual void PLY_import_set_vertex_count(const size_t N);
333  /** @} */
334 
335  /** @name Redefinition of PLY Export virtual methods from CPointsMap
336  @{ */
337  /** In a base class, will be called after PLY_export_get_vertex_count() once for each exported point.
338  * \param pt_color Will be NULL if the loaded file does not provide color info.
339  */
340  virtual void PLY_export_get_vertex(
341  const size_t idx,
343  bool &pt_has_color,
344  mrpt::utils::TColorf &pt_color) const;
345  /** @} */
346 
347  }; // End of class def.
348 
349  } // End of namespace
350 
351 #include <mrpt/utils/adapters.h>
352  namespace utils
353  {
354  /** Specialization mrpt::utils::PointCloudAdapter<mrpt::slam::CColouredPointsMap> \ingroup mrpt_adapters_grp */
355  template <>
356  class PointCloudAdapter<mrpt::slam::CColouredPointsMap>
357  {
358  private:
360  public:
361  typedef float coords_t; //!< The type of each point XYZ coordinates
362  static const int HAS_RGB = 1; //!< Has any color RGB info?
363  static const int HAS_RGBf = 1; //!< Has native RGB info (as floats)?
364  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
365 
366  /** Constructor (accept a const ref for convenience) */
367  inline PointCloudAdapter(const mrpt::slam::CColouredPointsMap &obj) : m_obj(*const_cast<mrpt::slam::CColouredPointsMap*>(&obj)) { }
368  /** Get number of points */
369  inline size_t size() const { return m_obj.size(); }
370  /** Set number of points (to uninitialized values) */
371  inline void resize(const size_t N) { m_obj.resize(N); }
372 
373  /** Get XYZ coordinates of i'th point */
374  template <typename T>
375  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
376  m_obj.getPointFast(idx,x,y,z);
377  }
378  /** Set XYZ coordinates of i'th point */
379  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
380  m_obj.setPointFast(idx,x,y,z);
381  }
383  /** Get XYZ_RGBf coordinates of i'th point */
384  template <typename T>
385  inline void getPointXYZ_RGBf(const size_t idx, T &x,T &y, T &z, float &r,float &g,float &b) const {
386  m_obj.getPoint(idx,x,y,z,r,g,b);
387  }
388  /** Set XYZ_RGBf coordinates of i'th point */
389  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) {
390  m_obj.setPoint(idx,x,y,z,r,g,b);
391  }
392 
393  /** Get XYZ_RGBu8 coordinates of i'th point */
394  template <typename T>
395  inline void getPointXYZ_RGBu8(const size_t idx, T &x,T &y, T &z, uint8_t &r,uint8_t &g,uint8_t &b) const {
396  float Rf,Gf,Bf;
397  m_obj.getPoint(idx,x,y,z,Rf,Gf,Bf);
398  r=Rf*255; g=Gf*255; b=Bf*255;
399  }
400  /** Set XYZ_RGBu8 coordinates of i'th point */
401  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) {
402  m_obj.setPoint(idx,x,y,z,r/255.f,g/255.f,b/255.f);
403  }
404 
405  /** Get RGBf color of i'th point */
406  inline void getPointRGBf(const size_t idx, float &r,float &g,float &b) const { m_obj.getPointColor_fast(idx,r,g,b); }
407  /** Set XYZ_RGBf coordinates of i'th point */
408  inline void setPointRGBf(const size_t idx, const float r,const float g,const float b) { m_obj.setPointColor_fast(idx,r,g,b); }
409 
410  /** Get RGBu8 color of i'th point */
411  inline void getPointRGBu8(const size_t idx, uint8_t &r,uint8_t &g,uint8_t &b) const {
412  float R,G,B;
413  m_obj.getPointColor_fast(idx,R,G,B);
414  r=R*255; g=G*255; b=B*255;
415  }
416  /** Set RGBu8 coordinates of i'th point */
417  inline void setPointRGBu8(const size_t idx,const uint8_t r,const uint8_t g,const uint8_t b) {
418  m_obj.setPointColor_fast(idx,r/255.f,g/255.f,b/255.f);
419  }
420 
421  }; // end of PointCloudAdapter<mrpt::slam::CColouredPointsMap>
422 
423  }
424 
425 } // End of namespace
426 
427 #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:142
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:356
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.
void setPoint(size_t index, float x, float y, float z)
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)
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.
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
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).
TColourOptions colorScheme
The options employed when inserting laser scans in the map.
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...
void getPointRGBf(const size_t idx, float &r, float &g, float &b) const
Get RGBf color of i&#39;th point.
double x
X coordinate.
A class used to store a 3D point.
Definition: CPoint3D.h:59
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:395
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
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.
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.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo