00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 } insertionOptions; 00463 00464 /** Virtual assignment operator, to be implemented in derived classes. 00465 */ 00466 virtual void copyFrom(const CPointsMap &obj) = 0; 00467 00468 /** Insert the contents of another map into this one, fusing the previous content with the new one. 00469 * This means that points very close to existing ones will be "fused", rather than "added". This prevents 00470 * the unbounded increase in size of these class of maps. 00471 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done 00472 * before calling this method. 00473 * \param otherMap The other map whose points are to be inserted into this one. 00474 * \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. 00475 * \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. 00476 */ 00477 virtual void fuseWith( 00478 CPointsMap *otherMap, 00479 float minDistForFuse = 0.02f, 00480 std::vector<bool> *notFusedPoints = NULL) = 0; 00481 00482 /** Transform the range scan into a set of cartessian coordinated 00483 * points. The options in "insertionOptions" are considered in this method. 00484 * \param rangeScan The scan to be inserted into this map 00485 * \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. 00486 * 00487 * NOTE: Only ranges marked as "valid=true" in the observation will be inserted 00488 * 00489 * \sa CObservation2DRangeScan 00490 */ 00491 virtual void loadFromRangeScan( 00492 const CObservation2DRangeScan &rangeScan, 00493 const CPose3D *robotPose = NULL ) = 0; 00494 00495 /** Load from a text file. In each line there are a point coordinates. 00496 * Returns false if any error occured, true elsewere. 00497 */ 00498 virtual bool load2D_from_text_file(std::string file) = 0; 00499 00500 /** Load from a text file. In each line there are a point coordinates. 00501 * Returns false if any error occured, true elsewere. 00502 */ 00503 virtual bool load3D_from_text_file(std::string file) = 0; 00504 00505 /** Save to a text file. In each line there are a point coordinates. 00506 * Returns false if any error occured, true elsewere. 00507 */ 00508 bool save2D_to_text_file(const std::string &file) const; 00509 00510 /** Save to a text file. In each line there are a point coordinates. 00511 * Returns false if any error occured, true elsewere. 00512 */ 00513 bool save3D_to_text_file(const std::string &file)const; 00514 00515 /** 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). 00516 */ 00517 void saveMetricMapRepresentationToFile( 00518 const std::string &filNamePrefix 00519 )const 00520 { 00521 std::string fil( filNamePrefix + std::string(".txt") ); 00522 save3D_to_text_file( fil ); 00523 } 00524 00525 00526 /** Clear the map, erasing all the points. 00527 */ 00528 virtual void clear() = 0; 00529 00530 /** Returns the number of stored points in the map. 00531 */ 00532 size_t size() const; 00533 00534 /** Returns the number of stored points in the map (DEPRECATED, use "size()" instead better) 00535 */ 00536 size_t getPointsCount() const; 00537 00538 /** Access to a given point from map, as a 2D point. First index is 0. 00539 * \return The return value is the weight of the point (the times it has been fused) 00540 * \exception Throws std::exception on index out of bound. 00541 */ 00542 unsigned long getPoint(size_t index,CPoint2D &p) const; 00543 00544 /** Access to a given point from map, as a 3D point. First index is 0. 00545 * \return The return value is the weight of the point (the times it has been fused) 00546 * \exception Throws std::exception on index out of bound. 00547 */ 00548 unsigned long getPoint(size_t index,CPoint3D &p) const; 00549 00550 /** Access to a given point from map, as a 3D point. First index is 0. 00551 * \return The return value is the weight of the point (the times it has been fused) 00552 * \exception Throws std::exception on index out of bound. 00553 */ 00554 unsigned long getPoint(size_t index,mrpt::math::TPoint3D &p) const; 00555 00556 /** Access to a given point from map, as a 2D point. First index is 0. 00557 * \return The return value is the weight of the point (the times it has been fused) 00558 * \exception Throws std::exception on index out of bound. 00559 */ 00560 unsigned long getPoint(size_t index,mrpt::math::TPoint2D &p) const; 00561 00562 /** Access to a given point from map. First index is 0. 00563 * \return The return value is the weight of the point (the times it has been fused) 00564 * \exception Throws std::exception on index out of bound. 00565 */ 00566 unsigned long getPoint(size_t index,float &x,float &y) const; 00567 00568 /** Access to a given point from map. 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,float &x,float &y,float &z) const; 00573 00574 /** Changes a given point from map, as a 2D point. First index is 0. 00575 * \exception Throws std::exception on index out of bound. 00576 */ 00577 virtual void setPoint(size_t index,CPoint2D &p)=0; 00578 00579 /** Changes a given point from map, as a 3D point. First index is 0. 00580 * \exception Throws std::exception on index out of bound. 00581 */ 00582 virtual void setPoint(size_t index,CPoint3D &p)=0; 00583 00584 /** Changes a given point from map. First index is 0. 00585 * \exception Throws std::exception on index out of bound. 00586 */ 00587 virtual void setPoint(size_t index,float x, float y)=0; 00588 00589 /** Changes a given point from map. First index is 0. 00590 * \exception Throws std::exception on index out of bound. 00591 */ 00592 virtual void setPoint(size_t index,float x, float y, float z)=0; 00593 00594 /** Provides a direct access to points buffer, or NULL if there is no points in the map. 00595 */ 00596 void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const; 00597 00598 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates. 00599 * 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. 00600 */ 00601 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys,std::vector<float> &zs, size_t decimation = 1 ) const; 00602 00603 inline void getAllPoints(std::vector<TPoint3D> &ps,size_t decimation=1) const { 00604 std::vector<float> dmy1,dmy2,dmy3; 00605 getAllPoints(dmy1,dmy2,dmy3,decimation); 00606 ps.resize(dmy1.size()); 00607 for (size_t i=0;i<dmy1.size();i++) { 00608 ps[i].x=static_cast<double>(dmy1[i]); 00609 ps[i].y=static_cast<double>(dmy2[i]); 00610 ps[i].z=static_cast<double>(dmy3[i]); 00611 } 00612 } 00613 00614 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates. 00615 * 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. 00616 * \sa setAllPoints 00617 */ 00618 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const; 00619 00620 inline void getAllPoints(std::vector<TPoint2D> &ps,size_t decimation=1) const { 00621 std::vector<float> dmy1,dmy2; 00622 getAllPoints(dmy1,dmy2,decimation); 00623 ps.resize(dmy1.size()); 00624 for (size_t i=0;i<dmy1.size();i++) { 00625 ps[i].x=static_cast<double>(dmy1[i]); 00626 ps[i].y=static_cast<double>(dmy2[i]); 00627 } 00628 } 00629 00630 /** Provides a way to insert individual points into the map */ 00631 virtual void insertPoint( float x, float y, float z = 0 ) = 0; 00632 00633 /** Provides a way to insert individual points into the map */ 00634 inline void insertPoint( const CPoint3D &p ) { 00635 insertPoint(p.x(),p.y(),p.z()); 00636 } 00637 00638 /** Provides a way to insert individual points into the map */ 00639 inline void insertPoint( const mrpt::math::TPoint3D &p ) { 00640 insertPoint(p.x,p.y,p.z); 00641 } 00642 00643 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00644 * This is useful for situations where it is approximately known the final size of the map. This method is more 00645 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00646 */ 00647 virtual void reserve(size_t newLength) = 0; 00648 00649 /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */ 00650 virtual void setAllPoints(const vector_float &X,const vector_float &Y,const vector_float &Z) = 0; 00651 00652 /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */ 00653 virtual void setAllPoints(const vector_float &X,const vector_float &Y) = 0; 00654 00655 /** Delete points out of the given "z" axis range have been removed. 00656 */ 00657 void clipOutOfRangeInZ(float zMin, float zMax); 00658 00659 /** Delete points which are more far than "maxRange" away from the given "point". 00660 */ 00661 void clipOutOfRange(const CPoint2D &point, float maxRange); 00662 00663 /** Remove from the map the points marked in a bool's array as "true". 00664 * 00665 * \exception std::exception If mask size is not equal to points count. 00666 */ 00667 virtual void applyDeletionMask( std::vector<bool> &mask ) = 0; 00668 00669 /** Computes the matchings between this and another 2D/3D points map. 00670 This includes finding: 00671 - The set of points pairs in each map 00672 - The mean squared distance between corresponding pairs. 00673 This method is the most time critical one into the ICP algorithm. 00674 00675 * \param otherMap [IN] The other map to compute the matching with. 00676 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00677 * \param maxDistForCorrespondence [IN] Maximum 2D distance between two points to be matched. 00678 * \param maxAngularDistForCorrespondence [IN] Maximum angular distance in radians to allow far points to be matched. 00679 * \param angularDistPivotPoint [IN] The point from which to measure the "angular distances" 00680 * \param correspondences [OUT] The detected matchings pairs. 00681 * \param correspondencesRatio [OUT] The number of correct correspondences. 00682 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00683 * \param covariance [OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired. 00684 * \param onlyKeepTheClosest [OUT] Returns only the closest correspondence (default=false) 00685 * 00686 * \sa computeMatching3DWith 00687 */ 00688 void computeMatchingWith2D( 00689 const CMetricMap *otherMap, 00690 const CPose2D &otherMapPose, 00691 float maxDistForCorrespondence, 00692 float maxAngularDistForCorrespondence, 00693 const CPose2D &angularDistPivotPoint, 00694 TMatchingPairList &correspondences, 00695 float &correspondencesRatio, 00696 float *sumSqrDist = NULL, 00697 bool onlyKeepTheClosest = false, 00698 bool onlyUniqueRobust = false ) const; 00699 00700 /** Computes the matchings between this and another 3D points map - method used in 3D-ICP. 00701 This method finds the set of point pairs in each map. 00702 00703 The 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 linear distance between two points to be matched. 00708 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00709 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00710 * \param correspondences [OUT] The detected matchings pairs. 00711 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00712 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00713 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00714 * 00715 * \sa compute3DMatchingRatio 00716 */ 00717 void computeMatchingWith3D( 00718 const CMetricMap *otherMap, 00719 const CPose3D &otherMapPose, 00720 float maxDistForCorrespondence, 00721 float maxAngularDistForCorrespondence, 00722 const CPoint3D &angularDistPivotPoint, 00723 TMatchingPairList &correspondences, 00724 float &correspondencesRatio, 00725 float *sumSqrDist = NULL, 00726 bool onlyKeepTheClosest = true, 00727 bool onlyUniqueRobust = false ) const; 00728 00729 00730 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator). 00731 */ 00732 void changeCoordinatesReference(const CPose2D &b); 00733 00734 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator). 00735 */ 00736 void changeCoordinatesReference(const CPose3D &b); 00737 00738 /** 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). 00739 */ 00740 void changeCoordinatesReference(const CPointsMap &other, const CPose3D &b); 00741 00742 /** Returns true if the map is empty/no observation has been inserted. 00743 */ 00744 virtual bool isEmpty() const; 00745 00746 /** Returns a 3D object representing the map. 00747 * The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B 00748 */ 00749 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00750 00751 00752 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00753 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00754 * \param otherMap [IN] The other map to compute the matching with. 00755 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00756 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00757 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00758 * 00759 * \return The matching ratio [0,1] 00760 * \sa computeMatchingWith2D 00761 */ 00762 float compute3DMatchingRatio( 00763 const CMetricMap *otherMap, 00764 const CPose3D &otherMapPose, 00765 float minDistForCorr = 0.10f, 00766 float minMahaDistForCorr = 2.0f 00767 ) const; 00768 00769 /** 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). 00770 */ 00771 float getLargestDistanceFromOrigin() const; 00772 00773 /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points. */ 00774 void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const; 00775 00776 inline void boundingBox(TPoint3D &pMin,TPoint3D &pMax) const { 00777 float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6; 00778 boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6); 00779 pMin.x=dmy1; 00780 pMin.y=dmy2; 00781 pMin.z=dmy3; 00782 pMax.x=dmy4; 00783 pMax.y=dmy5; 00784 pMax.z=dmy6; 00785 } 00786 00787 00788 /** The color [0,1] of points when extracted from getAs3DObject (default=blue) */ 00789 static float COLOR_3DSCENE_R; 00790 static float COLOR_3DSCENE_G; 00791 static float COLOR_3DSCENE_B; 00792 00793 }; // End of class def. 00794 00795 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointsMap , CMetricMap ) 00796 00797 } // End of namespace 00798 } // End of namespace 00799 00800 #endif
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |