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