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 §ion); 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 §ion); 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 ¢er, 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.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |