Main MRPT website > C++ reference
MRPT logo

CPointsMap.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 CPOINTSMAP_H
00029 #define CPOINTSMAP_H
00030 
00031 #include <mrpt/slam/CMetricMap.h>
00032 #include <mrpt/utils/CSerializable.h>
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/utils/CLoadableOptions.h>
00035 #include <mrpt/utils/safe_pointers.h>
00036 
00037 #include <mrpt/math/KDTreeCapable.h>
00038 
00039 #include <mrpt/poses/CPoint2D.h>
00040 #include <mrpt/math/lightweight_geom_data.h>
00041 
00042 #include <mrpt/maps/link_pragmas.h>
00043 
00044 namespace mrpt
00045 {
00046 namespace slam
00047 {
00048         using namespace mrpt::poses;
00049         using namespace mrpt::math;
00050 
00051         class CObservation2DRangeScan;
00052         class CSimplePointsMap;
00053         class CMultiMetricMap;
00054         class CColouredPointsMap;
00055         class COccupancyGridMap2D;
00056 
00057         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CPointsMap , CMetricMap, MAPS_IMPEXP )
00058 
00059         /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
00060          *  This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap.
00061          * \sa CMetricMap, CPoint, mrpt::utils::CSerializable
00062          */
00063         class MAPS_IMPEXP CPointsMap : public CMetricMap, public mrpt::utils::KDTreeCapable
00064         {
00065                 friend class CMultiMetricMap;
00066                 friend class CMultiMetricMapPDF;
00067                 friend class CSimplePointsMap;
00068                 friend class CColouredPointsMap;
00069                 friend class COccupancyGridMap2D;
00070 
00071                 // This must be added to any CSerializable derived class:
00072                 DEFINE_VIRTUAL_SERIALIZABLE( CPointsMap )
00073 
00074          protected:
00075                  /** The points coordinates
00076                   */
00077                  std::vector<float>             x,y,z;
00078 
00079                  /** The points weights
00080                   */
00081                  std::vector<uint32_t>          pointWeight;
00082 
00083                  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
00084                    * \sa getLargestDistanceFromOrigin
00085                    */
00086                  mutable float  m_largestDistanceFromOrigin;
00087 
00088                  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
00089                    * \sa getLargestDistanceFromOrigin
00090                    */
00091                  mutable bool   m_largestDistanceFromOriginIsUpdated;
00092 
00093                  void mark_as_modified() const; //!< Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and such.
00094 
00095 
00096 
00097                 /** @name Virtual methods that MUST be implemented by children classes of KDTreeCapable
00098                         @{ */
00099                 /** Must return the number of data points */
00100                 virtual size_t kdtree_get_point_count() const;
00101 
00102                 /** Must fill out the data points in "data", such as the i'th point will be stored in (data[i][0],...,data[i][nDims-1]). */
00103                 virtual void kdtree_fill_point_data(ANNpointArray &data, const int nDims) const;
00104 
00105                 /** @} */
00106 
00107          public:
00108                  /** Constructor
00109                    */
00110                  CPointsMap();
00111 
00112                  /** Virtual destructor.
00113                    */
00114                  virtual ~CPointsMap();
00115 
00116 
00117 
00118                 /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
00119                   */
00120                 virtual float squareDistanceToClosestCorrespondence(
00121                         float   x0,
00122                         float   y0 ) const;
00123 
00124                 inline float squareDistanceToClosestCorrespondenceT(const TPoint2D &p0) const   {
00125                         return squareDistanceToClosestCorrespondence(static_cast<float>(p0.x),static_cast<float>(p0.y));
00126                 }
00127 
00128 
00129                  /** With this struct options are provided to the observation insertion process.
00130                   * \sa CObservation::insertIntoPointsMap
00131                   */
00132                  struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions
00133                  {
00134                         /** Initilization of default parameters */
00135                         TInsertionOptions( );
00136                         /** See utils::CLoadableOptions */
00137                         void  loadFromConfigFile(const mrpt::utils::CConfigFileBase  &source,const std::string &section);
00138                         /** See utils::CLoadableOptions */
00139                         void  dumpToTextStream(CStream  &out) const;
00140 
00141                         float   minDistBetweenLaserPoints;   //!< The minimum distance between points (in 3D): If two points are too close, one of them is not inserted into the map. Default is 0.02 meters.
00142                         bool    addToExistingPointsMap;      //!< Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded, clearing all previous content. Default is false.
00143                         bool    also_interpolate;            //!< If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" intervals (Default is false).
00144                         bool    disableDeletion;             //!< If set to false (default=true) points in the same plane as the inserted scan and inside the free space, are erased: i.e. they don't exist yet.
00145                         bool    fuseWithExisting;            //!< If set to true (default=false), inserted points are "fused" with previously existent ones. This shrink the size of the points map, but its slower.
00146                         bool    isPlanarMap;                 //!< If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default value is false, thus 3D maps are generated). \sa  horizontalTolerance
00147                         float   horizontalTolerance;         //!< The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
00148                         float   maxDistForInterpolatePoints; //!< The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)
00149 
00150                  };
00151 
00152                 TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map
00153 
00154                  /** Options used when evaluating "computeObservationLikelihood" in the derived classes.
00155                   * \sa CObservation::computeObservationLikelihood
00156                   */
00157                  struct MAPS_IMPEXP TLikelihoodOptions: public utils::CLoadableOptions
00158                  {
00159                         /** Initilization of default parameters
00160                          */
00161                         TLikelihoodOptions( );
00162                         virtual ~TLikelihoodOptions() {}
00163 
00164                         /** See utils::CLoadableOptions */
00165                         void  loadFromConfigFile(
00166                                 const mrpt::utils::CConfigFileBase  &source,
00167                                 const std::string &section);
00168 
00169                         /** See utils::CLoadableOptions */
00170                         void  dumpToTextStream(CStream  &out) const;
00171 
00172                         void writeToStream(CStream &out) const;         //!< Binary dump to stream - for usage in derived classes' serialization
00173                         void readFromStream(CStream &in);                       //!< Binary dump to stream - for usage in derived classes' serialization
00174 
00175                         double          sigma_dist; //!< Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0.5meters)
00176                         double          max_corr_distance; //!< Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point has a minimum (but very small) likelihood to avoid underflows (default=1.0 meters)
00177                         uint32_t        decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=10)
00178                  };
00179 
00180                  TLikelihoodOptions  likelihoodOptions;
00181 
00182                  /** Virtual assignment operator, to be implemented in derived classes.
00183                    */
00184                  virtual void  copyFrom(const CPointsMap &obj) = 0;
00185 
00186                 /** Insert the contents of another map into this one, fusing the previous content with the new one.
00187                  *    This means that points very close to existing ones will be "fused", rather than "added". This prevents
00188                  *     the unbounded increase in size of these class of maps.
00189                  *              NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
00190                  *               before calling this method.
00191                  * \param otherMap The other map whose points are to be inserted into this one.
00192                  * \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.
00193                  * \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.
00194                  */
00195                 virtual void  fuseWith(
00196                         CPointsMap                      *otherMap,
00197                         float                           minDistForFuse  = 0.02f,
00198                         std::vector<bool>       *notFusedPoints = NULL) = 0;
00199 
00200                 /** Transform the range scan into a set of cartessian coordinated
00201                   *      points. The options in "insertionOptions" are considered in this method.
00202                   * \param rangeScan The scan to be inserted into this map
00203                   * \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.
00204                   *
00205                   *   NOTE: Only ranges marked as "valid=true" in the observation will be inserted
00206                   *
00207                   * \sa CObservation2DRangeScan
00208                   */
00209                 virtual void  loadFromRangeScan(
00210                                 const CObservation2DRangeScan &rangeScan,
00211                                 const CPose3D                             *robotPose = NULL ) = 0;
00212 
00213                 /** Load from a text file. In each line there are a point coordinates.
00214                  *   Returns false if any error occured, true elsewere.
00215                  */
00216                 virtual bool  load2D_from_text_file(std::string file) = 0;
00217 
00218                 /** Load from a text file. In each line there are a point coordinates.
00219                  *   Returns false if any error occured, true elsewere.
00220                  */
00221                 virtual bool  load3D_from_text_file(std::string file) = 0;
00222 
00223                 /**  Save to a text file. In each line there are a point coordinates.
00224                  *              Returns false if any error occured, true elsewere.
00225                  */
00226                 bool  save2D_to_text_file(const std::string &file) const;
00227 
00228                 /** Save to a text file. In each line there are a point coordinates.
00229                  *     Returns false if any error occured, true elsewere.
00230                  */
00231                 bool  save3D_to_text_file(const std::string &file)const;
00232 
00233                 /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
00234                   */
00235                 void  saveMetricMapRepresentationToFile(
00236                         const std::string       &filNamePrefix
00237                         )const
00238                 {
00239                         std::string             fil( filNamePrefix + std::string(".txt") );
00240                         save3D_to_text_file( fil );
00241                 }
00242 
00243 
00244                 /** Returns the number of stored points in the map.
00245                  */
00246                 size_t size() const;
00247 
00248                 /** Returns the number of stored points in the map (DEPRECATED, use "size()" instead better)
00249                  */
00250                 size_t getPointsCount() const;
00251 
00252                 /** Access to a given point from map, as a 2D point. First index is 0.
00253                  * \return The return value is the weight of the point (the times it has been fused)
00254                  * \exception Throws std::exception on index out of bound.
00255                  */
00256                 unsigned long  getPoint(size_t index,CPoint2D &p) const;
00257 
00258                 /** Access to a given point from map, as a 3D point. First index is 0.
00259                  * \return The return value is the weight of the point (the times it has been fused)
00260                  * \exception Throws std::exception on index out of bound.
00261                  */
00262                 unsigned long  getPoint(size_t index,CPoint3D &p) const;
00263 
00264                 /** Access to a given point from map, as a 3D point. First index is 0.
00265                  * \return The return value is the weight of the point (the times it has been fused)
00266                  * \exception Throws std::exception on index out of bound.
00267                  */
00268                 unsigned long  getPoint(size_t index,mrpt::math::TPoint3D &p) const;
00269 
00270                 /** Access to a given point from map, as a 2D point. First index is 0.
00271                  * \return The return value is the weight of the point (the times it has been fused)
00272                  * \exception Throws std::exception on index out of bound.
00273                  */
00274                 unsigned long  getPoint(size_t index,mrpt::math::TPoint2D &p) const;
00275 
00276                 /** Access to a given point from map. First index is 0.
00277                  * \return The return value is the weight of the point (the times it has been fused)
00278                  * \exception Throws std::exception on index out of bound.
00279                  */
00280                 unsigned long  getPoint(size_t index,float &x,float &y) const;
00281 
00282                 /** Access to a given point from map. First index is 0.
00283                  * \return The return value is the weight of the point (the times it has been fused)
00284                  * \exception Throws std::exception on index out of bound.
00285                  */
00286                 unsigned long  getPoint(size_t index,float &x,float &y,float &z) const;
00287 
00288 
00289                 /** Access to a given point from map, and its colors, if the map defines them (othersise, R=G=B=1.0). First index is 0.
00290                  * \return The return value is the weight of the point (the times it has been fused)
00291                  * \exception Throws std::exception on index out of bound.
00292                  */
00293                 virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const
00294                 {
00295                         getPoint(index,x,y,z);
00296                         R=G=B=1;
00297                 }
00298 
00299                 /** Returns true if the point map has a color field for each point */
00300                 virtual bool hasColorPoints() const { return false; }
00301 
00302                 /** Changes a given point from map, as a 2D point. First index is 0.
00303                  * \exception Throws std::exception on index out of bound.
00304                  */
00305                 virtual void  setPoint(size_t index,CPoint2D &p)=0;
00306 
00307                 /** Changes a given point from map, as a 3D point. First index is 0.
00308                  * \exception Throws std::exception on index out of bound.
00309                  */
00310                 virtual void  setPoint(size_t index,CPoint3D &p)=0;
00311 
00312                 /** Changes a given point from map. First index is 0.
00313                  * \exception Throws std::exception on index out of bound.
00314                  */
00315                 virtual void  setPoint(size_t index,float x, float y)=0;
00316 
00317                 /** Changes a given point from map. First index is 0.
00318                  * \exception Throws std::exception on index out of bound.
00319                  */
00320                 virtual void  setPoint(size_t index,float x, float y, float z)=0;
00321 
00322                 /** Provides a direct access to points buffer, or NULL if there is no points in the map.
00323                   */
00324                 void  getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const;
00325 
00326                 /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
00327                 inline const std::vector<float> & getPointsBufferRef_x() const { return x; }
00328                 /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
00329                 inline const std::vector<float> & getPointsBufferRef_y() const { return y; }
00330                 /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
00331                 inline const std::vector<float> & getPointsBufferRef_z() const { return z; }
00332 
00333                 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
00334                   * If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.
00335                   * \sa getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z
00336                   * \tparam VECTOR can be std::vector<float or double> or any row/column Eigen::Array or Eigen::Matrix (this includes mrpt::vector_float and mrpt::vector_double).
00337                   */
00338                 template <class VECTOR>
00339                 void  getAllPoints( VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation = 1 ) const
00340                 {
00341                         MRPT_START
00342                         ASSERT_(decimation>0)
00343                         const size_t Nout = x.size() / decimation;
00344                         xs.resize(Nout);
00345                         ys.resize(Nout);
00346                         zs.resize(Nout);
00347                         size_t idx_in, idx_out;
00348                         for (idx_in=0,idx_out=0;idx_out<Nout;idx_in+=decimation,++idx_out)
00349                         {
00350                                 xs[idx_out]=x[idx_in];
00351                                 ys[idx_out]=y[idx_in];
00352                                 zs[idx_out]=z[idx_in];
00353                         }
00354                         MRPT_END
00355                 }
00356 
00357                 inline void getAllPoints(std::vector<TPoint3D> &ps,size_t decimation=1) const   {
00358                         std::vector<float> dmy1,dmy2,dmy3;
00359                         getAllPoints(dmy1,dmy2,dmy3,decimation);
00360                         ps.resize(dmy1.size());
00361                         for (size_t i=0;i<dmy1.size();i++)      {
00362                                 ps[i].x=static_cast<double>(dmy1[i]);
00363                                 ps[i].y=static_cast<double>(dmy2[i]);
00364                                 ps[i].z=static_cast<double>(dmy3[i]);
00365                         }
00366                 }
00367 
00368                 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
00369                   * If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.
00370                   * \sa setAllPoints
00371                   */
00372                 void  getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const;
00373 
00374                 inline void getAllPoints(std::vector<TPoint2D> &ps,size_t decimation=1) const   {
00375                         std::vector<float> dmy1,dmy2;
00376                         getAllPoints(dmy1,dmy2,decimation);
00377                         ps.resize(dmy1.size());
00378                         for (size_t i=0;i<dmy1.size();i++)      {
00379                                 ps[i].x=static_cast<double>(dmy1[i]);
00380                                 ps[i].y=static_cast<double>(dmy2[i]);
00381                         }
00382                 }
00383 
00384                 /** Provides a way to insert individual points into the map */
00385                 virtual void  insertPoint( float x, float y, float z = 0 ) = 0;
00386 
00387                 /** Provides a way to insert individual points into the map */
00388                 inline void  insertPoint( const CPoint3D &p ) {
00389                         insertPoint(p.x(),p.y(),p.z());
00390                 }
00391 
00392                 /** Provides a way to insert individual points into the map */
00393                 inline void  insertPoint( const mrpt::math::TPoint3D &p ) {
00394                         insertPoint(p.x,p.y,p.z);
00395                 }
00396 
00397                 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
00398                   *  This is useful for situations where it is approximately known the final size of the map. This method is more
00399                   *  efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
00400                   */
00401                 virtual void reserve(size_t newLength) = 0;
00402 
00403                 /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */
00404                 virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z) = 0;
00405                 /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */
00406                 virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y) = 0;
00407 
00408                 /** Delete points out of the given "z" axis range have been removed.
00409                   */
00410                 void  clipOutOfRangeInZ(float zMin, float zMax);
00411 
00412                 /** Delete points which are more far than "maxRange" away from the given "point".
00413                   */
00414                 void  clipOutOfRange(const CPoint2D     &point, float maxRange);
00415 
00416                 /** Remove from the map the points marked in a bool's array as "true".
00417                   *
00418                   * \exception std::exception If mask size is not equal to points count.
00419                   */
00420                 virtual void  applyDeletionMask( std::vector<bool> &mask ) = 0;
00421 
00422                 /** Computes the matchings between this and another 2D/3D points map.
00423                    This includes finding:
00424                                 - The set of points pairs in each map
00425                                 - The mean squared distance between corresponding pairs.
00426                    This method is the most time critical one into the ICP algorithm.
00427 
00428                  * \param  otherMap                                       [IN] The other map to compute the matching with.
00429                  * \param  otherMapPose                           [IN] The pose of the other map as seen from "this".
00430                  * \param  maxDistForCorrespondence [IN] Maximum 2D distance between two points to be matched.
00431                  * \param  maxAngularDistForCorrespondence [IN] Maximum angular distance in radians to allow far points to be matched.
00432                  * \param  angularDistPivotPoint      [IN] The point from which to measure the "angular distances"
00433                  * \param  correspondences                        [OUT] The detected matchings pairs.
00434                  * \param  correspondencesRatio           [OUT] The number of correct correspondences.
00435                  * \param  sumSqrDist                             [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
00436                  * \param  covariance                             [OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired.
00437                  * \param  onlyKeepTheClosest             [OUT] Returns only the closest correspondence (default=false)
00438                  *
00439                  * \sa computeMatching3DWith
00440                  */
00441                 void  computeMatchingWith2D(
00442                                 const CMetricMap                                                *otherMap,
00443                                 const CPose2D                                                   &otherMapPose,
00444                                 float                                                                   maxDistForCorrespondence,
00445                                 float                                                                   maxAngularDistForCorrespondence,
00446                                 const CPose2D                                                   &angularDistPivotPoint,
00447                                 TMatchingPairList                                               &correspondences,
00448                                 float                                                                   &correspondencesRatio,
00449                                 float                                                                   *sumSqrDist     = NULL,
00450                                 bool                                                                    onlyKeepTheClosest = false,
00451                                 bool                                                                    onlyUniqueRobust = false ) const;
00452 
00453                 /** Computes the matchings between this and another 3D points map - method used in 3D-ICP.
00454                    This method finds the set of point pairs in each map.
00455 
00456                    The method is the most time critical one into the ICP algorithm.
00457 
00458                  * \param  otherMap                                       [IN] The other map to compute the matching with.
00459                  * \param  otherMapPose                           [IN] The pose of the other map as seen from "this".
00460                  * \param  maxDistForCorrespondence   [IN] Maximum 2D linear distance between two points to be matched.
00461                  * \param  maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
00462                  * \param  angularDistPivotPoint      [IN] The point used to calculate distances from in both maps.
00463                  * \param  correspondences                        [OUT] The detected matchings pairs.
00464                  * \param  correspondencesRatio           [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
00465                  * \param  sumSqrDist                             [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
00466                  * \param  onlyKeepTheClosest         [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
00467                  *
00468                  * \sa compute3DMatchingRatio
00469                  */
00470                 void  computeMatchingWith3D(
00471                         const CMetricMap                                                *otherMap,
00472                         const CPose3D                                                   &otherMapPose,
00473                         float                                                                   maxDistForCorrespondence,
00474                         float                                                                   maxAngularDistForCorrespondence,
00475                         const CPoint3D                                                  &angularDistPivotPoint,
00476                         TMatchingPairList                                               &correspondences,
00477                         float                                                                   &correspondencesRatio,
00478                         float                                                                   *sumSqrDist     = NULL,
00479                         bool                                                                    onlyKeepTheClosest = true,
00480                         bool                                                                    onlyUniqueRobust = false ) const;
00481 
00482 
00483                 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
00484                   */
00485                 void   changeCoordinatesReference(const CPose2D &b);
00486 
00487                 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
00488                   */
00489                 void   changeCoordinatesReference(const CPose3D &b);
00490 
00491                 /** Copy all the points from "other" map to "this", replacing each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
00492                   */
00493                 void   changeCoordinatesReference(const CPointsMap &other, const CPose3D &b);
00494 
00495                 /** Returns true if the map is empty/no observation has been inserted.
00496                    */
00497                 virtual bool  isEmpty() const;
00498 
00499                 /** STL-like method to check whether the map is empty: */
00500                 inline bool  empty() const { return isEmpty(); }
00501 
00502                 /** Returns a 3D object representing the map.
00503                   *  The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B
00504                   * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
00505                   */
00506                 virtual void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;
00507 
00508 
00509                 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
00510                  *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
00511                  * \param  otherMap                                       [IN] The other map to compute the matching with.
00512                  * \param  otherMapPose                           [IN] The 6D pose of the other map as seen from "this".
00513                  * \param  minDistForCorr                         [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
00514                  * \param  minMahaDistForCorr             [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
00515                  *
00516                  * \return The matching ratio [0,1]
00517                  * \sa computeMatchingWith2D
00518                  */
00519                 float  compute3DMatchingRatio(
00520                                 const CMetricMap                                                *otherMap,
00521                                 const CPose3D                                                   &otherMapPose,
00522                                 float                                                                   minDistForCorr = 0.10f,
00523                                 float                                                                   minMahaDistForCorr = 2.0f
00524                                 ) const;
00525 
00526                 /** This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one).
00527                   */
00528                 float  getLargestDistanceFromOrigin() const;
00529 
00530                 /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points. */
00531                 void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const;
00532 
00533                 inline void boundingBox(TPoint3D &pMin,TPoint3D &pMax) const    {
00534                         float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6;
00535                         boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6);
00536                         pMin.x=dmy1;
00537                         pMin.y=dmy2;
00538                         pMin.z=dmy3;
00539                         pMax.x=dmy4;
00540                         pMax.y=dmy5;
00541                         pMax.z=dmy6;
00542                 }
00543 
00544                 void extractCylinder( const CPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap );
00545 
00546 
00547                 /** The color [0,1] of points when extracted from getAs3DObject (default=blue) */
00548                 static float COLOR_3DSCENE_R;
00549                 static float COLOR_3DSCENE_G;
00550                 static float COLOR_3DSCENE_B;
00551 
00552 
00553                 /** Computes the likelihood of taking a given observation from a given pose in the world being modeled with this map.
00554                  * \param takenFrom The robot's pose the observation is supposed to be taken from.
00555                  * \param obs The observation.
00556                  * \return This method returns a likelihood in the range [0,1].
00557                  *
00558                  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF
00559                  * \note In CPointsMap this method is virtual so it can be redefined in derived classes, if desired.
00560                  */
00561                 virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00562 
00563         }; // End of class def.
00564 
00565         } // End of namespace
00566 
00567         namespace global_settings
00568         {
00569                 /** The size of points when exporting with getAs3DObject() (default=3.0)
00570                   * Affects to:
00571                   *             - mrpt::slam::CPointsMap and all its children classes.
00572                   */
00573                 extern MAPS_IMPEXP float POINTSMAPS_3DOBJECT_POINTSIZE;
00574         }
00575 } // End of namespace
00576 
00577 #endif



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