xform.c

Rob Hess Linux下的SIFT提取源码

}


/************************ Local funciton definitions *************************/

/*
  Returns a feature's match according to a specified match type

  @param feat feature
  @param mtype match type, one of FEATURE_FWD_MATCH, FEATURE_BCK_MATCH, or
    FEATURE_MDL_MATCH

  @return Returns feat's match corresponding to mtype or NULL for bad mtype
*/
static inline struct feature* get_match( struct feature* feat, int mtype )
{
  if( mtype == FEATURE_MDL_MATCH )
    return feat->mdl_match;
  if( mtype == FEATURE_BCK_MATCH )
    return feat->bck_match;
  if( mtype == FEATURE_FWD_MATCH )
    return feat->fwd_match;
  return NULL;
}



/*
  Finds all features with a match of a specified type and stores pointers
  to them in an array.  Additionally initializes each matched feature's
  feature_data field with a ransac_data structure.

  @param features array of features
  @param n number of features in features
  @param mtype match type, one of FEATURE_{FWD,BCK,MDL}_MATCH
  @param matched output as an array of pointers to features with a match of
    the specified type

  @return Returns the number of features output in matched.
*/
int get_matched_features( struct feature* features, int n, int mtype,
			  struct feature*** matched )
{
  struct feature** _matched;
  struct ransac_data* rdata;
  int i, m = 0;

  _matched = calloc( n, sizeof( struct feature* ) );
  for( i = 0; i < n; i++ )
    if( get_match( features + i, mtype ) )
      {
	rdata = malloc( sizeof( struct ransac_data ) );
	memset( rdata, 0, sizeof( struct ransac_data ) );
	rdata->orig_feat_data = features[i].feature_data;
	_matched[m] = features + i;
	_matched[m]->feature_data = rdata;
	m++;
      }
  *matched = _matched;
  return m;
}



/*
  Calculates the minimum number of inliers as a function of the number of
  putative correspondences.  Based on equation (7) in
  
  Chum, O. and Matas, J.  Matching with PROSAC -- Progressive Sample Consensus.
  In <EM>Conference on Computer Vision and Pattern Recognition (CVPR)</EM>,
  (2005), pp. 220--226.

  @param n number of putative correspondences
  @param m min number of correspondences to compute the model in question
  @param p_badsupp prob. that a bad model is supported by a correspondence
  @param p_badxform desired prob. that the final transformation returned is bad
  
  @return Returns the minimum number of inliers required to guarantee, based
    on p_badsupp, that the probability that the final transformation returned
    by RANSAC is less than p_badxform
*/
int calc_min_inliers( int n, int m, double p_badsupp, double p_badxform )
{
  double pi, sum;
  int i, j;

  for( j = m+1; j <= n; j++ )
    {
      sum = 0;
      for( i = j; i <= n; i++ )
	{
	  pi = (i-m) * log( p_badsupp ) + (n-i+m) * log( 1.0 - p_badsupp ) +
	    gsl_sf_lnchoose( n - m, i - m );
	  sum += exp( pi );
	}
      if( sum < p_badxform )
	break;
    }
  return j;
}



/*
  Draws a RANSAC sample from a set of features.

  @param features array of pointers to features from which to sample
  @param n number of features in features
  @param m size of the sample
  @param rng random number generator used to sample

  @return Returns an array of pointers to the sampled features; the sampled
    field of each sampled feature's ransac_data is set to 1
*/
struct feature** draw_ransac_sample( struct feature** features, int n,
				     int m, gsl_rng* rng )
{
  struct feature** sample, * feat;
  struct ransac_data* rdata;
  int i, x;

  for( i = 0; i < n; i++ )
    {
      rdata = feat_ransac_data( features[i] );
      rdata->sampled = 0;
    }

  sample = calloc( m, sizeof( struct feature* ) );
  for( i = 0; i < m; i++ )
    {
      do
	{
	  x = gsl_rng_uniform_int( rng, n );
	  feat = features[x];
	  rdata = feat_ransac_data( feat );
	}
      while( rdata->sampled );
      sample[i] = feat;
      rdata->sampled = 1;
    }

  return sample;
}



/*
  Extrancs raw point correspondence locations from a set of features

  @param features array of features from which to extract points and match
    points; each of these is assumed to have a match of type mtype
  @param n number of features
  @param mtype match type; if FEATURE_MDL_MATCH correspondences are assumed
    to be between each feature's img_pt field and it's match's mdl_pt field,
    otherwise, correspondences are assumed to be between img_pt and img_pt
  @param pts output as an array of raw point locations from features
  @param mpts output as an array of raw point locations from features' matches
*/
void extract_corresp_pts( struct feature** features, int n, int mtype,
			  CvPoint2D64f** pts, CvPoint2D64f** mpts )
{
  struct feature* match;
  CvPoint2D64f* _pts, * _mpts;
  int i;

  _pts = calloc( n, sizeof( CvPoint2D64f ) );
  _mpts = calloc( n, sizeof( CvPoint2D64f ) );

  if( mtype == FEATURE_MDL_MATCH )
    for( i = 0; i < n; i++ )
      {
	match = get_match( features[i], mtype );
	if( ! match )
	  fatal_error( "feature does not have match of type %d, %s line %d",
		       mtype, __FILE__, __LINE__ );
	_pts[i] = features[i]->img_pt;
	_mpts[i] = match->mdl_pt;
      }

  else
    for( i = 0; i < n; i++ )
      {
	match = get_match( features[i], mtype );
	if( ! match )
	  fatal_error( "feature does not have match of type %d, %s line %d",
		       mtype, __FILE__, __LINE__ );
	_pts[i] = features[i]->img_pt;
	_mpts[i] = match->img_pt;
      }

  *pts = _pts;
  *mpts = _mpts;
}



/*
  For a given model and error function, finds a consensus from a set of
  feature correspondences.

  @param features set of pointers to features; every feature is assumed to
    have a match of type mtype
  @param n number of features in features
  @param mtype determines the match field of each feature against which to
    measure error; if this is FEATURE_MDL_MATCH, correspondences are assumed
    to be between the feature's img_pt field and the match's mdl_pt field;
    otherwise matches are assumed to be between img_pt and img_pt
  @param M model for which a consensus set is being found
  @param err_fn error function used to measure distance from M
  @param err_tol correspondences within this distance of M are added to the
    consensus set
  @param consensus output as an array of pointers to features in the
    consensus set

  @return Returns the number of points in the consensus set
*/
int find_consensus( struct feature** features, int n, int mtype,
		    CvMat* M, ransac_err_fn err_fn, double err_tol,
		    struct feature*** consensus )
{
  struct feature** _consensus;
  struct feature* match;
  CvPoint2D64f pt, mpt;
  double err;
  int i, in = 0;

  _consensus = calloc( n, sizeof( struct feature* ) );

  if( mtype == FEATURE_MDL_MATCH )
    for( i = 0; i < n; i++ )
      {
	match = get_match( features[i], mtype );
	if( ! match )
	  fatal_error( "feature does not have match of type %d, %s line %d",
		       mtype, __FILE__, __LINE__ );
	pt = features[i]->img_pt;
	mpt = match->mdl_pt;
	err = err_fn( pt, mpt, M );
	if( err <= err_tol )
	  _consensus[in++] = features[i];
      }

  else
    for( i = 0; i < n; i++ )
      {
	match = get_match( features[i], mtype );
	if( ! match )
	  fatal_error( "feature does not have match of type %d, %s line %d",
		       mtype, __FILE__, __LINE__ );
	pt = features[i]->img_pt;
	mpt = match->img_pt;
	err = err_fn( pt, mpt, M );
	if( err <= err_tol )
	  _consensus[in++] = features[i];
      }
  *consensus = _consensus;
  return in;
}



/*
  Releases memory and reduces code size above

  @param pts1 an array of points
  @param pts2 an array of points
  @param features an array of pointers to features; can be NULL
*/
static inline void release_mem( CvPoint2D64f* pts1, CvPoint2D64f* pts2,
				struct feature** features )
{
  free( pts1 );
  free( pts2 );
  if( features )
    free( features );
}