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