00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 #ifndef KDTreeCapable_H 00029 #define KDTreeCapable_H 00030 00031 #include <mrpt/utils/utils_defs.h> 00032 00033 #include <mrpt/otherlibs/ann/ANN.h> // ANN: for kd-tree 00034 00035 namespace mrpt 00036 { 00037 namespace math 00038 { 00039 /** A base virtual class providing automatic, cached KD-tree-based look-up of points among data of arbitrary dimensionality. 00040 * Any derived class must only implement: 00041 * - kdtree_get_point_count() 00042 * - kdtree_fill_point_data() 00043 * and must be aware of the need to call "kdtree_mark_as_outdated()" when the data points change to mark the cached KD-tree as invalid. 00044 * 00045 * The KD-tree will be built on demand only upon call of any of the query methods provided by 00046 * this class (kdTreeClosestPoint2D, etc.). 00047 * 00048 * Notice that there is only ONE internal cached KD-tree, so if a method to query a 2D point is called, 00049 * then another method for 3D points, then again the 2D method, three KD-trees will be built. So, try 00050 * to group all the calls for a given dimensionality together or build different class instances for 00051 * queries of each dimensionality, etc. 00052 * 00053 * \sa See some of the derived classes for example implementations of the pure virtual methods. 00054 */ 00055 class BASE_IMPEXP KDTreeCapable 00056 { 00057 public: 00058 KDTreeCapable(); //!< Default ctor 00059 virtual ~KDTreeCapable(); //!< Dtor 00060 00061 00062 /** @name Public utility methods to query the KD-tree 00063 @{ */ 00064 00065 /** KD Tree-based search for the closest point (only ONE) to some given 2D coordinates. 00066 * This method automatically build the "KDTreeData" structure when: 00067 * - It is called for the first time 00068 * - The map has changed 00069 * - The KD-tree was build for 3D. 00070 * 00071 * \param x0 The X coordinate of the query. 00072 * \param y0 The Y coordinate of the query. 00073 * \param out_x The X coordinate of the found closest correspondence. 00074 * \param out_y The Y coordinate of the found closest correspondence. 00075 * \param out_dist_sqr The square distance between the query and the returned point. 00076 * 00077 * \return The index of the closest point in the map array. 00078 * \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D 00079 */ 00080 size_t kdTreeClosestPoint2D( 00081 float x0, 00082 float y0, 00083 float &out_x, 00084 float &out_y, 00085 float &out_dist_sqr 00086 ) const; 00087 00088 inline size_t kdTreeClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut,float &outDistSqr) const { 00089 float dmy1,dmy2; 00090 size_t res=kdTreeClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),dmy1,dmy2,outDistSqr); 00091 pOut.x=dmy1; 00092 pOut.y=dmy2; 00093 return res; 00094 } 00095 00096 /** Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor. 00097 */ 00098 float kdTreeClosestPoint2DsqrError( 00099 float x0, 00100 float y0 ) const; 00101 00102 inline float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const { 00103 return kdTreeClosestPoint2DsqrError(static_cast<float>(p0.x),static_cast<float>(p0.y)); 00104 } 00105 00106 /** KD Tree-based search for the TWO closest point to some given 2D coordinates. 00107 * This method automatically build the "KDTreeData" structure when: 00108 * - It is called for the first time 00109 * - The map has changed 00110 * - The KD-tree was build for 3D. 00111 * 00112 * \param x0 The X coordinate of the query. 00113 * \param y0 The Y coordinate of the query. 00114 * \param out_x1 The X coordinate of the first correspondence. 00115 * \param out_y1 The Y coordinate of the first correspondence. 00116 * \param out_x2 The X coordinate of the second correspondence. 00117 * \param out_y2 The Y coordinate of the second correspondence. 00118 * \param out_dist_sqr1 The square distance between the query and the first returned point. 00119 * \param out_dist_sqr2 The square distance between the query and the second returned point. 00120 * 00121 * \sa kdTreeClosestPoint2D 00122 */ 00123 void kdTreeTwoClosestPoint2D( 00124 float x0, 00125 float y0, 00126 float &out_x1, 00127 float &out_y1, 00128 float &out_x2, 00129 float &out_y2, 00130 float &out_dist_sqr1, 00131 float &out_dist_sqr2 ) const; 00132 00133 inline void kdTreeTwoClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut1,TPoint2D &pOut2,float &outDistSqr1,float &outDistSqr2) const { 00134 float dmy1,dmy2,dmy3,dmy4; 00135 kdTreeTwoClosestPoint2D(p0.x,p0.y,dmy1,dmy2,dmy3,dmy4,outDistSqr1,outDistSqr2); 00136 pOut1.x=static_cast<double>(dmy1); 00137 pOut1.y=static_cast<double>(dmy2); 00138 pOut2.x=static_cast<double>(dmy3); 00139 pOut2.y=static_cast<double>(dmy4); 00140 } 00141 00142 /** KD Tree-based search for the N closest point to some given 2D coordinates. 00143 * This method automatically build the "KDTreeData" structure when: 00144 * - It is called for the first time 00145 * - The map has changed 00146 * - The KD-tree was build for 3D. 00147 * 00148 * \param x0 The X coordinate of the query. 00149 * \param y0 The Y coordinate of the query. 00150 * \param N The number of closest points to search. 00151 * \param out_x The vector containing the X coordinates of the correspondences. 00152 * \param out_y The vector containing the Y coordinates of the correspondences. 00153 * \param out_dist_sqr The vector containing the square distance between the query and the returned points. 00154 * 00155 * \return The list of indices 00156 * \sa kdTreeClosestPoint2D 00157 * \sa kdTreeTwoClosestPoint2D 00158 */ 00159 std::vector<size_t> kdTreeNClosestPoint2D( 00160 float x0, 00161 float y0, 00162 size_t N, 00163 std::vector<float> &out_x, 00164 std::vector<float> &out_y, 00165 std::vector<float> &out_dist_sqr ) const; 00166 00167 inline std::vector<size_t> kdTreeNClosestPoint2D(const TPoint2D &p0,size_t N,std::vector<TPoint2D> &pOut,std::vector<float> &outDistSqr) const { 00168 std::vector<float> dmy1,dmy2; 00169 std::vector<size_t> res=kdTreeNClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),N,dmy1,dmy2,outDistSqr); 00170 pOut.resize(dmy1.size()); 00171 for (size_t i=0;i<dmy1.size();i++) { 00172 pOut[i].x=static_cast<double>(dmy1[i]); 00173 pOut[i].y=static_cast<double>(dmy2[i]); 00174 } 00175 return res; 00176 } 00177 00178 /** KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes. 00179 * This method automatically build the "KDTreeData" structure when: 00180 * - It is called for the first time 00181 * - The map has changed 00182 * - The KD-tree was build for 3D. 00183 * 00184 * \param x0 The X coordinate of the query. 00185 * \param y0 The Y coordinate of the query. 00186 * \param N The number of closest points to search. 00187 * \param out_idx The indexes of the found closest correspondence. 00188 * \param out_dist_sqr The square distance between the query and the returned point. 00189 * 00190 * \sa kdTreeClosestPoint2D 00191 */ 00192 void kdTreeNClosestPoint2DIdx( 00193 float x0, 00194 float y0, 00195 size_t N, 00196 std::vector<int> &out_idx, 00197 std::vector<float> &out_dist_sqr ) const; 00198 00199 inline void kdTreeNClosestPoint2DIdx(const TPoint2D &p0,size_t N,std::vector<int> &outIdx,std::vector<float> &outDistSqr) const { 00200 return kdTreeNClosestPoint2DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),N,outIdx,outDistSqr); 00201 } 00202 00203 /** KD Tree-based search for the closest point (only ONE) to some given 3D coordinates. 00204 * This method automatically build the "KDTreeData" structure when: 00205 * - It is called for the first time 00206 * - The map has changed 00207 * - The KD-tree was build for 2D. 00208 * 00209 * \param x0 The X coordinate of the query. 00210 * \param y0 The Y coordinate of the query. 00211 * \param z0 The Z coordinate of the query. 00212 * \param out_x The X coordinate of the found closest correspondence. 00213 * \param out_y The Y coordinate of the found closest correspondence. 00214 * \param out_z The Z coordinate of the found closest correspondence. 00215 * \param out_dist_sqr The square distance between the query and the returned point. 00216 * 00217 * \return The index of the closest point in the map array. 00218 * \sa kdTreeClosestPoint2D 00219 */ 00220 size_t kdTreeClosestPoint3D( 00221 float x0, 00222 float y0, 00223 float z0, 00224 float &out_x, 00225 float &out_y, 00226 float &out_z, 00227 float &out_dist_sqr 00228 ) const; 00229 00230 inline size_t kdTreeClosestPoint3D(const TPoint3D &p0,TPoint3D &pOut,float &outDistSqr) const { 00231 float dmy1,dmy2,dmy3; 00232 size_t res=kdTreeClosestPoint3D(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),dmy1,dmy2,dmy3,outDistSqr); 00233 pOut.x=static_cast<double>(dmy1); 00234 pOut.y=static_cast<double>(dmy2); 00235 pOut.z=static_cast<double>(dmy3); 00236 return res; 00237 } 00238 00239 /** KD Tree-based search for the N closest points to some given 3D coordinates. 00240 * This method automatically build the "KDTreeData" structure when: 00241 * - It is called for the first time 00242 * - The map has changed 00243 * - The KD-tree was build for 2D. 00244 * 00245 * \param x0 The X coordinate of the query. 00246 * \param y0 The Y coordinate of the query. 00247 * \param z0 The Z coordinate of the query. 00248 * \param N The number of closest points to search. 00249 * \param out_x The vector containing the X coordinates of the correspondences. 00250 * \param out_y The vector containing the Y coordinates of the correspondences. 00251 * \param out_z The vector containing the Z coordinates of the correspondences. 00252 * \param out_dist_sqr The vector containing the square distance between the query and the returned points. 00253 * 00254 * \sa kdTreeNClosestPoint2D 00255 */ 00256 void kdTreeNClosestPoint3D( 00257 float x0, 00258 float y0, 00259 float z0, 00260 size_t N, 00261 std::vector<float> &out_x, 00262 std::vector<float> &out_y, 00263 std::vector<float> &out_z, 00264 std::vector<float> &out_dist_sqr ) const; 00265 00266 inline void kdTreeNClosestPoint3D(const TPoint3D &p0,size_t N,std::vector<TPoint3D> &pOut,std::vector<float> &outDistSqr) const { 00267 std::vector<float> dmy1,dmy2,dmy3; 00268 kdTreeNClosestPoint3D(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,dmy1,dmy2,dmy3,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 pOut[i].z=static_cast<double>(dmy3[i]); 00274 } 00275 } 00276 00277 /** KD Tree-based search for the N closest point to some given 3D 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 2D. 00282 * 00283 * \param x0 The X coordinate of the query. 00284 * \param y0 The Y coordinate of the query. 00285 * \param z0 The Z 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 kdTreeNClosestPoint3DIdx( 00293 float x0, 00294 float y0, 00295 float z0, 00296 size_t N, 00297 std::vector<int> &out_idx, 00298 std::vector<float> &out_dist_sqr ) const; 00299 00300 inline void kdTreeNClosestPoint3DIdx(const TPoint3D &p0,size_t N,std::vector<int> &outIdx,std::vector<float> &outDistSqr) const { 00301 kdTreeNClosestPoint3DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,outIdx,outDistSqr); 00302 } 00303 00304 /* @} */ 00305 00306 protected: 00307 /** To be called by child classes when KD tree data changes. */ 00308 inline void kdtree_mark_as_outdated() const { m_KDTreeDataIsUpToDate = false; } 00309 00310 /** @name Virtual methods that MUST be implemented by children classes of KDTreeCapable 00311 @{ */ 00312 00313 /** Must return the number of data points */ 00314 virtual size_t kdtree_get_point_count() const = 0; 00315 00316 /** Must fill out the data points in "data", such as the i'th point will be stored in (data[i][0],...,data[i][nDims-1]). */ 00317 virtual void kdtree_fill_point_data(ANNpointArray &data, const int nDims) const = 0; 00318 /** @} */ 00319 00320 private: 00321 /** Internal structure with a KD-tree representation. 00322 */ 00323 struct BASE_IMPEXP TKDTreeData 00324 { 00325 /** Init the pointer to NULL. */ 00326 TKDTreeData(); 00327 00328 /** Copy constructor: It actually does NOT copy the kd-tree, a new object will be created if required! */ 00329 TKDTreeData(const TKDTreeData &o); 00330 00331 /** Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! */ 00332 TKDTreeData& operator =(const TKDTreeData &o); 00333 00334 /** Free memory (if allocated) */ 00335 ~TKDTreeData(); 00336 00337 /** Free memory (if allocated) */ 00338 void clear(); 00339 00340 ANNkd_tree *m_pDataTree; 00341 ANNpointArray m_DataPoints; 00342 ANNpoint m_QueryPoint; 00343 size_t m_nTreeSize; 00344 size_t m_nDim; 00345 size_t m_nk; 00346 }; 00347 00348 mutable TKDTreeData KDTreeData; 00349 00350 mutable bool m_KDTreeDataIsUpToDate; //!< whether the KD tree needs to be rebuilt or not. 00351 00352 void rebuild_kdTree(size_t nDims) const; //!< Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... usage (asking the child class for the data points). 00353 00354 00355 }; 00356 00357 } // End of namespace 00358 } // End of namespace 00359 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |