Main MRPT website > C++ reference
MRPT logo

CSimplePointsMap.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 CSimplePointsMap_H
00029 #define CSimplePointsMap_H
00030 
00031 #include <mrpt/slam/CPointsMap.h>
00032 #include <mrpt/slam/CObservation2DRangeScan.h>
00033 #include <mrpt/slam/CObservation3DRangeScan.h>
00034 #include <mrpt/utils/CSerializable.h>
00035 #include <mrpt/math/CMatrix.h>
00036 
00037 #include <mrpt/maps/link_pragmas.h>
00038 
00039 namespace mrpt
00040 {
00041         namespace slam
00042         {
00043 
00044                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CSimplePointsMap , CPointsMap, MAPS_IMPEXP )
00045 
00046                 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
00047                  *    This class stores the coordinates (x,y,z) and a "weight", or counter of how many times that point has been seen, used only if points fusion is enabled in the options structure.
00048                  * \sa CMetricMap, CPoint, mrpt::utils::CSerializable
00049                  */
00050                 class MAPS_IMPEXP CSimplePointsMap : public CPointsMap
00051                 {
00052                         // This must be added to any CSerializable derived class:
00053                         DEFINE_SERIALIZABLE( CSimplePointsMap )
00054                  public:
00055 
00056                          /** Destructor
00057                            */
00058                          virtual ~CSimplePointsMap();
00059 
00060                          /** Default constructor
00061                           */
00062                          CSimplePointsMap();
00063 
00064                          /** Copy operator
00065                           */
00066                          void  copyFrom(const CPointsMap &obj);
00067 
00068                         /** Transform the range scan into a set of cartesian coordinated
00069                           *      points. The options in "insertionOptions" are considered in this method.
00070                           * \param rangeScan The scan to be inserted into this map
00071                           * \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.
00072                           *
00073                           *   NOTE: Only ranges marked as "valid=true" in the observation will be inserted
00074                           *
00075                           * \sa CObservation2DRangeScan
00076                           */
00077                         void  loadFromRangeScan(
00078                                         const CObservation2DRangeScan &rangeScan,
00079                                         const CPose3D                             *robotPose = NULL );
00080 
00081                         /** Enter the set of cartesian coordinated points from the 3D range scan into
00082                           *      the map. The options in "insertionOptions" are considered in this method.
00083                           * \param rangeScan The 3D scan to be inserted into this map
00084                           * \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.
00085                           *
00086                           *   NOTE: Only ranges marked as "valid=true" in the observation will be inserted
00087                           *
00088                           * \sa CObservation3DRangeScan
00089                           */
00090                         void  loadFromRangeScan(
00091                                         const CObservation3DRangeScan &rangeScan,
00092                                         const CPose3D                             *robotPose = NULL );
00093 
00094                         /** Load from a text file. In each line there are a point coordinates.
00095                          *   Returns false if any error occured, true elsewere.
00096                          */
00097                         bool  load2D_from_text_file(std::string file);
00098 
00099                         /** Load from a text file. In each line there are a point coordinates.
00100                          *   Returns false if any error occured, true elsewere.
00101                          */
00102                         bool  load3D_from_text_file(std::string file);
00103 
00104 
00105                         /** Insert the contents of another map into this one, fusing the previous content with the new one.
00106                          *    This means that points very close to existing ones will be "fused", rather than "added". This prevents
00107                          *     the unbounded increase in size of these class of maps.
00108                          *              NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
00109                          *               before calling this method.
00110                          * \param otherMap The other map whose points are to be inserted into this one.
00111                          * \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.
00112                          * \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.
00113                          * \sa insertAnotherMap
00114                          */
00115                         void  fuseWith( CPointsMap                      *otherMap,
00116                                                                                 float                           minDistForFuse  = 0.02f,
00117                                                                                 std::vector<bool>       *notFusedPoints = NULL);
00118 
00119                         /** Insert the contents of another map into this one, without fusing close points.
00120                          * \param otherMap The other map whose points are to be inserted into this one.
00121                          * \param otherPose The pose of the other map in the coordinates of THIS map
00122                          * \sa fuseWith
00123                          */
00124                         void  insertAnotherMap(
00125                                                                                 CPointsMap                      *otherMap,
00126                                                                                 const CPose2D           &otherPose);
00127 
00128                         /** Changes a given point from map, as a 2D point. First index is 0.
00129                          * \exception Throws std::exception on index out of bound.
00130                          */
00131                         virtual void  setPoint(size_t index,CPoint2D &p);
00132 
00133                         /** Changes a given point from map, as a 3D point. First index is 0.
00134                          * \exception Throws std::exception on index out of bound.
00135                          */
00136                         virtual void  setPoint(size_t index,CPoint3D &p);
00137 
00138                         /** Changes a given point from map. First index is 0.
00139                          * \exception Throws std::exception on index out of bound.
00140                          */
00141                         virtual void  setPoint(size_t index,float x, float y);
00142 
00143                         /** Changes a given point from map. First index is 0.
00144                          * \exception Throws std::exception on index out of bound.
00145                          */
00146                         virtual void  setPoint(size_t index,float x, float y, float z);
00147 
00148                         /** Provides a way to insert individual points into the map:
00149                           */
00150                         void  insertPoint( float x, float y, float z = 0 );
00151 
00152                         /** Provides a way to insert individual points into the map:
00153                           */
00154                         void  insertPoint( const mrpt::math::TPoint3D &new_pnt ) {
00155                                 this->insertPoint(new_pnt.x,new_pnt.y,new_pnt.z);
00156                         }
00157 
00158                         /** Remove from the map the points marked in a bool's array as "true".
00159                           *
00160                           * \exception std::exception If mask size is not equal to points count.
00161                           */
00162                         void  applyDeletionMask( std::vector<bool> &mask );
00163 
00164                         /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
00165                           *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
00166                           */
00167                         void  auxParticleFilterCleanUp();
00168 
00169                         /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
00170                           *  This is useful for situations where it is approximately known the final size of the map. This method is more
00171                           *  efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
00172                           */
00173                         void reserve(size_t newLength);
00174 
00175                         /** 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).
00176                           * \tparam VECTOR can be mrpt::vector_float or std::vector<float> or any other column or row Eigen::Matrix.
00177                           */
00178                         template <typename VECTOR>
00179                         inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR())
00180                         {
00181                                 const size_t N = X.size();
00182                                 ASSERT_EQUAL_(X.size(),Y.size())
00183                                 ASSERT_(Z.size()==0 || Z.size()==X.size())
00184                                 x.resize(N); y.resize(N); z.resize(N);
00185                                 const bool z_valid = Z.empty();
00186                                 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]; }
00187                                 else         for (size_t i=0;i<N;i++) { this->x[i]=X[i]; this->y[i]=Y[i]; this->z[i]=0; }
00188                                 pointWeight.assign(N,1);
00189                                 mark_as_modified();
00190                         }
00191 
00192                         /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */
00193                         virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z)
00194                         {
00195                                 setAllPointsTemplate(X,Y,Z);
00196                         }
00197 
00198                         /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */
00199                         virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y)
00200                         {
00201                                 setAllPointsTemplate(X,Y);
00202                         }
00203 
00204                         /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it. 
00205                                 * Otherwise, return NULL
00206                                 */
00207                         virtual const CSimplePointsMap * getAsSimplePointsMap() const { return this; }
00208                         virtual       CSimplePointsMap * getAsSimplePointsMap()       { return this; }
00209 
00210                 protected:
00211                         /** Clear the map, erasing all the points.
00212                          */
00213                         virtual void  internal_clear();
00214 
00215                          /** Insert the observation information into this map. This method must be implemented
00216                           *    in derived classes.
00217                           * \param obs The observation
00218                           * \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)
00219                           *
00220                           * \sa CObservation::insertObservationInto
00221                           */
00222                         bool  internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00223 
00224                 }; // End of class def.
00225 
00226         } // End of namespace
00227 } // End of namespace
00228 
00229 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011