Main MRPT website > C++ reference
MRPT logo

CColouredPointsMap.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
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 
00045                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CColouredPointsMap, CPointsMap,MAPS_IMPEXP )
00046 
00047                 /** A map of 2D/3D points with individual colours (RGB).
00048                  *  For different color schemes, see CColouredPointsMap::colorScheme
00049                  *  Colors are defined in the range [0,1].
00050                  * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable
00051                  */
00052                 class MAPS_IMPEXP CColouredPointsMap : public CPointsMap
00053                 {
00054                         // This must be added to any CSerializable derived class:
00055                         DEFINE_SERIALIZABLE( CColouredPointsMap )
00056 
00057                 protected:
00058                         /** The color data */
00059                         std::vector<float>      m_color_R,m_color_G,m_color_B;
00060 
00061                         /** Minimum distance from where the points have been seen */
00062                         std::vector<float>      m_min_dist;
00063 
00064                         /** Clear the map, erasing all the points.
00065                          */
00066                         virtual void  internal_clear();
00067 
00068                          /** Insert the observation information into this map. This method must be implemented
00069                           *    in derived classes.
00070                           * \param obs The observation
00071                           * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
00072                           *
00073                           * \sa CObservation::insertObservationInto
00074                           */
00075                         virtual bool  internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00076 
00077                 public:
00078                          /** Destructor
00079                            */
00080                          virtual ~CColouredPointsMap();
00081 
00082                          /** Default constructor
00083                           */
00084                          CColouredPointsMap();
00085 
00086                          /** Copy operator
00087                           */
00088                          void  copyFrom(const CPointsMap &obj);
00089 
00090                         /** Transform the range scan into a set of cartessian coordinated
00091                           *      points. The options in "insertionOptions" are considered in this method.
00092                           * \param rangeScan The scan to be inserted into this map
00093                           * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class.
00094                           *
00095                           * \note Only ranges marked as "valid=true" in the observation will be inserted
00096                           *
00097                           * \sa CObservation2DRangeScan
00098                           */
00099                         void  loadFromRangeScan(
00100                                         const CObservation2DRangeScan &rangeScan,
00101                                         const CPose3D                             *robotPose = NULL );
00102 
00103                         /** Transform the range scan into a set of cartessian coordinated
00104                           *      points. The options in "insertionOptions" are considered in this method.
00105                           * \param rangeScan The scan to be inserted into this map
00106                           * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class.
00107                           *
00108                           * \sa CObservation3DRangeScan
00109                           */
00110                         void  loadFromRangeScan(
00111                                         const CObservation3DRangeScan &rangeScan,
00112                                         const CPose3D                             *robotPose = NULL );
00113 
00114                         /** Load from a text file. In each line there are a point coordinates.
00115                          *   Returns false if any error occured, true elsewere.
00116                          */
00117                         bool  load2D_from_text_file(std::string file);
00118 
00119                         /** Load from a text file. In each line there are a point coordinates.
00120                          *   Returns false if any error occured, true elsewere.
00121                          */
00122                         bool  load3D_from_text_file(std::string file);
00123 
00124                         /** 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.
00125                          *     Returns false if any error occured, true elsewere.
00126                          */
00127                         bool  save3D_and_colour_to_text_file(const std::string &file) const;
00128 
00129                         /** Insert the contents of another map into this one, fusing the previous content with the new one.
00130                          *    This means that points very close to existing ones will be "fused", rather than "added". This prevents
00131                          *     the unbounded increase in size of these class of maps.
00132                          *              NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
00133                          *               before calling this method.
00134                          * \param otherMap The other map whose points are to be inserted into this one.
00135                          * \param minDistForFuse Minimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added.
00136                          * \param notFusedPoints If a pointer is supplied, this list will contain at output a list with a "bool" value per point in "this" map. This will be false/true according to that point having been fused or not.
00137                          * \sa insertAnotherMap
00138                          */
00139                         void  fuseWith( CPointsMap                      *otherMap,
00140                                                                                 float                           minDistForFuse  = 0.02f,
00141                                                                                 std::vector<bool>       *notFusedPoints = NULL);
00142 
00143                         /** Changes a given point from map, as a 2D point. First index is 0.
00144                          * \exception Throws std::exception on index out of bound.
00145                          */
00146                         virtual void  setPoint(size_t index,CPoint2D &p);
00147 
00148                         /** Changes a given point from map, as a 3D point. First index is 0.
00149                          * \exception Throws std::exception on index out of bound.
00150                          */
00151                         virtual void  setPoint(size_t index,CPoint3D &p);
00152 
00153                         /** Changes a given point from map. First index is 0.
00154                          * \exception Throws std::exception on index out of bound.
00155                          */
00156                         virtual void  setPoint(size_t index,float x, float y);
00157 
00158                         /** Changes a given point from map. First index is 0.
00159                          * \exception Throws std::exception on index out of bound.
00160                          */
00161                         virtual void  setPoint(size_t index,float x, float y, float z);
00162 
00163                         /** Changes a given point from map. First index is 0.
00164                          * \exception Throws std::exception on index out of bound.
00165                          */
00166                         void  setPoint(size_t index,float x, float y, float z, float R, float G, float B);
00167 
00168                         /** Changes just the color of a given point from the map. First index is 0.
00169                          * \exception Throws std::exception on index out of bound.
00170                          */
00171                         void  setPointColor(size_t index,float R, float G, float B);
00172 
00173                         /** Provides a way to insert individual points into the map:
00174                           */
00175                         void  insertPoint( float x, float y, float z = 0 );
00176 
00177                         /** Provides a way to insert individual points into the map:
00178                           */
00179                         void  insertPoint( CPoint3D p );
00180 
00181                         /** Remove from the map the points marked in a bool's array as "true".
00182                           *
00183                           * \exception std::exception If mask size is not equal to points count.
00184                           */
00185                         void  applyDeletionMask( std::vector<bool> &mask );
00186 
00187                         /** Adds a new point given its coordinates and color (colors range is [0,1])
00188                           */
00189                         void  insertPoint( float x, float y, float z, float R, float G, float B );
00190 
00191                         /** Retrieves a point and its color (colors range is [0,1])
00192                           */
00193                         virtual void  getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const;
00194 
00195                         /** Retrieves a point color (colors range is [0,1])
00196                           */
00197                         void  getPointColor( size_t index, float &R, float &G, float &B ) const;
00198 
00199                         /** Returns true if the point map has a color field for each point */
00200                         virtual bool hasColorPoints() const { return true; }
00201 
00202                         /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
00203                           *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
00204                           */
00205                         void  auxParticleFilterCleanUp();
00206 
00207                         /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
00208                           *  This is useful for situations where it is approximately known the final size of the map. This method is more
00209                           *  efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
00210                           */
00211                         void reserve(size_t newLength);
00212 
00213                         /** Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros).
00214                           * RGB field of all points will be set to white color.
00215                           * \tparam VECTOR can be mrpt::vector_float or std::vector<float> or any other column or row Eigen::Matrix.
00216                           */
00217                         template <typename VECTOR>
00218                         inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR())
00219                         {
00220                                 const size_t N = X.size();
00221                                 ASSERT_EQUAL_(X.size(),Y.size())
00222                                 ASSERT_(Z.size()==0 || Z.size()==X.size())
00223                                 x.resize(N); y.resize(N); z.resize(N);
00224                                 const bool z_valid = Z.empty();
00225                                 if (z_valid) for (size_t i=0;i<N;i++) { this->x[i]=X[i]; this->y[i]=Y[i]; this->z[i]=Z[i]; }
00226                                 else         for (size_t i=0;i<N;i++) { this->x[i]=X[i]; this->y[i]=Y[i]; this->z[i]=0; }
00227                                 pointWeight.assign(N,1);
00228                                 m_color_R.assign(N,1);
00229                                 m_color_G.assign(N,1);
00230                                 m_color_B.assign(N,1);
00231                                 mark_as_modified();
00232                         }
00233 
00234                         /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */
00235                         virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z)
00236                         {
00237                                 setAllPointsTemplate(X,Y,Z);
00238                         }
00239 
00240                         /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */
00241                         virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y)
00242                         {
00243                                 setAllPointsTemplate(X,Y);
00244                         }
00245 
00246 
00247 
00248                         /** Override of the default 3D scene builder to account for the individual points' color.
00249                           * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
00250                           */
00251                         virtual void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;
00252 
00253                         /** Colour a set of points from a CObservationImage and the global pose of the robot
00254                           */
00255                         bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose );
00256 
00257                         /** The choices for coloring schemes:
00258                           *             - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max.
00259                           *     - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available.
00260                           * \sa TColourOptions
00261                           */
00262                         enum TColouringMethod
00263                         {
00264                                 cmFromHeightRelativeToSensor = 0,
00265                                 cmFromHeightRelativeToSensorJet = 0,
00266                                 cmFromHeightRelativeToSensorGray = 1,
00267                                 cmFromIntensityImage = 2
00268                         };
00269 
00270                         /** The definition of parameters for generating colors from laser scans */
00271                          struct MAPS_IMPEXP TColourOptions : public utils::CLoadableOptions
00272                          {
00273                                 /** Initilization of default parameters */
00274                                 TColourOptions( );
00275                                 virtual ~TColourOptions() {}
00276                                 /** See utils::CLoadableOptions
00277                                   */
00278                                 void  loadFromConfigFile(
00279                                         const mrpt::utils::CConfigFileBase  &source,
00280                                         const std::string &section);
00281 
00282                                 /** See utils::CLoadableOptions
00283                                   */
00284                                 void  dumpToTextStream(CStream  &out) const;
00285 
00286                                 TColouringMethod        scheme;
00287                                 float                           z_min,z_max;
00288                                 float                           d_max;
00289                          };
00290 
00291                          TColourOptions colorScheme;    //!< The options employed when inserting laser scans in the map.
00292 
00293                          void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value
00294 
00295                         /** @name Filter-by-height stuff
00296                                 @{ */
00297                 public:
00298                         /** Enable/disable the filter-by-height functionality \sa setHeightFilterLevels \note Default upon construction is disabled. */
00299                         inline void enableFilterByHeight(bool enable=true) { bFilterByHeight=enable; }
00300                         /** Return whether filter-by-height is enabled \sa enableFilterByHeight */
00301                         inline bool isFilterByHeightEnabled() const  { return bFilterByHeight; }
00302 
00303                         /** Set the min/max Z levels for points to be actually inserted in the map (only if \a enableFilterByHeight() was called before). */
00304                         inline void setHeightFilterLevels(const double _z_min, const double _z_max) { z_min=_z_min; z_max=_z_max; }
00305                         /** Get the min/max Z levels for points to be actually inserted in the map \sa enableFilterByHeight, setHeightFilterLevels */
00306                         inline void getHeightFilterLevels(double &_z_min, double &_z_max) const { _z_min=z_min; _z_max=z_max; }
00307 
00308                 private:
00309                          /** The minimum and maximum height for a certain laser scan to be inserted into this map
00310                                 \sa bFilterByHeight
00311                           */
00312                         double z_min, z_max;
00313 
00314                          /** Whether or not filter the input points by height
00315                             \sa z_min, z_max
00316                          */
00317                         bool bFilterByHeight;
00318 
00319                         /** @} */
00320 
00321                 }; // End of class def.
00322 
00323         } // End of namespace
00324 } // End of namespace
00325 
00326 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011