00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
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
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
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
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
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
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
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
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
00225
00226
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
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
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
00267
00268
00269
00270
00271 void expand_kd_node_subtree( struct kd_node* kd_node )
00272 {
00273
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
00293
00294
00295
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
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
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
00344
00345
00346
00347
00348
00349
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
00360
00361
00362
00363
00364
00365
00366
00367
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
00375 if( n == 1 )
00376 return array[0];
00377
00378
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
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
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
00416
00417
00418
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
00442
00443
00444
00445
00446
00447
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
00474
00475
00476
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
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
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
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
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
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
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
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
00646
00647
00648
00649
00650
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 }