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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013