Main MRPT website > C++ reference
MRPT logo

KDTreeCapable.h

Go to the documentation of this file.
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.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011