Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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  virtual void reserve(size_t newLength) MRPT_OVERRIDE; // See base class docs
51  virtual void resize(size_t newLength) MRPT_OVERRIDE; // See base class docs
52  virtual void setSize(size_t newLength) MRPT_OVERRIDE; // See base class docs
53 
54  /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
55  virtual void setPointFast(size_t index,float x, float y, float z) MRPT_OVERRIDE
56  {
57  this->x[index] = x;
58  this->y[index] = y;
59  this->z[index] = z;
60  }
61 
62  /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */
63  virtual void insertPointFast( float x, float y, float z = 0 ) MRPT_OVERRIDE;
64 
65  /** Virtual assignment operator, to be implemented in derived classes */
66  virtual void copyFrom(const CPointsMap &obj) MRPT_OVERRIDE;
67 
68  /** Get all the data fields for one point as a vector: [X Y Z R G B]
69  * Unlike getPointAllFields(), this method does not check for index out of bounds
70  * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
71  */
72  virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const MRPT_OVERRIDE {
73  point_data.resize(6);
74  point_data[0] = x[index];
75  point_data[1] = y[index];
76  point_data[2] = z[index];
77  point_data[3] = m_color_R[index];
78  point_data[4] = m_color_G[index];
79  point_data[5] = m_color_B[index];
80  }
81 
82  /** Set all the data fields for one point as a vector: [X Y Z R G B]
83  * Unlike setPointAllFields(), this method does not check for index out of bounds
84  * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
85  */
86  virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) MRPT_OVERRIDE {
87  ASSERTDEB_(point_data.size()==6)
88  x[index] = point_data[0];
89  y[index] = point_data[1];
90  z[index] = point_data[2];
91  m_color_R[index] = point_data[3];
92  m_color_G[index] = point_data[4];
93  m_color_B[index] = point_data[5];
94  }
95 
96  /** See CPointsMap::loadFromRangeScan() */
97  virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan,const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
98  /** See CPointsMap::loadFromRangeScan() */
99  virtual void loadFromRangeScan(const mrpt::obs::CObservation3DRangeScan &rangeScan,const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
101  protected:
102  /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */
103  virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints) MRPT_OVERRIDE;
105  // Friend methods:
106  template <class Derived> friend struct detail::loadFromRangeImpl;
107  template <class Derived> friend struct detail::pointmap_traits;
109  public:
110  /** @} */
111  // --------------------------------------------
113  /** 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.
114  * Returns false if any error occured, true elsewere.
115  */
116  bool save3D_and_colour_to_text_file(const std::string &file) const;
117 
118  /** Changes a given point from map. First index is 0.
119  * \exception Throws std::exception on index out of bound.
120  */
121  virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B) MRPT_OVERRIDE;
122 
123  // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "setPoint()"
124  /// \overload
125  inline void setPoint(size_t index,float x, float y, float z) {
126  ASSERT_BELOW_(index,this->size())
127  setPointFast(index,x,y,z);
128  mark_as_modified();
129  }
130  /// \overload
131  inline void setPoint(size_t index,mrpt::math::TPoint3Df &p) { setPoint(index,p.x,p.y,p.z); }
132  /// \overload
133  inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
134 
135 
136  /** Adds a new point given its coordinates and color (colors range is [0,1]) */
137  virtual void insertPoint( float x, float y, float z, float R, float G, float B ) MRPT_OVERRIDE;
138  // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "insertPoint()"
139  /// \overload
140  inline void insertPoint( const mrpt::poses::CPoint3D &p ) { insertPoint(p.x(),p.y(),p.z()); }
141  /// \overload
142  inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
143  /// \overload
144  inline void insertPoint( const mrpt::math::TPoint3Df &p ) { insertPoint(p.x,p.y,p.z); }
145  /// \overload
146  inline void insertPoint( float x, float y, float z) { insertPointFast(x,y,z); mark_as_modified(); }
147 
148  /** Changes just the color of a given point from the map. First index is 0.
149  * \exception Throws std::exception on index out of bound.
150  */
151  void setPointColor(size_t index,float R, float G, float B);
152 
153  /** Like \c setPointColor but without checking for out-of-index erors */
154  inline void setPointColor_fast(size_t index,float R, float G, float B)
155  {
156  this->m_color_R[index]=R;
157  this->m_color_G[index]=G;
158  this->m_color_B[index]=B;
159  }
160 
161  /** Retrieves a point and its color (colors range is [0,1])
162  */
163  virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const MRPT_OVERRIDE;
164 
165  /** Retrieves a point */
166  unsigned long getPoint( size_t index, float &x, float &y, float &z) const;
167 
168  /** Retrieves a point color (colors range is [0,1]) */
169  void getPointColor( size_t index, float &R, float &G, float &B ) const;
171  /** Like \c getPointColor but without checking for out-of-index erors */
172  inline void getPointColor_fast( size_t index, float &R, float &G, float &B ) const
173  {
174  R = m_color_R[index];
175  G = m_color_G[index];
176  B = m_color_B[index];
177  }
178 
179  /** Returns true if the point map has a color field for each point */
180  virtual bool hasColorPoints() const MRPT_OVERRIDE { return true; }
182  /** Override of the default 3D scene builder to account for the individual points' color.
183  * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
184  */
185  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const MRPT_OVERRIDE;
186 
187  /** Colour a set of points from a CObservationImage and the global pose of the robot */
188  bool colourFromObservation( const mrpt::obs::CObservationImage &obs, const mrpt::poses::CPose3D &robotPose );
189 
190  /** The choices for coloring schemes:
191  * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max.
192  * - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available.
193  * \sa TColourOptions
194  */
196  {
197  cmFromHeightRelativeToSensor = 0,
198  cmFromHeightRelativeToSensorJet = 0,
199  cmFromHeightRelativeToSensorGray = 1,
200  cmFromIntensityImage = 2
201  };
202 
203  /** The definition of parameters for generating colors from laser scans */
205  {
206  /** Initilization of default parameters */
207  TColourOptions( );
208  virtual ~TColourOptions() {}
210  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
211  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
212 
214  float z_min,z_max;
215  float d_max;
216  };
218  TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map.
220  void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value
222  /** @name PCL library support
223  @{ */
224 
225  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format \return false on any error */
226  virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const MRPT_OVERRIDE;
227 
228  /** Loads a PCL point cloud (WITH RGB information) into this MRPT class (for clouds without RGB data, see CPointsMap::setFromPCLPointCloud() ).
229  * Usage example:
230  * \code
231  * pcl::PointCloud<pcl::PointXYZRGB> cloud;
232  * mrpt::maps::CColouredPointsMap pc;
233  *
234  * pc.setFromPCLPointCloudRGB(cloud);
235  * \endcode
236  * \sa CPointsMap::setFromPCLPointCloud()
237  */
238  template <class POINTCLOUD>
239  void setFromPCLPointCloudRGB(const POINTCLOUD &cloud)
240  {
241  const size_t N = cloud.points.size();
242  clear();
243  reserve(N);
244  const float f = 1.0f/255.0f;
245  for (size_t i=0;i<N;++i)
246  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);
247  }
248 
249  /** Like CPointsMap::getPCLPointCloud() but for PointCloud<PointXYZRGB> */
250  template <class POINTCLOUD>
251  void getPCLPointCloudXYZRGB(POINTCLOUD &cloud) const
252  {
253  const size_t nThis = this->size();
254  this->getPCLPointCloud(cloud); // 1st: xyz data
255  // 2nd: RGB data
256  for (size_t i = 0; i < nThis; ++i) {
257  float R,G,B;
258  this->getPointColor_fast(i,R,G,B);
259  cloud.points[i].r = static_cast<uint8_t>(R*255);
260  cloud.points[i].g = static_cast<uint8_t>(G*255);
261  cloud.points[i].b = static_cast<uint8_t>(B*255);
262  }
263  }
264  /** @} */
265 
266  protected:
267  /** The color data */
268  std::vector<float> m_color_R,m_color_G,m_color_B;
269 
270  /** Minimum distance from where the points have been seen */
271  //std::vector<float> m_min_dist;
272 
273  /** Clear the map, erasing all the points */
274  virtual void internal_clear() MRPT_OVERRIDE;
275 
276  /** @name Redefinition of PLY Import virtual methods from CPointsMap
277  @{ */
278  /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
279  * \param pt_color Will be NULL if the loaded file does not provide color info.
280  */
281  virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL) MRPT_OVERRIDE;
283  /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
284  virtual void PLY_import_set_vertex_count(const size_t N) MRPT_OVERRIDE;
285  /** @} */
287  /** @name Redefinition of PLY Export virtual methods from CPointsMap
288  @{ */
289  void PLY_export_get_vertex(const size_t idx,mrpt::math::TPoint3Df &pt,bool &pt_has_color,mrpt::utils::TColorf &pt_color) const MRPT_OVERRIDE;
290  /** @} */
291 
293  mrpt::maps::CPointsMap::TInsertionOptions insertionOpts;
294  mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts;
297 
298  }; // End of class def.
301  } // End of namespace
302 
303 #include <mrpt/utils/adapters.h>
304  namespace utils
305  {
306  /** Specialization mrpt::utils::PointCloudAdapter<mrpt::maps::CColouredPointsMap> \ingroup mrpt_adapters_grp */
307  template <>
309  {
310  private:
312  public:
313  typedef float coords_t; //!< The type of each point XYZ coordinates
314  static const int HAS_RGB = 1; //!< Has any color RGB info?
315  static const int HAS_RGBf = 1; //!< Has native RGB info (as floats)?
316  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
317 
318  /** Constructor (accept a const ref for convenience) */
319  inline PointCloudAdapter(const mrpt::maps::CColouredPointsMap &obj) : m_obj(*const_cast<mrpt::maps::CColouredPointsMap*>(&obj)) { }
320  /** Get number of points */
321  inline size_t size() const { return m_obj.size(); }
322  /** Set number of points (to uninitialized values) */
323  inline void resize(const size_t N) { m_obj.resize(N); }
324 
325  /** Get XYZ coordinates of i'th point */
326  template <typename T>
327  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
328  m_obj.getPointFast(idx,x,y,z);
329  }
330  /** Set XYZ coordinates of i'th point */
331  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
332  m_obj.setPointFast(idx,x,y,z);
333  }
334 
335  /** Get XYZ_RGBf coordinates of i'th point */
336  template <typename T>
337  inline void getPointXYZ_RGBf(const size_t idx, T &x,T &y, T &z, float &r,float &g,float &b) const {
338  m_obj.getPoint(idx,x,y,z,r,g,b);
339  }
340  /** Set XYZ_RGBf coordinates of i'th point */
341  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) {
342  m_obj.setPoint(idx,x,y,z,r,g,b);
343  }
344 
345  /** Get XYZ_RGBu8 coordinates of i'th point */
346  template <typename T>
347  inline void getPointXYZ_RGBu8(const size_t idx, T &x,T &y, T &z, uint8_t &r,uint8_t &g,uint8_t &b) const {
348  float Rf,Gf,Bf;
349  m_obj.getPoint(idx,x,y,z,Rf,Gf,Bf);
350  r=Rf*255; g=Gf*255; b=Bf*255;
351  }
352  /** Set XYZ_RGBu8 coordinates of i'th point */
353  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) {
354  m_obj.setPoint(idx,x,y,z,r/255.f,g/255.f,b/255.f);
355  }
356 
357  /** Get RGBf color of i'th point */
358  inline void getPointRGBf(const size_t idx, float &r,float &g,float &b) const { m_obj.getPointColor_fast(idx,r,g,b); }
359  /** Set XYZ_RGBf coordinates of i'th point */
360  inline void setPointRGBf(const size_t idx, const float r,const float g,const float b) { m_obj.setPointColor_fast(idx,r,g,b); }
361 
362  /** Get RGBu8 color of i'th point */
363  inline void getPointRGBu8(const size_t idx, uint8_t &r,uint8_t &g,uint8_t &b) const {
364  float R,G,B;
365  m_obj.getPointColor_fast(idx,R,G,B);
366  r=R*255; g=G*255; b=B*255;
367  }
368  /** Set RGBu8 coordinates of i'th point */
369  inline void setPointRGBu8(const size_t idx,const uint8_t r,const uint8_t g,const uint8_t b) {
370  m_obj.setPointColor_fast(idx,r/255.f,g/255.f,b/255.f);
371  }
372 
373  }; // end of PointCloudAdapter<mrpt::maps::CColouredPointsMap>
374 
375  }
376 
377 } // End of namespace
378 
379 #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.
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...
void insertPoint(const mrpt::math::TPoint3D &p)
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
GLdouble GLdouble z
Definition: glext.h:3734
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 setPoint(size_t index, mrpt::math::TPoint3Df &p)
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 setPointColor_fast(size_t index, float R, float G, float B)
Like setPointColor but without checking for out-of-index erors.
STL namespace.
void setFromPCLPointCloudRGB(const POINTCLOUD &cloud)
Loads a PCL point cloud (WITH RGB information) into this MRPT class (for clouds without RGB data...
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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...
unsigned char uint8_t
Definition: rptypes.h:43
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 getPoint(size_t index, float &x, float &y, float &z, float &R, float &G, float &B) const MRPT_OVERRIDE
Retrieves a point and its color (colors range is [0,1])
#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).
GLuint index
Definition: glext.h:3891
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.
GLubyte g
Definition: glext.h:5575
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...
GLubyte GLubyte b
Definition: glext.h:5575
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).
virtual void setPoint(size_t index, float x, float y, float z, float R, float G, float B) MRPT_OVERRIDE
Changes a given point from map.
GLsizei const GLchar ** string
Definition: glext.h:3919
virtual void resize(size_t newLength) MRPT_OVERRIDE
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
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.
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)
#define MRPT_OVERRIDE
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
size_t size(const MATRIXLIKE &m, const int dim)
virtual void setPointFast(size_t index, float x, float y, float z) MRPT_OVERRIDE
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
const float R
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.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
void resize(const size_t N)
Set number of points (to uninitialized values)
virtual bool hasColorPoints() const MRPT_OVERRIDE
Returns true if the point map has a color field for each point.
GLenum GLint GLint y
Definition: glext.h:3516
void insertPoint(const mrpt::poses::CPoint3D &p)
std::vector< float > m_color_R
The color 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.
The definition of parameters for generating colors from laser scans.
An adapter to different kinds of point cloud object.
Definition: adapters.h:38
GLenum GLint x
Definition: glext.h:3516
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.
GLfloat GLfloat p
Definition: glext.h:5587
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019