PARP Research Group University of Murcia, Spain


examples/OpenCV/siftDetector/hess/kdtree.cpp

00001 /*
00002   Functions and structures for maintaining a k-d tree database of image
00003   features.
00004   
00005   For more information, refer to:
00006   
00007   Beis, J. S. and Lowe, D. G.  Shape indexing using approximate
00008   nearest-neighbor search in high-dimensional spaces.  In <EM>Conference
00009   on Computer Vision and Pattern Recognition (CVPR)</EM> (2003),
00010   pp. 1000--1006.
00011   
00012   Copyright (C) 2006-2007  Rob Hess <hess@eecs.oregonstate.edu>
00013 
00014   @version 1.1.1-20070913
00015 */
00016 
00017 #include "kdtree.h"
00018 #include "minpq.h"
00019 #include "imgfeatures.h"
00020 #include "utils.h"
00021 
00022 #include <cxcore.h>
00023 
00024 #include <stdio.h>
00025 
00026 struct bbf_data
00027 {
00028   double d;
00029   void* old_data;
00030 };
00031 
00032 /************************* Local Function Prototypes *************************/
00033 
00034 struct kd_node* kd_node_init( struct feature*, int );
00035 void expand_kd_node_subtree( struct kd_node* );
00036 void assign_part_key( struct kd_node* );
00037 double median_select( double*, int );
00038 double rank_select( double*, int, int );
00039 void insertion_sort( double*, int );
00040 int partition_array( double*, int, double );
00041 void partition_features( struct kd_node* );
00042 struct kd_node* explore_to_leaf( struct kd_node*, struct feature*,
00043                                  struct min_pq* );
00044 int insert_into_nbr_array( struct feature*, struct feature**, int, int );
00045 int within_rect( CvPoint2D64f, CvRect );
00046 
00047 
00048 /******************** Functions prototyped in keyptdb.h **********************/
00049 
00050 
00051 /*
00052   A function to build a k-d tree database from keypoints in an array.
00053   
00054   @param features an array of features
00055   @param n the number of features in features
00056   
00057   @return Returns the root of a kd tree built from features or NULL on
00058     error.
00059 */
00060 struct kd_node* kdtree_build( struct feature* features, int n )
00061 {
00062   struct kd_node* kd_root;
00063 
00064   if( ! features  ||  n <= 0 )
00065     {
00066       fprintf( stderr, "Warning: kdtree_build(): no features, %s, line %d\n",
00067                __FILE__, __LINE__ );
00068       return NULL;
00069     }
00070 
00071   kd_root = kd_node_init( features, n );
00072   expand_kd_node_subtree( kd_root );
00073 
00074   return kd_root;
00075 }
00076 
00077 
00078 
00079 /*
00080   Finds an image feature's approximate k nearest neighbors in a kd tree using
00081   Best Bin First search.
00082   
00083   @param kd_root root of an image feature kd tree
00084   @param feat image feature for whose neighbors to search
00085   @param k number of neighbors to find
00086    @param nbrs pointer to an array in which to store pointers to neighbors
00087      in order of increasing descriptor distance
00088   @param max_nn_chks search is cut off after examining this many tree entries
00089   
00090   @return Returns the number of neighbors found and stored in nbrs, or
00091     -1 on error.
00092 */
00093 int kdtree_bbf_knn( struct kd_node* kd_root, struct feature* feat, int k,
00094                     struct feature*** nbrs, int max_nn_chks )
00095 {
00096   struct kd_node* expl;
00097   struct min_pq* min_pq;
00098   struct feature* tree_feat, ** _nbrs;
00099   struct bbf_data* bbf_data;
00100   int i, t = 0, n = 0;
00101 
00102   if( ! nbrs  ||  ! feat  ||  ! kd_root )
00103     {
00104       fprintf( stderr, "Warning: NULL pointer error, %s, line %d\n",
00105                __FILE__, __LINE__ );
00106       return -1;
00107     }
00108 
00109   _nbrs = (struct feature **) calloc( k, sizeof( struct feature* ) );
00110   min_pq = minpq_init();
00111   minpq_insert( min_pq, kd_root, 0 );
00112   while( min_pq->n > 0  &&  t < max_nn_chks )
00113     {
00114       expl = (struct kd_node*)minpq_extract_min( min_pq );
00115       if( ! expl )
00116         {
00117           fprintf( stderr, "Warning: PQ unexpectedly empty, %s line %d\n",
00118                    __FILE__, __LINE__ );
00119           goto fail;
00120         }
00121 
00122       expl = explore_to_leaf( expl, feat, min_pq );
00123       if( ! expl )
00124         {
00125           fprintf( stderr, "Warning: PQ unexpectedly empty, %s line %d\n",
00126                    __FILE__, __LINE__ );
00127           goto fail;
00128         }
00129 
00130       for( i = 0; i < expl->n; i++ )
00131         {
00132           tree_feat = &expl->features[i];
00133           bbf_data = (struct bbf_data *) malloc( sizeof( struct bbf_data ) );
00134           if( ! bbf_data )
00135             {
00136               fprintf( stderr, "Warning: unable to allocate memory,"
00137                        " %s line %d\n", __FILE__, __LINE__ );
00138               goto fail;
00139             }
00140           bbf_data->old_data = tree_feat->feature_data;
00141           bbf_data->d = descr_dist_sq(feat, tree_feat);
00142           tree_feat->feature_data = bbf_data;
00143           n += insert_into_nbr_array( tree_feat, _nbrs, n, k );
00144         }
00145       t++;
00146     }
00147 
00148   minpq_release( &min_pq );
00149   for( i = 0; i < n; i++ )
00150     {
00151       bbf_data = (struct bbf_data *) _nbrs[i]->feature_data;
00152       _nbrs[i]->feature_data = bbf_data->old_data;
00153       free( bbf_data );
00154     }
00155   *nbrs = _nbrs;
00156   return n;
00157 
00158  fail:
00159   minpq_release( &min_pq );
00160   for( i = 0; i < n; i++ )
00161     {
00162       bbf_data = (struct bbf_data *) _nbrs[i]->feature_data;
00163       _nbrs[i]->feature_data = bbf_data->old_data;
00164       free( bbf_data );
00165     }
00166   free( _nbrs );
00167   *nbrs = NULL;
00168   return -1;
00169 }
00170 
00171 
00172 
00173 /*
00174   Finds an image feature's approximate k nearest neighbors within a specified
00175   spatial region in a kd tree using Best Bin First search.
00176   
00177   @param kd_root root of an image feature kd tree
00178   @param feat image feature for whose neighbors to search
00179   @param k number of neighbors to find
00180    @param nbrs pointer to an array in which to store pointers to neighbors
00181      in order of increasing descriptor distance
00182    @param max_nn_chks search is cut off after examining this many tree entries
00183    @param rect rectangular region in which to search for neighbors
00184    @param model if true, spatial search is based on kdtree features' model
00185      locations; otherwise it is based on their image locations
00186    
00187    @return Returns the number of neighbors found and stored in \a nbrs
00188      (in case \a k neighbors could not be found before examining
00189      \a max_nn_checks keypoint entries).
00190 */
00191 int kdtree_bbf_spatial_knn( struct kd_node* kd_root, struct feature* feat,
00192                             int k, struct feature*** nbrs, int max_nn_chks,
00193                             CvRect rect, int model )
00194 {
00195   struct feature** all_nbrs, ** sp_nbrs;
00196   CvPoint2D64f pt;
00197   int i, n, t = 0;
00198 
00199   n = kdtree_bbf_knn( kd_root, feat, max_nn_chks, &all_nbrs, max_nn_chks );
00200   sp_nbrs = (struct feature **) calloc( k, sizeof( struct feature* ) );
00201   for( i = 0; i < n; i++ )
00202     {
00203       if( model )
00204         pt = all_nbrs[i]->mdl_pt;
00205       else
00206         pt = all_nbrs[i]->img_pt;
00207 
00208       if( within_rect( pt, rect ) )
00209         {
00210           sp_nbrs[t++] = all_nbrs[i];
00211           if( t == k )
00212             goto end;
00213         }
00214     }
00215  end:
00216   free( all_nbrs );
00217   *nbrs = sp_nbrs;
00218   return t;
00219 }
00220 
00221 
00222 
00223 /*
00224   De-allocates memory held by a kd tree
00225   
00226   @param kd_root pointer to the root of a kd tree
00227 */
00228 void kdtree_release( struct kd_node* kd_root )
00229 {
00230   if( ! kd_root )
00231     return;
00232   kdtree_release( kd_root->kd_left );
00233   kdtree_release( kd_root->kd_right );
00234   free( kd_root );
00235 }
00236 
00237 
00238 /************************ Functions prototyped here **************************/
00239 
00240 
00241 /*
00242   Initializes a kd tree node with a set of features.  The node is not
00243   expanded, and no ordering is imposed on the features.
00244   
00245   @param features an array of image features
00246   @param n number of features
00247 
00248   @return Returns an unexpanded kd-tree node.
00249 */
00250 struct kd_node* kd_node_init( struct feature* features, int n )
00251 {
00252   struct kd_node* kd_node;
00253 
00254   kd_node = (struct kd_node *) malloc( sizeof( struct kd_node ) );
00255   memset( kd_node, 0, sizeof( struct kd_node ) );
00256   kd_node->ki = -1;
00257   kd_node->features = features;
00258   kd_node->n = n;
00259 
00260   return kd_node;
00261 }
00262 
00263 
00264 
00265 /*
00266   Recursively expands a specified kd tree node into a tree whose leaves
00267   contain one entry each.
00268 
00269   @param kd_node an unexpanded node in a kd tree
00270 */
00271 void expand_kd_node_subtree( struct kd_node* kd_node )
00272 {
00273   /* base case: leaf node */
00274   if( kd_node->n == 1  ||  kd_node->n == 0 )
00275     {
00276       kd_node->leaf = 1;
00277       return;
00278     }
00279 
00280   assign_part_key( kd_node );
00281   partition_features( kd_node );
00282 
00283   if( kd_node->kd_left )
00284     expand_kd_node_subtree( kd_node->kd_left );
00285   if( kd_node->kd_right )
00286     expand_kd_node_subtree( kd_node->kd_right );
00287 }
00288 
00289 
00290 
00291 /*
00292   Determines the descriptor index at which and the value with which to
00293   partition a kd tree node's features.
00294 
00295   @param kd_node a kd tree node
00296 */
00297 void assign_part_key( struct kd_node* kd_node )
00298 {
00299   struct feature* features;
00300   double kv, x, mean, var, var_max = 0;
00301   double* tmp;
00302   int d, n, i, j, ki = 0;
00303 
00304   features = kd_node->features;
00305   n = kd_node->n;
00306   d = features[0].d;
00307 
00308   /* partition key index is that along which descriptors have most variance */
00309   for( j = 0; j < d; j++ )
00310     {
00311       mean = var = 0;
00312       for( i = 0; i < n; i++ )
00313         mean += features[i].descr[j];
00314       mean /= n;
00315       for( i = 0; i < n; i++ )
00316         {
00317           x = features[i].descr[j] - mean;
00318           var += x * x;
00319         }
00320       var /= n;
00321 
00322       if( var > var_max )
00323         {
00324           ki = j;
00325           var_max = var;
00326         }
00327     }
00328 
00329   /* partition key value is median of descriptor values at ki */
00330   tmp = (double *) calloc( n, sizeof( double ) );
00331   for( i = 0; i < n; i++ )
00332     tmp[i] = features[i].descr[ki];
00333   kv = median_select( tmp, n );
00334   free( tmp );
00335 
00336   kd_node->ki = ki;
00337   kd_node->kv = kv;
00338 }
00339 
00340 
00341 
00342 /*
00343   Finds the median value of an array.  The array's elements are re-ordered
00344   by this function.
00345 
00346   @param array an array; the order of its elelemts is reordered
00347   @param n number of elements in array
00348 
00349   @return Returns the median value of array.
00350 */
00351 double median_select( double* array, int n )
00352 {
00353   return rank_select( array, n, (n - 1) / 2 );
00354 }
00355 
00356 
00357 
00358 /*
00359   Finds the element of a specified rank in an array using the linear time
00360   median-of-medians algorithm by Blum, Floyd, Pratt, Rivest, and Tarjan.
00361   The elements of the array are re-ordered by this function.
00362 
00363   @param array an array; the order of its elelemts is reordered
00364   @param n number of elements in array
00365   @param r the zero-based rank of the element to be selected
00366   
00367   @return Returns the element from array with zero-based rank r.
00368 */
00369 double rank_select( double* array, int n, int r )
00370 {
00371   double* tmp, med;
00372   int gr_5, gr_tot, rem_elts, i, j;
00373 
00374   /* base case */
00375   if( n == 1 )
00376     return array[0];
00377 
00378   /* divide array into groups of 5 and sort them */
00379   gr_5 = n / 5;
00380   gr_tot = cvCeil( n / 5.0 );
00381   rem_elts = n % 5;
00382   tmp = array;
00383   for( i = 0; i < gr_5; i++ )
00384     {
00385       insertion_sort( tmp, 5 );
00386       tmp += 5;
00387     }
00388   insertion_sort( tmp, rem_elts );
00389 
00390   /* recursively find the median of the medians of the groups of 5 */
00391   tmp = (double *) calloc( gr_tot, sizeof( double ) );
00392   for( i = 0, j = 2; i < gr_5; i++, j += 5 )
00393     tmp[i] = array[j];
00394   if( rem_elts )
00395     tmp[i++] = array[n - 1 - rem_elts/2];
00396   med = rank_select( tmp, i, ( i - 1 ) / 2 );
00397   free( tmp );
00398   
00399   /* partition around median of medians and recursively select if necessary */
00400   j = partition_array( array, n, med );
00401   if( r == j )
00402     return med;
00403   else if( r < j )
00404     return rank_select( array, j, r );
00405   else
00406     {
00407       array += j+1;
00408       return rank_select( array, ( n - j - 1 ), ( r - j - 1 ) );
00409     }
00410 }
00411 
00412 
00413 
00414 /*
00415   Sorts an array in place into increasing order using insertion sort.
00416 
00417   @param array an array
00418   @param n number of elements
00419 */
00420 void insertion_sort( double* array, int n )
00421 {
00422   double k;
00423   int i, j;
00424 
00425   for( i = 1; i < n; i++ )
00426     {
00427       k = array[i];
00428       j = i-1;
00429       while( j >= 0  &&  array[j] > k )
00430         {
00431           array[j+1] = array[j];
00432           j -= 1;
00433         }
00434       array[j+1] = k;
00435     }
00436 }
00437 
00438 
00439 
00440 /*
00441   Partitions an array around a specified value.
00442 
00443   @param array an array
00444   @param n number of elements
00445   @param pivot value around which to partition
00446 
00447   @return Returns index of the pivot after partitioning
00448 */
00449 int partition_array( double* array, int n, double pivot )
00450 {
00451   double tmp;
00452   int p, i, j;
00453 
00454   i = -1;
00455   for( j = 0; j < n; j++ )
00456     if( array[j] <= pivot )
00457       {
00458         tmp = array[++i];
00459         array[i] = array[j];
00460         array[j] = tmp;
00461         if( array[i] == pivot )
00462           p = i;
00463       }
00464   array[p] = array[i];
00465   array[i] = pivot;
00466   
00467   return i;
00468 }
00469 
00470 
00471 
00472 /*
00473   Partitions the features at a specified kd tree node to create its two
00474   children.
00475 
00476   @param kd_node a kd tree node whose partition key is set
00477 */
00478 void partition_features( struct kd_node* kd_node )
00479 {
00480   struct feature* features, tmp;
00481   double kv;
00482   int n, ki, p, i, j = -1;
00483 
00484   features = kd_node->features;
00485   n = kd_node->n;
00486   ki = kd_node->ki;
00487   kv = kd_node->kv;
00488   for( i = 0; i < n; i++ )
00489     if( features[i].descr[ki] <= kv )
00490       {
00491         tmp = features[++j];
00492         features[j] = features[i];
00493         features[i] = tmp;
00494         if( features[j].descr[ki] == kv )
00495           p = j;
00496       }
00497   tmp = features[p];
00498   features[p] = features[j];
00499   features[j] = tmp;
00500 
00501   /* if all records fall on same side of partition, make node a leaf */
00502   if( j == n - 1 )
00503     {
00504       kd_node->leaf = 1;
00505       return;
00506     }
00507 
00508   kd_node->kd_left = kd_node_init( features, j + 1 );
00509   kd_node->kd_right = kd_node_init( features + ( j + 1 ), ( n - j - 1 ) );
00510 }
00511 
00512 
00513 
00514 /*
00515   Explores a kd tree from a given node to a leaf.  Branching decisions are
00516   made at each node based on the descriptor of a given feature.  Each node
00517   examined but not explored is put into a priority queue to be explored
00518   later, keyed based on the distance from its partition key value to the
00519   given feature's desctiptor.
00520   
00521   @param kd_node root of the subtree to be explored
00522   @param feat feature upon which branching decisions are based
00523   @param min_pq a minimizing priority queue into which tree nodes are placed
00524     as described above
00525 
00526   @return Returns a pointer to the leaf node at which exploration ends or
00527     NULL on error.
00528 */
00529 struct kd_node* explore_to_leaf( struct kd_node* kd_node, struct feature* feat,
00530                                  struct min_pq* min_pq )
00531 {
00532   struct kd_node* unexpl, * expl = kd_node;
00533   double kv;
00534   int ki;
00535 
00536   while( expl  &&  ! expl->leaf )
00537     {
00538       ki = expl->ki;
00539       kv = expl->kv;
00540       
00541       if( ki >= feat->d )
00542         {
00543           fprintf( stderr, "Warning: comparing imcompatible descriptors, %s" \
00544                    " line %d\n", __FILE__, __LINE__ );
00545           return NULL;
00546         }
00547       if( feat->descr[ki] <= kv )
00548         {
00549           unexpl = expl->kd_right;
00550           expl = expl->kd_left;
00551         }
00552       else
00553         {
00554           unexpl = expl->kd_left;
00555           expl = expl->kd_right;
00556         }
00557       
00558       if( minpq_insert( min_pq, unexpl, ABS( kv - feat->descr[ki] ) ) )
00559         {
00560           fprintf( stderr, "Warning: unable to insert into PQ, %s, line %d\n",
00561                    __FILE__, __LINE__ );
00562           return NULL;
00563         }
00564     }
00565 
00566   return expl;
00567 }
00568 
00569 
00570 
00571 /*
00572   Inserts a feature into the nearest-neighbor array so that the array remains
00573   in order of increasing descriptor distance from the search feature.
00574 
00575   @param feat feature to be inderted into the array; it's feature_data field
00576     should be a pointer to a bbf_data with d equal to the squared descriptor
00577     distance between feat and the search feature
00578   @param nbrs array of nearest neighbors neighbors
00579   @param n number of elements already in nbrs and
00580   @param k maximum number of elements in nbrs
00581 
00582   @return If feat was successfully inserted into nbrs, returns 1; otherwise
00583     returns 0.
00584 */
00585 int insert_into_nbr_array( struct feature* feat, struct feature** nbrs,
00586                            int n, int k )
00587 {
00588   struct bbf_data* fdata, * ndata;
00589   double dn, df;
00590   int i, ret = 0;
00591 
00592   if( n == 0 )
00593     {
00594       nbrs[0] = feat;
00595       return 1;
00596     }
00597 
00598   /* check at end of array */
00599   fdata = (struct bbf_data*)feat->feature_data;
00600   df = fdata->d;
00601   ndata = (struct bbf_data*)nbrs[n-1]->feature_data;
00602   dn = ndata->d;
00603   if( df >= dn )
00604     {
00605       if( n == k )
00606         {
00607           feat->feature_data = fdata->old_data;
00608           free( fdata );
00609           return 0;
00610         }
00611       nbrs[n] = feat;
00612       return 1;
00613     }
00614 
00615   /* find the right place in the array */
00616   if( n < k )
00617     {
00618       nbrs[n] = nbrs[n-1];
00619       ret = 1;
00620     }
00621   else
00622     {
00623       nbrs[n-1]->feature_data = ndata->old_data;
00624       free( ndata );
00625     }
00626   i = n-2;
00627   while( i >= 0 )
00628     {
00629       ndata = (struct bbf_data*)nbrs[i]->feature_data;
00630       dn = ndata->d;
00631       if( dn <= df )
00632         break;
00633       nbrs[i+1] = nbrs[i];
00634       i--;
00635     }
00636   i++;
00637   nbrs[i] = feat;
00638 
00639   return ret;
00640 }
00641 
00642 
00643 
00644 /*
00645   Determines whether a given point lies within a specified rectangular region
00646 
00647   @param pt point
00648   @param rect rectangular region
00649 
00650   @return Returns 1 if pt is inside rect or 0 otherwise
00651 */
00652 int within_rect( CvPoint2D64f pt, CvRect rect )
00653 {
00654   if( pt.x < rect.x  ||  pt.y < rect.y )
00655     return 0;
00656   if( pt.x > rect.x + rect.width  ||  pt.y > rect.y + rect.height )
00657     return 0;
00658   return 1;
00659 }



QVision framework. PARP research group, copyright 2007, 2008.