kdtree.c

Rob Hess Linux下的SIFT提取源码

  for( i = 0; i < n; i++ )
    tmp[i] = features[i].descr[ki];
  kv = median_select( tmp, n );
  free( tmp );

  kd_node->ki = ki;
  kd_node->kv = kv;
}



/*
  Finds the median value of an array.  The array's elements are re-ordered
  by this function.

  @param array an array; the order of its elelemts is reordered
  @param n number of elements in array

  @return Returns the median value of array.
*/
double median_select( double* array, int n )
{
  return rank_select( array, n, (n - 1) / 2 );
}



/*
  Finds the element of a specified rank in an array using the linear time
  median-of-medians algorithm by Blum, Floyd, Pratt, Rivest, and Tarjan.
  The elements of the array are re-ordered by this function.

  @param array an array; the order of its elelemts is reordered
  @param n number of elements in array
  @param r the zero-based rank of the element to be selected
  
  @return Returns the element from array with zero-based rank r.
*/
double rank_select( double* array, int n, int r )
{
  double* tmp, med;
  int gr_5, gr_tot, rem_elts, i, j;

  /* base case */
  if( n == 1 )
    return array[0];

  /* divide array into groups of 5 and sort them */
  gr_5 = n / 5;
  gr_tot = cvCeil( n / 5.0 );
  rem_elts = n % 5;
  tmp = array;
  for( i = 0; i < gr_5; i++ )
    {
      insertion_sort( tmp, 5 );
      tmp += 5;
    }
  insertion_sort( tmp, rem_elts );

  /* recursively find the median of the medians of the groups of 5 */
  tmp = calloc( gr_tot, sizeof( double ) );
  for( i = 0, j = 2; i < gr_5; i++, j += 5 )
    tmp[i] = array[j];
  if( rem_elts )
    tmp[i++] = array[n - 1 - rem_elts/2];
  med = rank_select( tmp, i, ( i - 1 ) / 2 );
  free( tmp );
  
  /* partition around median of medians and recursively select if necessary */
  j = partition_array( array, n, med );
  if( r == j )
    return med;
  else if( r < j )
    return rank_select( array, j, r );
  else
    {
      array += j+1;
      return rank_select( array, ( n - j - 1 ), ( r - j - 1 ) );
    }
}



/*
  Sorts an array in place into increasing order using insertion sort.

  @param array an array
  @param n number of elements
*/
void insertion_sort( double* array, int n )
{
  double k;
  int i, j;

  for( i = 1; i < n; i++ )
    {
      k = array[i];
      j = i-1;
      while( j >= 0  &&  array[j] > k )
	{
	  array[j+1] = array[j];
	  j -= 1;
	}
      array[j+1] = k;
    }
}



/*
  Partitions an array around a specified value.

  @param array an array
  @param n number of elements
  @param pivot value around which to partition

  @return Returns index of the pivot after partitioning
*/
int partition_array( double* array, int n, double pivot )
{
  double tmp;
  int p, i, j;

  i = -1;
  for( j = 0; j < n; j++ )
    if( array[j] <= pivot )
      {
	tmp = array[++i];
	array[i] = array[j];
	array[j] = tmp;
	if( array[i] == pivot )
	  p = i;
      }
  array[p] = array[i];
  array[i] = pivot;
  
  return i;
}



/*
  Partitions the features at a specified kd tree node to create its two
  children.

  @param kd_node a kd tree node whose partition key is set
*/
void partition_features( struct kd_node* kd_node )
{
  struct feature* features, tmp;
  double kv;
  int n, ki, p, i, j = -1;

  features = kd_node->features;
  n = kd_node->n;
  ki = kd_node->ki;
  kv = kd_node->kv;
  for( i = 0; i < n; i++ )
    if( features[i].descr[ki] <= kv )
      {
	tmp = features[++j];
	features[j] = features[i];
	features[i] = tmp;
	if( features[j].descr[ki] == kv )
	  p = j;
      }
  tmp = features[p];
  features[p] = features[j];
  features[j] = tmp;

  /* if all records fall on same side of partition, make node a leaf */
  if( j == n - 1 )
    {
      kd_node->leaf = 1;
      return;
    }

  kd_node->kd_left = kd_node_init( features, j + 1 );
  kd_node->kd_right = kd_node_init( features + ( j + 1 ), ( n - j - 1 ) );
}



/*
  Explores a kd tree from a given node to a leaf.  Branching decisions are
  made at each node based on the descriptor of a given feature.  Each node
  examined but not explored is put into a priority queue to be explored
  later, keyed based on the distance from its partition key value to the
  given feature's desctiptor.
  
  @param kd_node root of the subtree to be explored
  @param feat feature upon which branching decisions are based
  @param min_pq a minimizing priority queue into which tree nodes are placed
    as described above

  @return Returns a pointer to the leaf node at which exploration ends or
    NULL on error.
*/
struct kd_node* explore_to_leaf( struct kd_node* kd_node, struct feature* feat,
				 struct min_pq* min_pq )
{
  struct kd_node* unexpl, * expl = kd_node;
  double kv;
  int ki;

  while( expl  &&  ! expl->leaf )
    {
      ki = expl->ki;
      kv = expl->kv;
      
      if( ki >= feat->d )
	{
	  fprintf( stderr, "Warning: comparing imcompatible descriptors, %s" \
		   " line %d\n", __FILE__, __LINE__ );
	  return NULL;
	}
      if( feat->descr[ki] <= kv )
	{
	  unexpl = expl->kd_right;
	  expl = expl->kd_left;
	}
      else
	{
	  unexpl = expl->kd_left;
	  expl = expl->kd_right;
	}
      
      if( minpq_insert( min_pq, unexpl, ABS( kv - feat->descr[ki] ) ) )
	{
	  fprintf( stderr, "Warning: unable to insert into PQ, %s, line %d\n",
		   __FILE__, __LINE__ );
	  return NULL;
	}
    }

  return expl;
}



/*
  Inserts a feature into the nearest-neighbor array so that the array remains
  in order of increasing descriptor distance from the search feature.

  @param feat feature to be inderted into the array; it's feature_data field
    should be a pointer to a bbf_data with d equal to the squared descriptor
    distance between feat and the search feature
  @param nbrs array of nearest neighbors neighbors
  @param n number of elements already in nbrs and
  @param k maximum number of elements in nbrs

  @return If feat was successfully inserted into nbrs, returns 1; otherwise
    returns 0.
*/
int insert_into_nbr_array( struct feature* feat, struct feature** nbrs,
			   int n, int k )
{
  struct bbf_data* fdata, * ndata;
  double dn, df;
  int i, ret = 0;

  if( n == 0 )
    {
      nbrs[0] = feat;
      return 1;
    }

  /* check at end of array */
  fdata = (struct bbf_data*)feat->feature_data;
  df = fdata->d;
  ndata = (struct bbf_data*)nbrs[n-1]->feature_data;
  dn = ndata->d;
  if( df >= dn )
    {
      if( n == k )
	{
	  feat->feature_data = fdata->old_data;
	  free( fdata );
	  return 0;
	}
      nbrs[n] = feat;
      return 1;
    }

  /* find the right place in the array */
  if( n < k )
    {
      nbrs[n] = nbrs[n-1];
      ret = 1;
    }
  else
    {
      nbrs[n-1]->feature_data = ndata->old_data;
      free( ndata );
    }
  i = n-2;
  while( i >= 0 )
    {
      ndata = (struct bbf_data*)nbrs[i]->feature_data;
      dn = ndata->d;
      if( dn <= df )
	break;
      nbrs[i+1] = nbrs[i];
      i--;
    }
  i++;
  nbrs[i] = feat;

  return ret;
}



/*
  Determines whether a given point lies within a specified rectangular region

  @param pt point
  @param rect rectangular region

  @return Returns 1 if pt is inside rect or 0 otherwise
*/
int within_rect( CvPoint2D64f pt, CvRect rect )
{
  if( pt.x < rect.x  ||  pt.y < rect.y )
    return 0;
  if( pt.x > rect.x + rect.width  ||  pt.y > rect.y + rect.height )
    return 0;
  return 1;
}