The MRPT project: CColouredPointsMap.h Source File

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                       http://www.mrpt.org/                                |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CColouredPointsMap_H
00029 #define CColouredPointsMap_H
00030 
00031 #include <mrpt/slam/CPointsMap.h>
00032 #include <mrpt/slam/CObservation2DRangeScan.h>
00033 #include <mrpt/slam/CObservationImage.h>
00034 #include <mrpt/utils/CSerializable.h>
00035 #include <mrpt/math/CMatrix.h>
00036 #include <mrpt/utils/stl_extensions.h>
00037 
00038 #include <mrpt/maps/link_pragmas.h>
00039 
00040 namespace mrpt
00041 {
00042         namespace slam
00043         {
00044                 class CObservation3DRangeScan;
00045 
00046 
00047                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CColouredPointsMap, CPointsMap,MAPS_IMPEXP )
00048 
00049                 /** A map of 2D/3D points with individual colours (RGB).
00050                  *  For different color schemes, see CColouredPointsMap::colorScheme
00051                  *  Colors are defined in the range [0,1].
00052                  * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable
00053                  * \ingroup mrpt_maps_grp
00054                  */
00055                 class MAPS_IMPEXP CColouredPointsMap : public CPointsMap
00056                 {
00057                         // This must be added to any CSerializable derived class:
00058                         DEFINE_SERIALIZABLE( CColouredPointsMap )
00059 
00060                 public:
00061                          /** Destructor
00062                            */
00063                          virtual ~CColouredPointsMap();
00064 
00065                          /** Default constructor
00066                           */
00067                          CColouredPointsMap();
00068 
00069                         // --------------------------------------------
00070                         /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
00071                                 @{ */
00072 
00073                         /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
00074                           *  This is useful for situations where it is approximately known the final size of the map. This method is more
00075                           *  efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
00076                           */
00077                         virtual void reserve(size_t newLength);
00078 
00079                         /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
00080                           *  and old contents are not changed.
00081                           * \sa reserve, setPoint, setPointFast, setSize
00082                           */
00083                         virtual void resize(size_t newLength);
00084 
00085                         /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
00086                           *  and leaving all points to default values.
00087                           * \sa reserve, setPoint, setPointFast, setSize
00088                           */
00089                         virtual void setSize(size_t newLength);
00090 
00091                         /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified()  \sa setPoint */
00092                         virtual void  setPointFast(size_t index,float x, float y, float z)
00093                         {
00094                                 this->x[index] = x;
00095                                 this->y[index] = y;
00096                                 this->z[index] = z;
00097                         }
00098 
00099                         /** The virtual method for \a insertPoint() *without* calling mark_as_modified()   */
00100                         virtual void  insertPointFast( float x, float y, float z = 0 );
00101 
00102                          /** Virtual assignment operator, to be implemented in derived classes.
00103                            */
00104                          virtual void  copyFrom(const CPointsMap &obj);
00105 
00106                         /** Get all the data fields for one point as a vector: [X Y Z R G B]
00107                           *  Unlike getPointAllFields(), this method does not check for index out of bounds
00108                           * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
00109                           */
00110                         virtual void  getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const {
00111                                 point_data.resize(6);
00112                                 point_data[0] = x[index];
00113                                 point_data[1] = y[index];
00114                                 point_data[2] = z[index];
00115                                 point_data[3] = m_color_R[index];
00116                                 point_data[4] = m_color_G[index];
00117                                 point_data[5] = m_color_B[index];
00118                         }
00119 
00120                         /** Set all the data fields for one point as a vector: [X Y Z R G B]
00121                           *  Unlike setPointAllFields(), this method does not check for index out of bounds
00122                           * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
00123                           */
00124                         virtual void  setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) {
00125                                 ASSERTDEB_(point_data.size()==6)
00126                                 x[index] = point_data[0];
00127                                 y[index] = point_data[1];
00128                                 z[index] = point_data[2];
00129                                 m_color_R[index] = point_data[3];
00130                                 m_color_G[index] = point_data[4];
00131                                 m_color_B[index] = point_data[5];
00132                         }
00133 
00134                         /** See CPointsMap::loadFromRangeScan() */
00135                         virtual void  loadFromRangeScan(
00136                                         const CObservation2DRangeScan &rangeScan,
00137                                         const CPose3D                             *robotPose = NULL );
00138 
00139                         /** See CPointsMap::loadFromRangeScan() */
00140                         virtual void  loadFromRangeScan(
00141                                         const CObservation3DRangeScan &rangeScan,
00142                                         const CPose3D                             *robotPose = NULL );
00143 
00144                 protected:
00145 
00146                         /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data  */
00147                         virtual void  addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints);
00148 
00149 
00150                         // Friend methods:
00151                         template <class Derived> friend struct detail::loadFromRangeImpl;
00152                         template <class Derived> friend struct detail::pointmap_traits;
00153 
00154                 public:
00155 
00156 
00157                         /** @} */
00158                         // --------------------------------------------
00159 
00160                         /** 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.
00161                          *     Returns false if any error occured, true elsewere.
00162                          */
00163                         bool  save3D_and_colour_to_text_file(const std::string &file) const;
00164 
00165                         /** Changes a given point from map. First index is 0.
00166                          * \exception Throws std::exception on index out of bound.
00167                          */
00168                         virtual void  setPoint(size_t index,float x, float y, float z, float R, float G, float B);
00169 
00170                         // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "setPoint()"
00171                         /// \overload
00172                         inline void  setPoint(size_t index,float x, float y, float z) {
00173                                 ASSERT_BELOW_(index,this->size())
00174                                 setPointFast(index,x,y,z);
00175                                 mark_as_modified();
00176                         }
00177                         /// \overload
00178                         inline void  setPoint(size_t index,CPoint2D &p) {  setPoint(index,p.x(),p.y(),0); }
00179                         /// \overload
00180                         inline void  setPoint(size_t index,CPoint3D &p)  { setPoint(index,p.x(),p.y(),p.z()); }
00181                         /// \overload
00182                         inline void  setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
00183 
00184 
00185                         /** Adds a new point given its coordinates and color (colors range is [0,1]) */
00186                         virtual void  insertPoint( float x, float y, float z, float R, float G, float B );
00187                         // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "insertPoint()"
00188                         /// \overload of \a insertPoint()
00189                         inline void  insertPoint( const CPoint3D &p ) { insertPoint(p.x(),p.y(),p.z()); }
00190                         /// \overload
00191                         inline void  insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
00192                         /// \overload
00193                         inline void  insertPoint( float x, float y, float z) { insertPointFast(x,y,z); mark_as_modified(); }
00194 
00195                         /** Changes just the color of a given point from the map. First index is 0.
00196                          * \exception Throws std::exception on index out of bound.
00197                          */
00198                         void  setPointColor(size_t index,float R, float G, float B);
00199 
00200                         /** Like \c setPointColor but without checking for out-of-index erors */
00201                         inline void  setPointColor_fast(size_t index,float R, float G, float B)
00202                         {
00203                                 this->m_color_R[index]=R;
00204                                 this->m_color_G[index]=G;
00205                                 this->m_color_B[index]=B;
00206                         }
00207 
00208                         /** Retrieves a point and its color (colors range is [0,1])
00209                           */
00210                         virtual void  getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const;
00211 
00212                         /** Retrieves a point  */
00213                         unsigned long  getPoint( size_t index, float &x, float &y, float &z) const;
00214 
00215                         /** Retrieves a point color (colors range is [0,1]) */
00216                         void  getPointColor( size_t index, float &R, float &G, float &B ) const;
00217 
00218                         /** Like \c getPointColor but without checking for out-of-index erors */
00219                         inline void  getPointColor_fast( size_t index, float &R, float &G, float &B ) const
00220                         {
00221                                 R = m_color_R[index];
00222                                 G = m_color_G[index];
00223                                 B = m_color_B[index];
00224                         }
00225 
00226                         /** Returns true if the point map has a color field for each point */
00227                         virtual bool hasColorPoints() const { return true; }
00228 
00229                         /** Override of the default 3D scene builder to account for the individual points' color.
00230                           * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
00231                           */
00232                         virtual void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;
00233 
00234                         /** Colour a set of points from a CObservationImage and the global pose of the robot
00235                           */
00236                         bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose );
00237 
00238                         /** The choices for coloring schemes:
00239                           *             - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max.
00240                           *     - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available.
00241                           * \sa TColourOptions
00242                           */
00243                         enum TColouringMethod
00244                         {
00245                                 cmFromHeightRelativeToSensor = 0,
00246                                 cmFromHeightRelativeToSensorJet = 0,
00247                                 cmFromHeightRelativeToSensorGray = 1,
00248                                 cmFromIntensityImage = 2
00249                         };
00250 
00251                         /** The definition of parameters for generating colors from laser scans */
00252                          struct MAPS_IMPEXP TColourOptions : public utils::CLoadableOptions
00253                          {
00254                                 /** Initilization of default parameters */
00255                                 TColourOptions( );
00256                                 virtual ~TColourOptions() {}
00257                                 /** See utils::CLoadableOptions
00258                                   */
00259                                 void  loadFromConfigFile(
00260                                         const mrpt::utils::CConfigFileBase  &source,
00261                                         const std::string &section);
00262 
00263                                 /** See utils::CLoadableOptions
00264                                   */
00265                                 void  dumpToTextStream(CStream  &out) const;
00266 
00267                                 TColouringMethod        scheme;
00268                                 float                           z_min,z_max;
00269                                 float                           d_max;
00270                          };
00271 
00272                          TColourOptions colorScheme;    //!< The options employed when inserting laser scans in the map.
00273 
00274                          void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value
00275 
00276             /** @name PCL library support
00277                 @{ */
00278 
00279             /** Save the point cloud as a PCL PCD file, in either ASCII or binary format \return false on any error */
00280             virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const;
00281 
00282             /** @} */
00283 
00284 
00285                 protected:
00286                         /** The color data */
00287                         std::vector<float>      m_color_R,m_color_G,m_color_B;
00288 
00289                         /** Minimum distance from where the points have been seen */
00290                         //std::vector<float>    m_min_dist;
00291 
00292                         /** Clear the map, erasing all the points.
00293                          */
00294                         virtual void  internal_clear();
00295 
00296                         /** @name Redefinition of PLY Import virtual methods from CPointsMap
00297                             @{ */
00298                         /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
00299                           *  \param pt_color Will be NULL if the loaded file does not provide color info.
00300                           */
00301                         virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL);
00302 
00303                         /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
00304                         virtual void PLY_import_set_vertex_count(const size_t N);
00305                         /** @} */
00306 
00307                         /** @name Redefinition of PLY Export virtual methods from CPointsMap
00308                             @{ */
00309                         /** In a base class, will be called after PLY_export_get_vertex_count() once for each exported point.
00310                           *  \param pt_color Will be NULL if the loaded file does not provide color info.
00311                           */
00312                         virtual void PLY_export_get_vertex(
00313                                 const size_t idx,
00314                                 mrpt::math::TPoint3Df &pt,
00315                                 bool &pt_has_color,
00316                                 mrpt::utils::TColorf &pt_color) const;
00317                         /** @} */
00318 
00319                 }; // End of class def.
00320 
00321         } // End of namespace
00322 
00323 #include <mrpt/utils/adapters.h>
00324         namespace utils
00325         {
00326                 /** Specialization mrpt::utils::PointCloudAdapter<mrpt::slam::CColouredPointsMap> \ingroup mrpt_adapters_grp */
00327                 template <>
00328                 class PointCloudAdapter<mrpt::slam::CColouredPointsMap>
00329                 {
00330                 private:
00331                         mrpt::slam::CColouredPointsMap &m_obj;
00332                 public:
00333                         typedef float  coords_t;         //!< The type of each point XYZ coordinates
00334                         static const int HAS_RGB   = 1;  //!< Has any color RGB info?
00335                         static const int HAS_RGBf  = 1;  //!< Has native RGB info (as floats)?
00336                         static const int HAS_RGBu8 = 0;  //!< Has native RGB info (as uint8_t)?
00337 
00338                         /** Constructor (accept a const ref for convenience) */
00339                         inline PointCloudAdapter(const mrpt::slam::CColouredPointsMap &obj) : m_obj(*const_cast<mrpt::slam::CColouredPointsMap*>(&obj)) { }
00340                         /** Get number of points */
00341                         inline size_t size() const { return m_obj.size(); }
00342                         /** Set number of points (to uninitialized values) */
00343                         inline void resize(const size_t N) { m_obj.resize(N); }
00344 
00345                         /** Get XYZ coordinates of i'th point */
00346                         template <typename T>
00347                         inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
00348                                 m_obj.getPointFast(idx,x,y,z);
00349                         }
00350                         /** Set XYZ coordinates of i'th point */
00351                         inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
00352                                 m_obj.setPointFast(idx,x,y,z);
00353                         }
00354 
00355                         /** Get XYZ_RGBf coordinates of i'th point */
00356                         template <typename T>
00357                         inline void getPointXYZ_RGBf(const size_t idx, T &x,T &y, T &z, float &r,float &g,float &b) const {
00358                                 m_obj.getPoint(idx,x,y,z,r,g,b);
00359                         }
00360                         /** Set XYZ_RGBf coordinates of i'th point */
00361                         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) {
00362                                 m_obj.setPoint(idx,x,y,z,r,g,b);
00363                         }
00364 
00365                         /** Get XYZ_RGBu8 coordinates of i'th point */
00366                         template <typename T>
00367                         inline void getPointXYZ_RGBu8(const size_t idx, T &x,T &y, T &z, uint8_t &r,uint8_t &g,uint8_t &b) const {
00368                                 float Rf,Gf,Bf;
00369                                 m_obj.getPoint(idx,x,y,z,Rf,Gf,Bf);
00370                                 r=Rf*255; g=Gf*255; b=Bf*255;
00371                         }
00372                         /** Set XYZ_RGBu8 coordinates of i'th point */
00373                         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) {
00374                                 m_obj.setPoint(idx,x,y,z,r/255.f,g/255.f,b/255.f);
00375                         }
00376 
00377                         /** Get RGBf color of i'th point */
00378                         inline void getPointRGBf(const size_t idx, float &r,float &g,float &b) const { m_obj.getPointColor_fast(idx,r,g,b); }
00379                         /** Set XYZ_RGBf coordinates of i'th point */
00380                         inline void setPointRGBf(const size_t idx, const float r,const float g,const float b) { m_obj.setPointColor_fast(idx,r,g,b); }
00381 
00382                         /** Get RGBu8 color of i'th point */
00383                         inline void getPointRGBu8(const size_t idx, uint8_t &r,uint8_t &g,uint8_t &b) const {
00384                                 float R,G,B;
00385                                 m_obj.getPointColor_fast(idx,R,G,B);
00386                                 r=R*255; g=G*255; b=B*255;
00387                         }
00388                         /** Set RGBu8 coordinates of i'th point */
00389                         inline void setPointRGBu8(const size_t idx,const uint8_t r,const uint8_t g,const uint8_t b) {
00390                                 m_obj.setPointColor_fast(idx,r/255.f,g/255.f,b/255.f);
00391                         }
00392 
00393                 }; // end of PointCloudAdapter<mrpt::slam::CColouredPointsMap>
00394 
00395         }
00396 
00397 } // End of namespace
00398 
00399 #endif