/** a node in a k-d tree */

struct kd_node{

int ki;                      /**< partition key index */

double kv;                   /**< partition key value */

int leaf;                    /**< 1 if node is a leaf, 0 otherwise */

struct feature* features;    /**< features at this node */

int n;                       /**< number of features */

struct kd_node* kd_left;     /**< left child */

struct kd_node* kd_right;    /**< right child */

};

min_pq = minpq_init();

minpq_insert( min_pq, kd_root, 0 );

while( min_pq->n > 0  &&  t < max_nn_chks ) //队列里有东西就继续搜，同时控制在t<200（即200步内）

{

expl = (struct kd_node*)minpq_extract_min( min_pq ); //取出最小的，front & pop

expl = explore_to_leaf( expl, feat, min_pq ); //从该点开始，explore到leaf，路过的“有意义的点”就塞到最小队列min_pq中。

for( i = 0; i < expl->n; i++ ) //

{

tree_feat = &expl->features[i];

bbf_data->old_data = tree_feat->feature_data;

bbf_data->d = descr_dist_sq(feat, tree_feat); //两feat均方差

tree_feat->feature_data = bbf_data;

n += insert_into_nbr_array( tree_feat, _nbrs, n, k ); //按从小到大塞到neighbor数组里，到时候取前k个就是 KNN 咯~ n 每次加1或0，表示目前已有的元素个数

}

t++;

}

struct kd_node* explore_to_leaf( struct kd_node* kd_node, struct feature* feat,

struct min_pq* min_pq )//expl, feat, min_pq

{

struct kd_node* unexpl, * expl = kd_node;

double kv;

int ki;

while( expl  &&  ! expl->leaf )

{

ki = expl->ki;

kv = expl->kv;

if( feat->descr[ki] <= kv ) {

unexpl = expl->kd_right;

expl = expl->kd_left; //走左边，右边点将被记下来

}

else {

unexpl = expl->kd_left;

expl = expl->kd_right; //走右边，左边点将被记下来

}

minpq_insert( min_pq, unexpl, ABS( kv - feat->descr[ki] ) ) ;//将这些点插入进来，key键值为|kv - feat->descr[ki]| 即第ki维上的差值

}

return expl;

}

d0 = descr_dist_sq( feat, nbrs[0] );//计算两特征间squared Euclidian distance

d1 = descr_dist_sq( feat, nbrs[1] );

if( d0 < d1 * NN_SQ_DIST_RATIO_THR )//如果d0/d1小于阈值0.49

{

pt1 = cvPoint( cvRound( feat->x ), cvRound( feat->y ) );

pt2 = cvPoint( cvRound( nbrs[0]->x ), cvRound( nbrs[0]->y ) );

pt2.y += img1->height;

cvLine( stacked, pt1, pt2, CV_RGB(255,0,255), 1, 8, 0 );//画线

m++;//matches个数

feat1[i].fwd_match = nbrs[0];

}

nm = get_matched_features( features, n, mtype, &matched );

/* initialize random number generator */

rng = gsl_rng_alloc( gsl_rng_mt19937 );

gsl_rng_set( rng, time(NULL) );

in_min = calc_min_inliers( nm, m, RANSAC_PROB_BAD_SUPP, p_badxform ); //符合这一要求的内点至少得有多少个

p = pow( 1.0 - pow( in_frac, m ), k );

i = 0;

while( p > p_badxform )//p>0.01

{

sample = draw_ransac_sample( matched, nm, m, rng );

extract_corresp_pts( sample, m, mtype, &pts, &mpts );

M = xform_fn( pts, mpts, m );

if( ! M )

goto iteration_end;

in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);

if( in > in_max )  {

if( consensus_max )

free( consensus_max );

consensus_max = consensus;

in_max = in;

in_frac = (double)in_max / nm;

}

else

free( consensus );

cvReleaseMat( &M );

iteration_end:

release_mem( pts, mpts, sample );

p = pow( 1.0 - pow( in_frac, m ), ++k );

}

/* calculate final transform based on best consensus set */

if( in_max >= in_min )

{

extract_corresp_pts( consensus_max, in_max, mtype, &pts, &mpts );

M = xform_fn( pts, mpts, in_max );

in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);

cvReleaseMat( &M );

release_mem( pts, mpts, consensus_max );

extract_corresp_pts( consensus, in, mtype, &pts, &mpts );

M = xform_fn( pts, mpts, in );