00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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/poses/CPoint2D.h> 00038 #include <mrpt/math/lightweight_geom_data.h> 00039 00040 #include <mrpt/otherlibs/ann/ANN.h> // ANN: for kd-tree 00041 00042 00043 namespace mrpt 00044 { 00045 namespace slam 00046 { 00047 using namespace mrpt::poses; 00048 using namespace mrpt::math; 00049 00050 class CObservation2DRangeScan; 00051 class CSimplePointsMap; 00052 class CMultiMetricMap; 00053 class CColouredPointsMap; 00054 class COccupancyGridMap2D; 00055 00056 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors. 00057 * This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap. 00058 * \sa CMetricMap, CPoint, mrpt::utils::CSerializable 00059 */ 00060 class MRPTDLLIMPEXP CPointsMap : public CMetricMap 00061 { 00062 friend class CMultiMetricMap; 00063 friend class CMultiMetricMapPDF; 00064 friend class CSimplePointsMap; 00065 friend class CColouredPointsMap; 00066 friend class COccupancyGridMap2D; 00067 00068 // This must be added to any CSerializable derived class: 00069 DEFINE_VIRTUAL_SERIALIZABLE( CPointsMap ) 00070 00071 protected: 00072 /** Only for objects stored into a CMultiMetricMap (among a grid map). Otherwise, "parent" will be NULL. 00073 * This wrapper class is for managing elegantly copy constructors & = operators in parent class. 00074 */ 00075 utils::safe_ptr<CMultiMetricMap> m_parent; 00076 00077 /** The points coordinates 00078 */ 00079 std::vector<float> x,y,z; 00080 00081 /** The points weights 00082 */ 00083 std::vector<uint32_t> pointWeight; 00084 00085 /** Auxiliary variables used in "getLargestDistanceFromOrigin" 00086 * \sa getLargestDistanceFromOrigin 00087 */ 00088 mutable float m_largestDistanceFromOrigin; 00089 00090 /** Auxiliary variables used in "getLargestDistanceFromOrigin" 00091 * \sa getLargestDistanceFromOrigin 00092 */ 00093 mutable bool m_largestDistanceFromOriginIsUpdated; 00094 00095 /** Auxiliary variables used in "buildKDTree2D" / "buildKDTree3D" 00096 */ 00097 mutable bool m_KDTreeDataIsUpdated; 00098 00099 00100 /** Internal structure with a KD-tree representation. 00101 */ 00102 struct MRPTDLLIMPEXP TKDTreeData 00103 { 00104 /** Init the pointer to NULL. 00105 */ 00106 TKDTreeData(); 00107 00108 /** Copy constructor, invoked when copying CPointsMap: It actually does NOT copy the kd-tree, a new object will be created if required! 00109 */ 00110 TKDTreeData(const TKDTreeData &o); 00111 00112 /** Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! 00113 */ 00114 TKDTreeData& operator =(const TKDTreeData &o); 00115 00116 /** Free memory (if allocated) 00117 */ 00118 ~TKDTreeData(); 00119 00120 /** Free memory (if allocated) 00121 */ 00122 void clear(); 00123 00124 ANNkd_tree *m_pDataTree; 00125 ANNpointArray m_DataPoints; 00126 ANNdist m_NearNeighbourDistances[10]; 00127 ANNidx m_NearNeighbourIndices[10]; 00128 ANNpoint m_QueryPoint; 00129 size_t m_nTreeSize; 00130 size_t m_nDim; 00131 size_t m_nk; 00132 }; 00133 00134 mutable TKDTreeData KDTreeData; 00135 00136 /** Private method to construct the KD-tree (if required) 00137 */ 00138 void build_kdTree2D() const; 00139 00140 /** Private method to construct the KD-tree (if required) 00141 */ 00142 void build_kdTree3D() const; 00143 00144 public: 00145 /** Constructor 00146 */ 00147 CPointsMap(); 00148 00149 /** Virtual destructor. 00150 */ 00151 virtual ~CPointsMap(); 00152 00153 00154 /** KD Tree-based search for the closest point (only ONE) to some given 2D coordinates. 00155 * This method automatically build the "KDTreeData" structure when: 00156 * - It is called for the first time 00157 * - The map has changed 00158 * - The KD-tree was build for 3D. 00159 * 00160 * \param x0 The X coordinate of the query. 00161 * \param y0 The Y coordinate of the query. 00162 * \param out_x The X coordinate of the found closest correspondence. 00163 * \param out_y The Y coordinate of the found closest correspondence. 00164 * \param out_dist_sqr The square distance between the query and the returned point. 00165 * 00166 * \return The index of the closest point in the map array. 00167 * \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D 00168 */ 00169 size_t kdTreeClosestPoint2D( 00170 float x0, 00171 float y0, 00172 float &out_x, 00173 float &out_y, 00174 float &out_dist_sqr 00175 ) const; 00176 00177 inline size_t kdTreeClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut,float &outDistSqr) const { 00178 float dmy1,dmy2; 00179 size_t res=kdTreeClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),dmy1,dmy2,outDistSqr); 00180 pOut.x=dmy1; 00181 pOut.y=dmy2; 00182 return res; 00183 } 00184 00185 /** Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor. 00186 */ 00187 float kdTreeClosestPoint2DsqrError( 00188 float x0, 00189 float y0 ) const; 00190 00191 inline float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const { 00192 return kdTreeClosestPoint2DsqrError(static_cast<float>(p0.x),static_cast<float>(p0.y)); 00193 } 00194 00195 /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map. 00196 */ 00197 virtual float squareDistanceToClosestCorrespondence( 00198 float x0, 00199 float y0 ) const; 00200 00201 inline float squareDistanceToClosestCorrespondenceT(const TPoint2D &p0) const { 00202 return squareDistanceToClosestCorrespondence(static_cast<float>(p0.x),static_cast<float>(p0.y)); 00203 } 00204 00205 00206 /** KD Tree-based search for the TWO closest point to some given 2D coordinates. 00207 * This method automatically build the "KDTreeData" structure when: 00208 * - It is called for the first time 00209 * - The map has changed 00210 * - The KD-tree was build for 3D. 00211 * 00212 * \param x0 The X coordinate of the query. 00213 * \param y0 The Y coordinate of the query. 00214 * \param out_x1 The X coordinate of the first correspondence. 00215 * \param out_y1 The Y coordinate of the first correspondence. 00216 * \param out_x2 The X coordinate of the second correspondence. 00217 * \param out_y2 The Y coordinate of the second correspondence. 00218 * \param out_dist_sqr1 The square distance between the query and the first returned point. 00219 * \param out_dist_sqr2 The square distance between the query and the second returned point. 00220 * 00221 * \sa kdTreeClosestPoint2D 00222 */ 00223 void kdTreeTwoClosestPoint2D( 00224 float x0, 00225 float y0, 00226 float &out_x1, 00227 float &out_y1, 00228 float &out_x2, 00229 float &out_y2, 00230 float &out_dist_sqr1, 00231 float &out_dist_sqr2 ) const; 00232 00233 inline void kdTreeTwoClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut1,TPoint2D &pOut2,float &outDistSqr1,float &outDistSqr2) const { 00234 float dmy1,dmy2,dmy3,dmy4; 00235 kdTreeTwoClosestPoint2D(p0.x,p0.y,dmy1,dmy2,dmy3,dmy4,outDistSqr1,outDistSqr2); 00236 pOut1.x=static_cast<double>(dmy1); 00237 pOut1.y=static_cast<double>(dmy2); 00238 pOut2.x=static_cast<double>(dmy3); 00239 pOut2.y=static_cast<double>(dmy4); 00240 } 00241 00242 /** KD Tree-based search for the N closest point to some given 2D coordinates. 00243 * This method automatically build the "KDTreeData" structure when: 00244 * - It is called for the first time 00245 * - The map has changed 00246 * - The KD-tree was build for 3D. 00247 * 00248 * \param x0 The X coordinate of the query. 00249 * \param y0 The Y coordinate of the query. 00250 * \param N The number of closest points to search. 00251 * \param out_x The vector containing the X coordinates of the correspondences. 00252 * \param out_y The vector containing the Y coordinates of the correspondences. 00253 * \param out_dist_sqr The vector containing the square distance between the query and the returned points. 00254 * 00255 * \return The list of indices 00256 * \sa kdTreeClosestPoint2D 00257 * \sa kdTreeTwoClosestPoint2D 00258 */ 00259 std::vector<size_t> kdTreeNClosestPoint2D( 00260 float x0, 00261 float y0, 00262 unsigned int N, 00263 std::vector<float> &out_x, 00264 std::vector<float> &out_y, 00265 std::vector<float> &out_dist_sqr ) const; 00266 00267 inline std::vector<size_t> kdTreeNClosestPoint2D(const TPoint2D &p0,unsigned int N,std::vector<TPoint2D> &pOut,std::vector<float> &outDistSqr) const { 00268 std::vector<float> dmy1,dmy2; 00269 std::vector<size_t> res=kdTreeNClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),N,dmy1,dmy2,outDistSqr); 00270 pOut.resize(dmy1.size()); 00271 for (size_t i=0;i<dmy1.size();i++) { 00272 pOut[i].x=static_cast<double>(dmy1[i]); 00273 pOut[i].y=static_cast<double>(dmy2[i]); 00274 } 00275 return res; 00276 } 00277 00278 /** KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes. 00279 * This method automatically build the "KDTreeData" structure when: 00280 * - It is called for the first time 00281 * - The map has changed 00282 * - The KD-tree was build for 3D. 00283 * 00284 * \param x0 The X coordinate of the query. 00285 * \param y0 The Y coordinate of the query. 00286 * \param N The number of closest points to search. 00287 * \param out_idx The indexes of the found closest correspondence. 00288 * \param out_dist_sqr The square distance between the query and the returned point. 00289 * 00290 * \sa kdTreeClosestPoint2D 00291 */ 00292 void kdTreeNClosestPoint2DIdx( 00293 float x0, 00294 float y0, 00295 unsigned int N, 00296 std::vector<int> &out_idx, 00297 std::vector<float> &out_dist_sqr ) const; 00298 00299 inline void kdTreeNClosestPoint2DIdx(const TPoint2D &p0,unsigned int N,std::vector<int> &outIdx,std::vector<float> &outDistSqr) const { 00300 return kdTreeNClosestPoint2DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),N,outIdx,outDistSqr); 00301 } 00302 00303 /** KD Tree-based search for the closest point (only ONE) to some given 3D coordinates. 00304 * This method automatically build the "KDTreeData" structure when: 00305 * - It is called for the first time 00306 * - The map has changed 00307 * - The KD-tree was build for 2D. 00308 * 00309 * \param x0 The X coordinate of the query. 00310 * \param y0 The Y coordinate of the query. 00311 * \param z0 The Z coordinate of the query. 00312 * \param out_x The X coordinate of the found closest correspondence. 00313 * \param out_y The Y coordinate of the found closest correspondence. 00314 * \param out_z The Z coordinate of the found closest correspondence. 00315 * \param out_dist_sqr The square distance between the query and the returned point. 00316 * 00317 * \return The index of the closest point in the map array. 00318 * \sa kdTreeClosestPoint2D 00319 */ 00320 size_t kdTreeClosestPoint3D( 00321 float x0, 00322 float y0, 00323 float z0, 00324 float &out_x, 00325 float &out_y, 00326 float &out_z, 00327 float &out_dist_sqr 00328 ) const; 00329 00330 inline size_t kdTreeClosestPoint3D(const TPoint3D &p0,TPoint3D &pOut,float &outDistSqr) const { 00331 float dmy1,dmy2,dmy3; 00332 size_t res=kdTreeClosestPoint3D(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),dmy1,dmy2,dmy3,outDistSqr); 00333 pOut.x=static_cast<double>(dmy1); 00334 pOut.y=static_cast<double>(dmy2); 00335 pOut.z=static_cast<double>(dmy3); 00336 return res; 00337 } 00338 00339 /** KD Tree-based search for the N closest points to some given 3D coordinates. 00340 * This method automatically build the "KDTreeData" structure when: 00341 * - It is called for the first time 00342 * - The map has changed 00343 * - The KD-tree was build for 2D. 00344 * 00345 * \param x0 The X coordinate of the query. 00346 * \param y0 The Y coordinate of the query. 00347 * \param z0 The Z coordinate of the query. 00348 * \param N The number of closest points to search. 00349 * \param out_x The vector containing the X coordinates of the correspondences. 00350 * \param out_y The vector containing the Y coordinates of the correspondences. 00351 * \param out_z The vector containing the Z coordinates of the correspondences. 00352 * \param out_dist_sqr The vector containing the square distance between the query and the returned points. 00353 * 00354 * \sa kdTreeNClosestPoint2D 00355 */ 00356 void kdTreeNClosestPoint3D( 00357 float x0, 00358 float y0, 00359 float z0, 00360 unsigned int N, 00361 std::vector<float> &out_x, 00362 std::vector<float> &out_y, 00363 std::vector<float> &out_z, 00364 std::vector<float> &out_dist_sqr ) const; 00365 00366 inline void kdTreeNClosestPoint3D(const TPoint3D &p0,unsigned int N,std::vector<TPoint3D> &pOut,std::vector<float> &outDistSqr) const { 00367 std::vector<float> dmy1,dmy2,dmy3; 00368 kdTreeNClosestPoint3D(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,dmy1,dmy2,dmy3,outDistSqr); 00369 pOut.resize(dmy1.size()); 00370 for (size_t i=0;i<dmy1.size();i++) { 00371 pOut[i].x=static_cast<double>(dmy1[i]); 00372 pOut[i].y=static_cast<double>(dmy2[i]); 00373 pOut[i].z=static_cast<double>(dmy3[i]); 00374 } 00375 } 00376 00377 /** KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes. 00378 * This method automatically build the "KDTreeData" structure when: 00379 * - It is called for the first time 00380 * - The map has changed 00381 * - The KD-tree was build for 2D. 00382 * 00383 * \param x0 The X coordinate of the query. 00384 * \param y0 The Y coordinate of the query. 00385 * \param z0 The Z coordinate of the query. 00386 * \param N The number of closest points to search. 00387 * \param out_idx The indexes of the found closest correspondence. 00388 * \param out_dist_sqr The square distance between the query and the returned point. 00389 * 00390 * \sa kdTreeClosestPoint2D 00391 */ 00392 void kdTreeNClosestPoint3DIdx( 00393 float x0, 00394 float y0, 00395 float z0, 00396 unsigned int N, 00397 std::vector<int> &out_idx, 00398 std::vector<float> &out_dist_sqr ) const; 00399 00400 inline void kdTreeNClosestPoint3DIdx(const TPoint3D &p0,unsigned int N,std::vector<int> &outIdx,std::vector<float> &outDistSqr) const { 00401 kdTreeNClosestPoint3DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,outIdx,outDistSqr); 00402 } 00403 00404 /** With this struct options are provided to the observation insertion process. 00405 * \sa CObservation::insertIntoPointsMap 00406 */ 00407 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions 00408 { 00409 /** Initilization of default parameters 00410 */ 00411 TInsertionOptions( ); 00412 virtual ~TInsertionOptions() {} 00413 00414 /** See utils::CLoadableOptions 00415 */ 00416 void loadFromConfigFile( 00417 const mrpt::utils::CConfigFileBase &source, 00418 const std::string §ion); 00419 00420 /** See utils::CLoadableOptions 00421 */ 00422 void dumpToTextStream(CStream &out) const; 00423 00424 00425 /** 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. 00426 */ 00427 float minDistBetweenLaserPoints; 00428 00429 /** Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded, clearing all previous content. Default is false. 00430 */ 00431 bool addToExistingPointsMap; 00432 00433 /** If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" intervals (Default is false). 00434 */ 00435 bool also_interpolate; 00436 00437 /** 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. 00438 */ 00439 bool disableDeletion; 00440 00441 /** 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. 00442 */ 00443 bool fuseWithExisting; 00444 00445 /** 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). 00446 * \sa horizontalTolerance 00447 */ 00448 bool isPlanarMap; 00449 00450 /** The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0). 00451 */ 00452 float horizontalTolerance; 00453 00454 /** Applicable only to points map INTO a MRML::CMultiMetricMap, If set to true only points in static cells will be taken into account for matching, ICP, ... 00455 */ 00456 bool matchStaticPointsOnly; 00457 00458 /** The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true) 00459 */ 00460 float maxDistForInterpolatePoints; 00461 00462 }; 00463 00464 TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map 00465 00466 /** Options used when evaluating "computeObservationLikelihood" in the derived classes. 00467 * \sa CObservation::computeObservationLikelihood 00468 */ 00469 struct MRPTDLLIMPEXP TLikelihoodOptions: public utils::CLoadableOptions 00470 { 00471 /** Initilization of default parameters 00472 */ 00473 TLikelihoodOptions( ); 00474 virtual ~TLikelihoodOptions() {} 00475 00476 /** See utils::CLoadableOptions */ 00477 void loadFromConfigFile( 00478 const mrpt::utils::CConfigFileBase &source, 00479 const std::string §ion); 00480 00481 /** See utils::CLoadableOptions */ 00482 void dumpToTextStream(CStream &out) const; 00483 00484 void writeToStream(CStream &out) const; //!< Binary dump to stream - for usage in derived classes' serialization 00485 void readFromStream(CStream &in); //!< Binary dump to stream - for usage in derived classes' serialization 00486 00487 double sigma_dist; //!< Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0.5meters) 00488 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) 00489 uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=10) 00490 }; 00491 00492 TLikelihoodOptions likelihoodOptions; 00493 00494 /** Virtual assignment operator, to be implemented in derived classes. 00495 */ 00496 virtual void copyFrom(const CPointsMap &obj) = 0; 00497 00498 /** Insert the contents of another map into this one, fusing the previous content with the new one. 00499 * This means that points very close to existing ones will be "fused", rather than "added". This prevents 00500 * the unbounded increase in size of these class of maps. 00501 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done 00502 * before calling this method. 00503 * \param otherMap The other map whose points are to be inserted into this one. 00504 * \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. 00505 * \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. 00506 */ 00507 virtual void fuseWith( 00508 CPointsMap *otherMap, 00509 float minDistForFuse = 0.02f, 00510 std::vector<bool> *notFusedPoints = NULL) = 0; 00511 00512 /** Transform the range scan into a set of cartessian coordinated 00513 * points. The options in "insertionOptions" are considered in this method. 00514 * \param rangeScan The scan to be inserted into this map 00515 * \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. 00516 * 00517 * NOTE: Only ranges marked as "valid=true" in the observation will be inserted 00518 * 00519 * \sa CObservation2DRangeScan 00520 */ 00521 virtual void loadFromRangeScan( 00522 const CObservation2DRangeScan &rangeScan, 00523 const CPose3D *robotPose = NULL ) = 0; 00524 00525 /** Load from a text file. In each line there are a point coordinates. 00526 * Returns false if any error occured, true elsewere. 00527 */ 00528 virtual bool load2D_from_text_file(std::string file) = 0; 00529 00530 /** Load from a text file. In each line there are a point coordinates. 00531 * Returns false if any error occured, true elsewere. 00532 */ 00533 virtual bool load3D_from_text_file(std::string file) = 0; 00534 00535 /** Save to a text file. In each line there are a point coordinates. 00536 * Returns false if any error occured, true elsewere. 00537 */ 00538 bool save2D_to_text_file(const std::string &file) const; 00539 00540 /** Save to a text file. In each line there are a point coordinates. 00541 * Returns false if any error occured, true elsewere. 00542 */ 00543 bool save3D_to_text_file(const std::string &file)const; 00544 00545 /** 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). 00546 */ 00547 void saveMetricMapRepresentationToFile( 00548 const std::string &filNamePrefix 00549 )const 00550 { 00551 std::string fil( filNamePrefix + std::string(".txt") ); 00552 save3D_to_text_file( fil ); 00553 } 00554 00555 00556 /** Clear the map, erasing all the points. 00557 */ 00558 virtual void clear() = 0; 00559 00560 /** Returns the number of stored points in the map. 00561 */ 00562 size_t size() const; 00563 00564 /** Returns the number of stored points in the map (DEPRECATED, use "size()" instead better) 00565 */ 00566 size_t getPointsCount() const; 00567 00568 /** Access to a given point from map, as a 2D point. First index is 0. 00569 * \return The return value is the weight of the point (the times it has been fused) 00570 * \exception Throws std::exception on index out of bound. 00571 */ 00572 unsigned long getPoint(size_t index,CPoint2D &p) const; 00573 00574 /** Access to a given point from map, as a 3D point. First index is 0. 00575 * \return The return value is the weight of the point (the times it has been fused) 00576 * \exception Throws std::exception on index out of bound. 00577 */ 00578 unsigned long getPoint(size_t index,CPoint3D &p) const; 00579 00580 /** Access to a given point from map, as a 3D point. First index is 0. 00581 * \return The return value is the weight of the point (the times it has been fused) 00582 * \exception Throws std::exception on index out of bound. 00583 */ 00584 unsigned long getPoint(size_t index,mrpt::math::TPoint3D &p) const; 00585 00586 /** Access to a given point from map, as a 2D point. First index is 0. 00587 * \return The return value is the weight of the point (the times it has been fused) 00588 * \exception Throws std::exception on index out of bound. 00589 */ 00590 unsigned long getPoint(size_t index,mrpt::math::TPoint2D &p) const; 00591 00592 /** Access to a given point from map. First index is 0. 00593 * \return The return value is the weight of the point (the times it has been fused) 00594 * \exception Throws std::exception on index out of bound. 00595 */ 00596 unsigned long getPoint(size_t index,float &x,float &y) const; 00597 00598 /** Access to a given point from map. First index is 0. 00599 * \return The return value is the weight of the point (the times it has been fused) 00600 * \exception Throws std::exception on index out of bound. 00601 */ 00602 unsigned long getPoint(size_t index,float &x,float &y,float &z) const; 00603 00604 /** Changes a given point from map, as a 2D point. First index is 0. 00605 * \exception Throws std::exception on index out of bound. 00606 */ 00607 virtual void setPoint(size_t index,CPoint2D &p)=0; 00608 00609 /** Changes a given point from map, as a 3D point. First index is 0. 00610 * \exception Throws std::exception on index out of bound. 00611 */ 00612 virtual void setPoint(size_t index,CPoint3D &p)=0; 00613 00614 /** Changes a given point from map. First index is 0. 00615 * \exception Throws std::exception on index out of bound. 00616 */ 00617 virtual void setPoint(size_t index,float x, float y)=0; 00618 00619 /** Changes a given point from map. First index is 0. 00620 * \exception Throws std::exception on index out of bound. 00621 */ 00622 virtual void setPoint(size_t index,float x, float y, float z)=0; 00623 00624 /** Provides a direct access to points buffer, or NULL if there is no points in the map. 00625 */ 00626 void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const; 00627 00628 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates. 00629 * 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. 00630 */ 00631 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys,std::vector<float> &zs, size_t decimation = 1 ) const; 00632 00633 inline void getAllPoints(std::vector<TPoint3D> &ps,size_t decimation=1) const { 00634 std::vector<float> dmy1,dmy2,dmy3; 00635 getAllPoints(dmy1,dmy2,dmy3,decimation); 00636 ps.resize(dmy1.size()); 00637 for (size_t i=0;i<dmy1.size();i++) { 00638 ps[i].x=static_cast<double>(dmy1[i]); 00639 ps[i].y=static_cast<double>(dmy2[i]); 00640 ps[i].z=static_cast<double>(dmy3[i]); 00641 } 00642 } 00643 00644 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates. 00645 * 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. 00646 * \sa setAllPoints 00647 */ 00648 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const; 00649 00650 inline void getAllPoints(std::vector<TPoint2D> &ps,size_t decimation=1) const { 00651 std::vector<float> dmy1,dmy2; 00652 getAllPoints(dmy1,dmy2,decimation); 00653 ps.resize(dmy1.size()); 00654 for (size_t i=0;i<dmy1.size();i++) { 00655 ps[i].x=static_cast<double>(dmy1[i]); 00656 ps[i].y=static_cast<double>(dmy2[i]); 00657 } 00658 } 00659 00660 /** Provides a way to insert individual points into the map */ 00661 virtual void insertPoint( float x, float y, float z = 0 ) = 0; 00662 00663 /** Provides a way to insert individual points into the map */ 00664 inline void insertPoint( const CPoint3D &p ) { 00665 insertPoint(p.x(),p.y(),p.z()); 00666 } 00667 00668 /** Provides a way to insert individual points into the map */ 00669 inline void insertPoint( const mrpt::math::TPoint3D &p ) { 00670 insertPoint(p.x,p.y,p.z); 00671 } 00672 00673 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00674 * This is useful for situations where it is approximately known the final size of the map. This method is more 00675 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00676 */ 00677 virtual void reserve(size_t newLength) = 0; 00678 00679 /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */ 00680 virtual void setAllPoints(const vector_float &X,const vector_float &Y,const vector_float &Z) = 0; 00681 00682 /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */ 00683 virtual void setAllPoints(const vector_float &X,const vector_float &Y) = 0; 00684 00685 /** Delete points out of the given "z" axis range have been removed. 00686 */ 00687 void clipOutOfRangeInZ(float zMin, float zMax); 00688 00689 /** Delete points which are more far than "maxRange" away from the given "point". 00690 */ 00691 void clipOutOfRange(const CPoint2D &point, float maxRange); 00692 00693 /** Remove from the map the points marked in a bool's array as "true". 00694 * 00695 * \exception std::exception If mask size is not equal to points count. 00696 */ 00697 virtual void applyDeletionMask( std::vector<bool> &mask ) = 0; 00698 00699 /** Computes the matchings between this and another 2D/3D points map. 00700 This includes finding: 00701 - The set of points pairs in each map 00702 - The mean squared distance between corresponding pairs. 00703 This method is the most time critical one into the ICP algorithm. 00704 00705 * \param otherMap [IN] The other map to compute the matching with. 00706 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00707 * \param maxDistForCorrespondence [IN] Maximum 2D distance between two points to be matched. 00708 * \param maxAngularDistForCorrespondence [IN] Maximum angular distance in radians to allow far points to be matched. 00709 * \param angularDistPivotPoint [IN] The point from which to measure the "angular distances" 00710 * \param correspondences [OUT] The detected matchings pairs. 00711 * \param correspondencesRatio [OUT] The number of correct correspondences. 00712 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00713 * \param covariance [OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired. 00714 * \param onlyKeepTheClosest [OUT] Returns only the closest correspondence (default=false) 00715 * 00716 * \sa computeMatching3DWith 00717 */ 00718 void computeMatchingWith2D( 00719 const CMetricMap *otherMap, 00720 const CPose2D &otherMapPose, 00721 float maxDistForCorrespondence, 00722 float maxAngularDistForCorrespondence, 00723 const CPose2D &angularDistPivotPoint, 00724 TMatchingPairList &correspondences, 00725 float &correspondencesRatio, 00726 float *sumSqrDist = NULL, 00727 bool onlyKeepTheClosest = false, 00728 bool onlyUniqueRobust = false ) const; 00729 00730 /** Computes the matchings between this and another 3D points map - method used in 3D-ICP. 00731 This method finds the set of point pairs in each map. 00732 00733 The method is the most time critical one into the ICP algorithm. 00734 00735 * \param otherMap [IN] The other map to compute the matching with. 00736 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00737 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched. 00738 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00739 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00740 * \param correspondences [OUT] The detected matchings pairs. 00741 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00742 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00743 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00744 * 00745 * \sa compute3DMatchingRatio 00746 */ 00747 void computeMatchingWith3D( 00748 const CMetricMap *otherMap, 00749 const CPose3D &otherMapPose, 00750 float maxDistForCorrespondence, 00751 float maxAngularDistForCorrespondence, 00752 const CPoint3D &angularDistPivotPoint, 00753 TMatchingPairList &correspondences, 00754 float &correspondencesRatio, 00755 float *sumSqrDist = NULL, 00756 bool onlyKeepTheClosest = true, 00757 bool onlyUniqueRobust = false ) const; 00758 00759 00760 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator). 00761 */ 00762 void changeCoordinatesReference(const CPose2D &b); 00763 00764 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator). 00765 */ 00766 void changeCoordinatesReference(const CPose3D &b); 00767 00768 /** 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). 00769 */ 00770 void changeCoordinatesReference(const CPointsMap &other, const CPose3D &b); 00771 00772 /** Returns true if the map is empty/no observation has been inserted. 00773 */ 00774 virtual bool isEmpty() const; 00775 00776 /** Returns a 3D object representing the map. 00777 * The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B 00778 */ 00779 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00780 00781 00782 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00783 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00784 * \param otherMap [IN] The other map to compute the matching with. 00785 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00786 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00787 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00788 * 00789 * \return The matching ratio [0,1] 00790 * \sa computeMatchingWith2D 00791 */ 00792 float compute3DMatchingRatio( 00793 const CMetricMap *otherMap, 00794 const CPose3D &otherMapPose, 00795 float minDistForCorr = 0.10f, 00796 float minMahaDistForCorr = 2.0f 00797 ) const; 00798 00799 /** 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). 00800 */ 00801 float getLargestDistanceFromOrigin() const; 00802 00803 /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points. */ 00804 void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const; 00805 00806 inline void boundingBox(TPoint3D &pMin,TPoint3D &pMax) const { 00807 float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6; 00808 boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6); 00809 pMin.x=dmy1; 00810 pMin.y=dmy2; 00811 pMin.z=dmy3; 00812 pMax.x=dmy4; 00813 pMax.y=dmy5; 00814 pMax.z=dmy6; 00815 } 00816 00817 void extractCylinder( const CPoint2D ¢er, const double radius, const double zmin, const double zmax, CPointsMap *outMap ); 00818 00819 00820 /** The color [0,1] of points when extracted from getAs3DObject (default=blue) */ 00821 static float COLOR_3DSCENE_R; 00822 static float COLOR_3DSCENE_G; 00823 static float COLOR_3DSCENE_B; 00824 00825 00826 /** Computes the likelihood of taking a given observation from a given pose in the world being modeled with this map. 00827 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00828 * \param obs The observation. 00829 * \return This method returns a likelihood in the range [0,1]. 00830 * 00831 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF 00832 * \note In CPointsMap this method is virtual so it can be redefined in derived classes, if desired. 00833 */ 00834 virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00835 00836 }; // End of class def. 00837 00838 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointsMap , CMetricMap ) 00839 00840 } // End of namespace 00841 } // End of namespace 00842 00843 #endif
| Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010 |