kdtree.c
上传用户:lwxipeng
上传日期:2022-05-16
资源大小:15982k
文件大小:15k
源码类别:

视频捕捉/采集

开发平台:

Visual C++

  1. /*
  2. Functions and structures for maintaining a k-d tree database of image
  3. features.
  4. For more information, refer to:
  5. Beis, J. S. and Lowe, D. G.  Shape indexing using approximate
  6. nearest-neighbor search in high-dimensional spaces.  In <EM>Conference
  7. on Computer Vision and Pattern Recognition (CVPR)</EM> (2003),
  8. pp. 1000--1006.
  9. Copyright (C) 2006  Rob Hess <hess@eecs.oregonstate.edu>
  10. @version 1.1.1-20070330
  11. */
  12. #include "minpq.h"
  13. #include "imgfeatures.h"
  14. #include "utils.h"
  15. #include "kdtree.h"
  16. #include <cxcore.h>
  17. #include <stdio.h>
  18. struct bbf_data
  19. {
  20. double d;
  21. void* old_data;
  22. };
  23. /************************* Local Function Prototypes *************************/
  24. struct kd_node* kd_node_init( struct feature*, int );
  25. void expand_kd_node_subtree( struct kd_node* );
  26. void assign_part_key( struct kd_node* );
  27. double median_select( double*, int );
  28. double rank_select( double*, int, int );
  29. void insertion_sort( double*, int );
  30. int partition_array( double*, int, double );
  31. void partition_features( struct kd_node* );
  32. struct kd_node* explore_to_leaf( struct kd_node*, struct feature*,
  33. struct min_pq* );
  34. int insert_into_nbr_array( struct feature*, struct feature**, int, int );
  35. int within_rect( CvPoint2D64f, CvRect );
  36. /******************** Functions prototyped in keyptdb.h **********************/
  37. /*
  38. A function to build a k-d tree database from keypoints in an array.
  39. @param features an array of features
  40. @param n the number of features in features
  41. @return Returns the root of a kd tree built from features or NULL on error.
  42. */
  43. struct kd_node* kdtree_build( struct feature* features, int n )
  44. {
  45. struct kd_node* kd_root;
  46. if( ! features  ||  n <= 0 )
  47. {
  48. fprintf( stderr, "Warning: kdtree_build(): no features, %s, line %dn",
  49. __FILE__, __LINE__ );
  50. return NULL;
  51. }
  52. kd_root = kd_node_init( features, n );
  53. expand_kd_node_subtree( kd_root );
  54. return kd_root;
  55. }
  56. /*
  57. Finds an image feature's approximate k nearest neighbors in a kd tree using
  58. Best Bin First search.
  59. @param kd_root root of an image feature kd tree
  60. @param feat image feature for whose neighbors to search
  61. @param k number of neighbors to find
  62. @param nbrs pointer to an array in which to store pointers to neighbors
  63. in order of increasing descriptor distance
  64. @param max_nn_chks search is cut off after examining this many tree entries
  65. @return Returns the number of neighbors found and stored in nbrs, or
  66. -1 on error.
  67. */
  68. int kdtree_bbf_knn( struct kd_node* kd_root, struct feature* feat, int k,
  69. struct feature*** nbrs, int max_nn_chks )
  70. {
  71. struct kd_node* expl;
  72. struct min_pq* min_pq;
  73. struct feature* tree_feat, ** _nbrs;
  74. struct bbf_data* bbf_data;
  75. int i, t = 0, n = 0;
  76. if( ! nbrs  ||  ! feat  ||  ! kd_root )
  77. {
  78. fprintf( stderr, "Warning: NULL pointer error, %s, line %dn",
  79. __FILE__, __LINE__ );
  80. return -1;
  81. }
  82. _nbrs = calloc( k, sizeof( struct feature* ) );
  83. min_pq = minpq_init();
  84. minpq_insert( min_pq, kd_root, 0 );
  85. while( min_pq->n > 0  &&  t < max_nn_chks )
  86. {
  87. expl = (struct kd_node*)minpq_extract_min( min_pq );
  88. if( ! expl )
  89. {
  90. fprintf( stderr, "Warning: PQ unexpectedly empty, %s line %dn",
  91. __FILE__, __LINE__ );
  92. goto fail;
  93. }
  94. expl = explore_to_leaf( expl, feat, min_pq );
  95. if( ! expl )
  96. {
  97. fprintf( stderr, "Warning: PQ unexpectedly empty, %s line %dn",
  98. __FILE__, __LINE__ );
  99. goto fail;
  100. }
  101. for( i = 0; i < expl->n; i++ )
  102. {
  103. tree_feat = &expl->features[i];
  104. bbf_data = malloc( sizeof( struct bbf_data ) );
  105. if( ! bbf_data )
  106. {
  107. fprintf( stderr, "Warning: unable to allocate memory,"
  108. " %s line %dn", __FILE__, __LINE__ );
  109. goto fail;
  110. }
  111. bbf_data->old_data = tree_feat->feature_data;
  112. bbf_data->d = descr_dist_sq(feat, tree_feat);
  113. tree_feat->feature_data = bbf_data;
  114. n += insert_into_nbr_array( tree_feat, _nbrs, n, k );
  115. }
  116. t++;
  117. }
  118. minpq_release( &min_pq );
  119. for( i = 0; i < n; i++ )
  120. {
  121. bbf_data = _nbrs[i]->feature_data;
  122. _nbrs[i]->feature_data = bbf_data->old_data;
  123. free( bbf_data );
  124. }
  125. *nbrs = _nbrs;
  126. return n;
  127. fail:
  128. minpq_release( &min_pq );
  129. for( i = 0; i < n; i++ )
  130. {
  131. bbf_data = _nbrs[i]->feature_data;
  132. _nbrs[i]->feature_data = bbf_data->old_data;
  133. free( bbf_data );
  134. }
  135. free( _nbrs );
  136. *nbrs = NULL;
  137. return -1;
  138. }
  139. /*
  140. Finds an image feature's approximate k nearest neighbors within a specified
  141. spatial region in a kd tree using Best Bin First search.
  142. @param kd_root root of an image feature kd tree
  143. @param feat image feature for whose neighbors to search
  144. @param k number of neighbors to find
  145. @param nbrs pointer to an array in which to store pointers to neighbors
  146. in order of increasing descriptor distance
  147. @param max_nn_chks search is cut off after examining this many tree entries
  148. @param rect rectangular region in which to search for neighbors
  149. @param model if true, spatial search is based on kdtree features' model
  150. locations; otherwise it is based on their image locations
  151. @return Returns the number of neighbors found and stored in a nbrs
  152. (in case a k neighbors could not be found before examining
  153. a max_nn_checks keypoint entries).
  154. */
  155. int kdtree_bbf_spatial_knn( struct kd_node* kd_root, struct feature* feat,
  156.    int k, struct feature*** nbrs, int max_nn_chks,
  157.    CvRect rect, int model )
  158. {
  159. struct feature** all_nbrs, ** sp_nbrs;
  160. CvPoint2D64f pt;
  161. int i, n, t = 0;
  162. n = kdtree_bbf_knn( kd_root, feat, max_nn_chks, &all_nbrs, max_nn_chks );
  163. sp_nbrs = calloc( k, sizeof( struct feature* ) );
  164. for( i = 0; i < n; i++ )
  165. {
  166. if( model )
  167. pt = all_nbrs[i]->mdl_pt;
  168. else
  169. pt = all_nbrs[i]->img_pt;
  170. if( within_rect( pt, rect ) )
  171. {
  172. sp_nbrs[t++] = all_nbrs[i];
  173. if( t == k )
  174. goto end;
  175. }
  176. }
  177. end:
  178. free( all_nbrs );
  179. *nbrs = sp_nbrs;
  180. return t;
  181. }
  182. /*
  183. De-allocates memory held by a kd tree
  184. @param kd_root pointer to the root of a kd tree
  185. */
  186. void kdtree_release( struct kd_node* kd_root )
  187. {
  188. if( ! kd_root )
  189. return;
  190. kdtree_release( kd_root->kd_left );
  191. kdtree_release( kd_root->kd_right );
  192. free( kd_root );
  193. }
  194. /************************ Functions prototyped here **************************/
  195. /*
  196. Initializes a kd tree node with a set of features.  The node is not
  197. expanded, and no ordering is imposed on the features.
  198. @param features an array of image features
  199. @param n number of features
  200. @return Returns an unexpanded kd-tree node.
  201. */
  202. struct kd_node* kd_node_init( struct feature* features, int n )
  203. {
  204. struct kd_node* kd_node;
  205. kd_node = malloc( sizeof( struct kd_node ) );
  206. memset( kd_node, 0, sizeof( struct kd_node ) );
  207. kd_node->ki = -1;
  208. kd_node->features = features;
  209. kd_node->n = n;
  210. return kd_node;
  211. }
  212. /*
  213. Recursively expands a specified kd tree node into a tree whose leaves
  214. contain one entry each.
  215. @param kd_node an unexpanded node in a kd tree
  216. */
  217. void expand_kd_node_subtree( struct kd_node* kd_node )
  218. {
  219. /* base case: leaf node */
  220. if( kd_node->n == 1  ||  kd_node->n == 0 )
  221. {
  222. kd_node->leaf = 1;
  223. return;
  224. }
  225. assign_part_key( kd_node );
  226. partition_features( kd_node );
  227. if( kd_node->kd_left )
  228. expand_kd_node_subtree( kd_node->kd_left );
  229. if( kd_node->kd_right )
  230. expand_kd_node_subtree( kd_node->kd_right );
  231. }
  232. /*
  233. Determines the descriptor index at which and the value with which to
  234. partition a kd tree node's features.
  235. @param kd_node a kd tree node
  236. */
  237. void assign_part_key( struct kd_node* kd_node )
  238. {
  239. struct feature* features;
  240. double kv, x, mean, var, var_max = 0;
  241. double* tmp;
  242. int d, n, i, j, ki = 0;
  243. features = kd_node->features;
  244. n = kd_node->n;
  245. d = features[0].d;
  246. /* partition key index is that along which descriptors have most variance */
  247. for( j = 0; j < d; j++ )
  248. {
  249. mean = var = 0;
  250. for( i = 0; i < n; i++ )
  251. mean += features[i].descr[j];
  252. mean /= n;
  253. for( i = 0; i < n; i++ )
  254. {
  255. x = features[i].descr[j] - mean;
  256. var += x * x;
  257. }
  258. var /= n;
  259. if( var > var_max )
  260. {
  261. ki = j;
  262. var_max = var;
  263. }
  264. }
  265. /* partition key value is median of descriptor values at ki */
  266. tmp = calloc( n, sizeof( double ) );
  267. for( i = 0; i < n; i++ )
  268. tmp[i] = features[i].descr[ki];
  269. kv = median_select( tmp, n );
  270. free( tmp );
  271. kd_node->ki = ki;
  272. kd_node->kv = kv;
  273. }
  274. /*
  275. Finds the median value of an array.  The array's elements are re-ordered
  276. by this function.
  277. @param array an array; the order of its elelemts is reordered
  278. @param n number of elements in array
  279. @return Returns the median value of array.
  280. */
  281. double median_select( double* array, int n )
  282. {
  283. return rank_select( array, n, (n - 1) / 2 );
  284. }
  285. /*
  286. Finds the element of a specified rank in an array using the linear time
  287. median-of-medians algorithm by Blum, Floyd, Pratt, Rivest, and Tarjan.
  288. The elements of the array are re-ordered by this function.
  289. @param array an array; the order of its elelemts is reordered
  290. @param n number of elements in array
  291. @param r the zero-based rank of the element to be selected
  292. @return Returns the element from array with zero-based rank r.
  293. */
  294. double rank_select( double* array, int n, int r )
  295. {
  296. double* tmp, med;
  297. int gr_5, gr_tot, rem_elts, i, j;
  298. /* base case */
  299. if( n == 1 )
  300. return array[0];
  301. /* divide array into groups of 5 and sort them */
  302. gr_5 = n / 5;
  303. gr_tot = cvCeil( n / 5.0 );
  304. rem_elts = n % 5;
  305. tmp = array;
  306. for( i = 0; i < gr_5; i++ )
  307. {
  308. insertion_sort( tmp, 5 );
  309. tmp += 5;
  310. }
  311. insertion_sort( tmp, rem_elts );
  312. /* recursively find the median of the medians of the groups of 5 */
  313. tmp = calloc( gr_tot, sizeof( double ) );
  314. for( i = 0, j = 2; i < gr_5; i++, j += 5 )
  315. tmp[i] = array[j];
  316. if( rem_elts )
  317. tmp[i++] = array[n - 1 - rem_elts/2];
  318. med = rank_select( tmp, i, ( i - 1 ) / 2 );
  319. free( tmp );
  320. /* partition around median of medians and recursively select if necessary */
  321. j = partition_array( array, n, med );
  322. if( r == j )
  323. return med;
  324. else if( r < j )
  325. return rank_select( array, j, r );
  326. else
  327. {
  328. array += j+1;
  329. return rank_select( array, ( n - j - 1 ), ( r - j - 1 ) );
  330. }
  331. }
  332. /*
  333. Sorts an array in place into increasing order using insertion sort.
  334. @param array an array
  335. @param n number of elements
  336. */
  337. void insertion_sort( double* array, int n )
  338. {
  339. double k;
  340. int i, j;
  341. for( i = 1; i < n; i++ )
  342. {
  343. k = array[i];
  344. j = i-1;
  345. while( j >= 0  &&  array[j] > k )
  346. {
  347. array[j+1] = array[j];
  348. j -= 1;
  349. }
  350. array[j+1] = k;
  351. }
  352. }
  353. /*
  354. Partitions an array around a specified value.
  355. @param array an array
  356. @param n number of elements
  357. @param pivot value around which to partition
  358. @return Returns index of the pivot after partitioning
  359. */
  360. int partition_array( double* array, int n, double pivot )
  361. {
  362. double tmp;
  363. int p, i, j;
  364. i = -1;
  365. for( j = 0; j < n; j++ )
  366. if( array[j] <= pivot )
  367. {
  368. tmp = array[++i];
  369. array[i] = array[j];
  370. array[j] = tmp;
  371. if( array[i] == pivot )
  372. p = i;
  373. }
  374. array[p] = array[i];
  375. array[i] = pivot;
  376. return i;
  377. }
  378. /*
  379. Partitions the features at a specified kd tree node to create its two
  380. children.
  381. @param kd_node a kd tree node whose partition key is set
  382. */
  383. void partition_features( struct kd_node* kd_node )
  384. {
  385. struct feature* features, tmp;
  386. double kv;
  387. int n, ki, p, i, j = -1;
  388. features = kd_node->features;
  389. n = kd_node->n;
  390. ki = kd_node->ki;
  391. kv = kd_node->kv;
  392. for( i = 0; i < n; i++ )
  393. if( features[i].descr[ki] <= kv )
  394. {
  395. tmp = features[++j];
  396. features[j] = features[i];
  397. features[i] = tmp;
  398. if( features[j].descr[ki] == kv )
  399. p = j;
  400. }
  401. tmp = features[p];
  402. features[p] = features[j];
  403. features[j] = tmp;
  404. /* if all records fall on same side of partition, make node a leaf */
  405. if( j == n - 1 )
  406. {
  407. kd_node->leaf = 1;
  408. return;
  409. }
  410. kd_node->kd_left = kd_node_init( features, j + 1 );
  411. kd_node->kd_right = kd_node_init( features + ( j + 1 ), ( n - j - 1 ) );
  412. }
  413. /*
  414. Explores a kd tree from a given node to a leaf.  Branching decisions are
  415. made at each node based on the descriptor of a given feature.  Each node
  416. examined but not explored is put into a priority queue to be explored
  417. later, keyed based on the distance from its partition key value to the
  418. given feature's desctiptor.
  419. @param kd_node root of the subtree to be explored
  420. @param feat feature upon which branching decisions are based
  421. @param min_pq a minimizing priority queue into which tree nodes are placed
  422. as described above
  423. @return Returns a pointer to the leaf node at which exploration ends or
  424. NULL on error.
  425. */
  426. struct kd_node* explore_to_leaf( struct kd_node* kd_node, struct feature* feat,
  427. struct min_pq* min_pq )
  428. {
  429. struct kd_node* unexpl, * expl = kd_node;
  430. double kv;
  431. int ki;
  432. while( expl  &&  ! expl->leaf )
  433. {
  434. ki = expl->ki;
  435. kv = expl->kv;
  436. if( ki >= feat->d )
  437. {
  438. fprintf( stderr, "Warning: comparing imcompatible descriptors, %s" 
  439. " line %dn", __FILE__, __LINE__ );
  440. return NULL;
  441. }
  442. if( feat->descr[ki] <= kv )
  443. {
  444. unexpl = expl->kd_right;
  445. expl = expl->kd_left;
  446. }
  447. else
  448. {
  449. unexpl = expl->kd_left;
  450. expl = expl->kd_right;
  451. }
  452. if( minpq_insert( min_pq, unexpl, ABS( kv - feat->descr[ki] ) ) )
  453. {
  454. fprintf( stderr, "Warning: unable to insert into PQ, %s, line %dn",
  455. __FILE__, __LINE__ );
  456. return NULL;
  457. }
  458. }
  459. return expl;
  460. }
  461. /*
  462. Inserts a feature into the nearest-neighbor array so that the array remains
  463. in order of increasing descriptor distance from the search feature.
  464. @param feat feature to be inderted into the array; it's feature_data field
  465. should be a pointer to a bbf_data with d equal to the squared descriptor
  466. distance between feat and the search feature
  467. @param nbrs array of nearest neighbors neighbors
  468. @param n number of elements already in nbrs and
  469. @param k maximum number of elements in nbrs
  470. @return If feat was successfully inserted into nbrs, returns 1; otherwise
  471. returns 0.
  472. */
  473. int insert_into_nbr_array( struct feature* feat, struct feature** nbrs,
  474.   int n, int k )
  475. {
  476. struct bbf_data* fdata, * ndata;
  477. double dn, df;
  478. int i, ret = 0;
  479. if( n == 0 )
  480. {
  481. nbrs[0] = feat;
  482. return 1;
  483. }
  484. /* check at end of array */
  485. fdata = (struct bbf_data*)feat->feature_data;
  486. df = fdata->d;
  487. ndata = (struct bbf_data*)nbrs[n-1]->feature_data;
  488. dn = ndata->d;
  489. if( df >= dn )
  490. {
  491. if( n == k )
  492. {
  493. feat->feature_data = fdata->old_data;
  494. free( fdata );
  495. return 0;
  496. }
  497. nbrs[n] = feat;
  498. return 1;
  499. }
  500. /* find the right place in the array */
  501. if( n < k )
  502. {
  503. nbrs[n] = nbrs[n-1];
  504. ret = 1;
  505. }
  506. else
  507. {
  508. nbrs[n-1]->feature_data = ndata->old_data;
  509. free( ndata );
  510. }
  511. i = n-2;
  512. while( i >= 0 )
  513. {
  514. ndata = (struct bbf_data*)nbrs[i]->feature_data;
  515. dn = ndata->d;
  516. if( dn <= df )
  517. break;
  518. nbrs[i+1] = nbrs[i];
  519. i--;
  520. }
  521. i++;
  522. nbrs[i] = feat;
  523. return ret;
  524. }
  525. /*
  526. Determines whether a given point lies within a specified rectangular region
  527. @param pt point
  528. @param rect rectangular region
  529. @return Returns 1 if pt is inside rect or 0 otherwise
  530. */
  531. int within_rect( CvPoint2D64f pt, CvRect rect )
  532. {
  533. if( pt.x < rect.x  ||  pt.y < rect.y )
  534. return 0;
  535. if( pt.x > rect.x + rect.width  ||  pt.y > rect.y + rect.height )
  536. return 0;
  537. return 1;
  538. }