PARP Research Group University of Murcia, Spain


examples/OpenCV/siftDetector/hess/xform.cpp

00001 /*
00002   This file contains definitions for functions to compute transforms from
00003   image feature correspondences
00004   
00005   Copyright (C) 2006-2007  Rob Hess <hess@eecs.oregonstate.edu>
00006 
00007   @version 1.1.1-20070913
00008 */
00009 
00010 #include "xform.h"
00011 #include "imgfeatures.h"
00012 #include "utils.h"
00013 
00014 #include <cxcore.h>
00015 
00016 #include <gsl/gsl_sf.h>
00017 #include <gsl/gsl_rng.h>
00018 #include <gsl/gsl_randist.h>
00019 
00020 #include <time.h>
00021 
00022 /************************* Local Function Prototypes *************************/
00023 
00024 static inline struct feature* get_match( struct feature*, int );
00025 int get_matched_features( struct feature*, int, int, struct feature*** );
00026 int calc_min_inliers( int, int, double, double );
00027 struct feature** draw_ransac_sample( struct feature**, int, int, gsl_rng* );
00028 void extract_corresp_pts( struct feature**, int, int, CvPoint2D64f**,
00029                           CvPoint2D64f** );
00030 int find_consensus( struct feature**, int, int, CvMat*, ransac_err_fn,
00031                     double, struct feature*** );
00032 static inline void release_mem( CvPoint2D64f*, CvPoint2D64f*,
00033                                 struct feature** );
00034 
00035 /********************** Functions prototyped in model.h **********************/
00036 
00037 
00038 /*
00039   Calculates a best-fit image transform from image feature correspondences
00040   using RANSAC.
00041   
00042   For more information refer to:
00043   
00044   Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
00045   model fitting with applications to image analysis and automated cartography.
00046   <EM>Communications of the ACM, 24</EM>, 6 (1981), pp. 381--395.
00047   
00048   @param features an array of features; only features with a non-NULL match
00049     of type mtype are used in homography computation
00050   @param n number of features in feat
00051   @param mtype determines which of each feature's match fields to use
00052     for model computation; should be one of FEATURE_FWD_MATCH,
00053     FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
00054     correspondences are assumed to be between a feature's img_pt field
00055     and its match's mdl_pt field, otherwise correspondences are assumed to
00056     be between the the feature's img_pt field and its match's img_pt field
00057   @param xform_fn pointer to the function used to compute the desired
00058     transformation from feature correspondences
00059   @param m minimum number of correspondences necessary to instantiate the
00060     model computed by xform_fn
00061   @param p_badxform desired probability that the final transformation
00062     returned by RANSAC is corrupted by outliers (i.e. the probability that
00063     no samples of all inliers were drawn)
00064   @param err_fn pointer to the function used to compute a measure of error
00065     between putative correspondences and a computed model
00066   @param err_tol correspondences within this distance of a computed model are
00067     considered as inliers
00068   @param inliers if not NULL, output as an array of pointers to the final
00069     set of inliers
00070   @param n_in if not NULL and \a inliers is not NULL, output as the final
00071     number of inliers
00072   
00073   @return Returns a transformation matrix computed using RANSAC or NULL
00074     on error or if an acceptable transform could not be computed.
00075 */
00076 CvMat* ransac_xform( struct feature* features, int n, int mtype,
00077                      ransac_xform_fn xform_fn, int m, double p_badxform,
00078                      ransac_err_fn err_fn, double err_tol,
00079                      struct feature*** inliers, int* n_in )
00080 {
00081   struct feature** matched, ** sample, ** consensus, ** consensus_max = NULL;
00082   struct ransac_data* rdata;
00083   CvPoint2D64f* pts, * mpts;
00084   CvMat* M = NULL;
00085   gsl_rng* rng;
00086   double p, in_frac = RANSAC_INLIER_FRAC_EST;
00087   int i, nm, in, in_min, in_max = 0, k = 0;
00088 
00089   nm = get_matched_features( features, n, mtype, &matched );
00090   if( nm < m )
00091     {
00092       fprintf( stderr, "Warning: not enough matches to compute xform, %s" \
00093                " line %d\n", __FILE__, __LINE__ );
00094       goto end;
00095     }
00096 
00097   /* initialize random number generator */
00098   rng = gsl_rng_alloc( gsl_rng_mt19937 );
00099   gsl_rng_set( rng, time(NULL) );
00100 
00101   in_min = calc_min_inliers( nm, m, RANSAC_PROB_BAD_SUPP, p_badxform );
00102   p = pow( 1.0 - pow( in_frac, m ), k );
00103   while( p > p_badxform )
00104     {
00105       sample = draw_ransac_sample( matched, nm, m, rng );
00106       extract_corresp_pts( sample, m, mtype, &pts, &mpts );
00107       M = xform_fn( pts, mpts, m );
00108       if( ! M )
00109         goto iteration_end;
00110       in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
00111       if( in > in_max )
00112         {
00113           if( consensus_max )
00114             free( consensus_max );
00115           consensus_max = consensus;
00116           in_max = in;
00117           in_frac = (double)in_max / nm;
00118         }
00119       else
00120         free( consensus );
00121       cvReleaseMat( &M );
00122 
00123     iteration_end:
00124       release_mem( pts, mpts, sample );
00125       p = pow( 1.0 - pow( in_frac, m ), ++k );
00126     }
00127 
00128   /* calculate final transform based on best consensus set */
00129   if( in_max >= in_min )
00130     {
00131       extract_corresp_pts( consensus_max, in_max, mtype, &pts, &mpts );
00132       M = xform_fn( pts, mpts, in_max );
00133       in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
00134       cvReleaseMat( &M );
00135       release_mem( pts, mpts, consensus_max );
00136       extract_corresp_pts( consensus, in, mtype, &pts, &mpts );
00137       M = xform_fn( pts, mpts, in );
00138       if( inliers )
00139         {
00140           *inliers = consensus;
00141           consensus = NULL;
00142         }
00143       if( n_in )
00144         *n_in = in;
00145       release_mem( pts, mpts, consensus );
00146     }
00147   else if( consensus_max )
00148     {
00149       if( inliers )
00150         *inliers = NULL;
00151       if( n_in )
00152         *n_in = 0;
00153       free( consensus_max );
00154     }
00155 
00156   gsl_rng_free( rng );
00157  end:
00158   for( i = 0; i < nm; i++ )
00159     {
00160       rdata = feat_ransac_data( matched[i] );
00161       matched[i]->feature_data = rdata->orig_feat_data;
00162       free( rdata );
00163     }
00164   free( matched );
00165   return M;
00166 }
00167 
00168 
00169 
00170 /*
00171   Calculates a least-squares planar homography from point correspondeces.
00172   
00173   @param pts array of points
00174   @param mpts array of corresponding points; each pts[i], i=1..n, corresponds
00175     to mpts[i]
00176   @param n number of points in both pts and mpts; must be at least 4
00177   
00178   @return Returns the 3 x 3 least-squares planar homography matrix that
00179     transforms points in pts to their corresponding points in mpts or NULL if
00180     fewer than 4 correspondences were provided
00181 */
00182 CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n )
00183 {
00184   CvMat* H, * A, * B, X;
00185   double x[9];
00186   int i;
00187 
00188   if( n < 4 )
00189     {
00190       fprintf( stderr, "Warning: too few points in lsq_homog(), %s line %d\n",
00191                __FILE__, __LINE__ );
00192       return NULL;
00193     }
00194 
00195   /* set up matrices so we can unstack homography into X; AX = B */
00196   A = cvCreateMat( 2*n, 8, CV_64FC1 );
00197   B = cvCreateMat( 2*n, 1, CV_64FC1 );
00198   X = cvMat( 8, 1, CV_64FC1, x );
00199   H = cvCreateMat(3, 3, CV_64FC1);
00200   cvZero( A );
00201   for( i = 0; i < n; i++ )
00202     {
00203       cvmSet( A, i, 0, pts[i].x );
00204       cvmSet( A, i+n, 3, pts[i].x );
00205       cvmSet( A, i, 1, pts[i].y );
00206       cvmSet( A, i+n, 4, pts[i].y );
00207       cvmSet( A, i, 2, 1.0 );
00208       cvmSet( A, i+n, 5, 1.0 );
00209       cvmSet( A, i, 6, -pts[i].x * mpts[i].x );
00210       cvmSet( A, i, 7, -pts[i].y * mpts[i].x );
00211       cvmSet( A, i+n, 6, -pts[i].x * mpts[i].y );
00212       cvmSet( A, i+n, 7, -pts[i].y * mpts[i].y );
00213       cvmSet( B, i, 0, mpts[i].x );
00214       cvmSet( B, i+n, 0, mpts[i].y );
00215     }
00216   cvSolve( A, B, &X, CV_SVD );
00217   x[8] = 1.0;
00218   X = cvMat( 3, 3, CV_64FC1, x );
00219   cvConvert( &X, H );
00220 
00221   cvReleaseMat( &A );
00222   cvReleaseMat( &B );
00223   return H;
00224 }
00225 
00226 
00227 
00228 /*
00229   Calculates the transfer error between a point and its correspondence for
00230   a given homography, i.e. for a point x, it's correspondence x', and
00231   homography H, computes d(x', Hx)^2.
00232   
00233   @param pt a point
00234   @param mpt pt's correspondence
00235   @param H a homography matrix
00236   
00237   @return Returns the transfer error between pt and mpt given H
00238 */
00239 double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H )
00240 {
00241   CvPoint2D64f xpt = persp_xform_pt( pt, H );
00242   
00243   return sqrt( dist_sq_2D( xpt, mpt ) );
00244 }
00245 
00246 
00247 
00248 /*
00249   Performs a perspective transformation on a single point.  That is, for a
00250   point (x, y) and a 3 x 3 matrix T this function returns the point
00251   (u, v), where
00252   
00253   [x' y' w']^T = T * [x y 1]^T,
00254   
00255   and
00256   
00257   (u, v) = (x'/w', y'/w').
00258 
00259   Note that affine transforms are a subset of perspective transforms.
00260   
00261   @param pt a 2D point
00262   @param T a perspective transformation matrix
00263   
00264   @return Returns the point (u, v) as above.
00265 */
00266 CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T )
00267 {
00268   CvMat XY, UV;
00269   double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 };
00270   CvPoint2D64f rslt;
00271 
00272   cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP );
00273   cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP );
00274   cvMatMul( T, &XY, &UV );
00275   rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] );
00276 
00277   return rslt;
00278 }
00279 
00280 
00281 /************************ Local funciton definitions *************************/
00282 
00283 /*
00284   Returns a feature's match according to a specified match type
00285 
00286   @param feat feature
00287   @param mtype match type, one of FEATURE_FWD_MATCH, FEATURE_BCK_MATCH, or
00288     FEATURE_MDL_MATCH
00289 
00290   @return Returns feat's match corresponding to mtype or NULL for bad mtype
00291 */
00292 static inline struct feature* get_match( struct feature* feat, int mtype )
00293 {
00294   if( mtype == FEATURE_MDL_MATCH )
00295     return feat->mdl_match;
00296   if( mtype == FEATURE_BCK_MATCH )
00297     return feat->bck_match;
00298   if( mtype == FEATURE_FWD_MATCH )
00299     return feat->fwd_match;
00300   return NULL;
00301 }
00302 
00303 
00304 
00305 /*
00306   Finds all features with a match of a specified type and stores pointers
00307   to them in an array.  Additionally initializes each matched feature's
00308   feature_data field with a ransac_data structure.
00309 
00310   @param features array of features
00311   @param n number of features in features
00312   @param mtype match type, one of FEATURE_{FWD,BCK,MDL}_MATCH
00313   @param matched output as an array of pointers to features with a match of
00314     the specified type
00315 
00316   @return Returns the number of features output in matched.
00317 */
00318 int get_matched_features( struct feature* features, int n, int mtype,
00319                           struct feature*** matched )
00320 {
00321   struct feature** _matched;
00322   struct ransac_data* rdata;
00323   int i, m = 0;
00324 
00325   _matched = (struct feature **) calloc( n, sizeof( struct feature* ) );
00326   for( i = 0; i < n; i++ )
00327     if( get_match( features + i, mtype ) )
00328       {
00329         rdata = (struct ransac_data *) malloc( sizeof( struct ransac_data ) );
00330         memset( rdata, 0, sizeof( struct ransac_data ) );
00331         rdata->orig_feat_data = features[i].feature_data;
00332         _matched[m] = features + i;
00333         _matched[m]->feature_data = rdata;
00334         m++;
00335       }
00336   *matched = _matched;
00337   return m;
00338 }
00339 
00340 
00341 
00342 /*
00343   Calculates the minimum number of inliers as a function of the number of
00344   putative correspondences.  Based on equation (7) in
00345   
00346   Chum, O. and Matas, J.  Matching with PROSAC -- Progressive Sample Consensus.
00347   In <EM>Conference on Computer Vision and Pattern Recognition (CVPR)</EM>,
00348   (2005), pp. 220--226.
00349 
00350   @param n number of putative correspondences
00351   @param m min number of correspondences to compute the model in question
00352   @param p_badsupp prob. that a bad model is supported by a correspondence
00353   @param p_badxform desired prob. that the final transformation returned is bad
00354   
00355   @return Returns the minimum number of inliers required to guarantee, based
00356     on p_badsupp, that the probability that the final transformation returned
00357     by RANSAC is less than p_badxform
00358 */
00359 int calc_min_inliers( int n, int m, double p_badsupp, double p_badxform )
00360 {
00361   double pi, sum;
00362   int i, j;
00363 
00364   for( j = m+1; j <= n; j++ )
00365     {
00366       sum = 0;
00367       for( i = j; i <= n; i++ )
00368         {
00369           pi = (i-m) * log( p_badsupp ) + (n-i+m) * log( 1.0 - p_badsupp ) +
00370             gsl_sf_lnchoose( n - m, i - m );
00371           sum += exp( pi );
00372         }
00373       if( sum < p_badxform )
00374         break;
00375     }
00376   return j;
00377 }
00378 
00379 
00380 
00381 /*
00382   Draws a RANSAC sample from a set of features.
00383 
00384   @param features array of pointers to features from which to sample
00385   @param n number of features in features
00386   @param m size of the sample
00387   @param rng random number generator used to sample
00388 
00389   @return Returns an array of pointers to the sampled features; the sampled
00390     field of each sampled feature's ransac_data is set to 1
00391 */
00392 struct feature** draw_ransac_sample( struct feature** features, int n,
00393                                      int m, gsl_rng* rng )
00394 {
00395   struct feature** sample, * feat;
00396   struct ransac_data* rdata;
00397   int i, x;
00398 
00399   for( i = 0; i < n; i++ )
00400     {
00401       rdata = feat_ransac_data( features[i] );
00402       rdata->sampled = 0;
00403     }
00404 
00405   sample = (struct feature **) calloc( m, sizeof( struct feature* ) );
00406   for( i = 0; i < m; i++ )
00407     {
00408       do
00409         {
00410           x = gsl_rng_uniform_int( rng, n );
00411           feat = features[x];
00412           rdata = feat_ransac_data( feat );
00413         }
00414       while( rdata->sampled );
00415       sample[i] = feat;
00416       rdata->sampled = 1;
00417     }
00418 
00419   return sample;
00420 }
00421 
00422 
00423 
00424 /*
00425   Extrancs raw point correspondence locations from a set of features
00426 
00427   @param features array of features from which to extract points and match
00428     points; each of these is assumed to have a match of type mtype
00429   @param n number of features
00430   @param mtype match type; if FEATURE_MDL_MATCH correspondences are assumed
00431     to be between each feature's img_pt field and it's match's mdl_pt field,
00432     otherwise, correspondences are assumed to be between img_pt and img_pt
00433   @param pts output as an array of raw point locations from features
00434   @param mpts output as an array of raw point locations from features' matches
00435 */
00436 void extract_corresp_pts( struct feature** features, int n, int mtype,
00437                           CvPoint2D64f** pts, CvPoint2D64f** mpts )
00438 {
00439   struct feature* match;
00440   CvPoint2D64f* _pts, * _mpts;
00441   int i;
00442 
00443   _pts = (CvPoint2D64f *) calloc( n, sizeof( CvPoint2D64f ) );
00444   _mpts = (CvPoint2D64f *)  calloc( n, sizeof( CvPoint2D64f ) );
00445 
00446   if( mtype == FEATURE_MDL_MATCH )
00447     for( i = 0; i < n; i++ )
00448       {
00449         match = get_match( features[i], mtype );
00450         if( ! match )
00451           fatal_error( "feature does not have match of type %d, %s line %d",
00452                        mtype, __FILE__, __LINE__ );
00453         _pts[i] = features[i]->img_pt;
00454         _mpts[i] = match->mdl_pt;
00455       }
00456 
00457   else
00458     for( i = 0; i < n; i++ )
00459       {
00460         match = get_match( features[i], mtype );
00461         if( ! match )
00462           fatal_error( "feature does not have match of type %d, %s line %d",
00463                        mtype, __FILE__, __LINE__ );
00464         _pts[i] = features[i]->img_pt;
00465         _mpts[i] = match->img_pt;
00466       }
00467 
00468   *pts = _pts;
00469   *mpts = _mpts;
00470 }
00471 
00472 
00473 
00474 /*
00475   For a given model and error function, finds a consensus from a set of
00476   feature correspondences.
00477 
00478   @param features set of pointers to features; every feature is assumed to
00479     have a match of type mtype
00480   @param n number of features in features
00481   @param mtype determines the match field of each feature against which to
00482     measure error; if this is FEATURE_MDL_MATCH, correspondences are assumed
00483     to be between the feature's img_pt field and the match's mdl_pt field;
00484     otherwise matches are assumed to be between img_pt and img_pt
00485   @param M model for which a consensus set is being found
00486   @param err_fn error function used to measure distance from M
00487   @param err_tol correspondences within this distance of M are added to the
00488     consensus set
00489   @param consensus output as an array of pointers to features in the
00490     consensus set
00491 
00492   @return Returns the number of points in the consensus set
00493 */
00494 int find_consensus( struct feature** features, int n, int mtype,
00495                     CvMat* M, ransac_err_fn err_fn, double err_tol,
00496                     struct feature*** consensus )
00497 {
00498   struct feature** _consensus;
00499   struct feature* match;
00500   CvPoint2D64f pt, mpt;
00501   double err;
00502   int i, in = 0;
00503 
00504   _consensus = (struct feature **) calloc( n, sizeof( struct feature* ) );
00505 
00506   if( mtype == FEATURE_MDL_MATCH )
00507     for( i = 0; i < n; i++ )
00508       {
00509         match = get_match( features[i], mtype );
00510         if( ! match )
00511           fatal_error( "feature does not have match of type %d, %s line %d",
00512                        mtype, __FILE__, __LINE__ );
00513         pt = features[i]->img_pt;
00514         mpt = match->mdl_pt;
00515         err = err_fn( pt, mpt, M );
00516         if( err <= err_tol )
00517           _consensus[in++] = features[i];
00518       }
00519 
00520   else
00521     for( i = 0; i < n; i++ )
00522       {
00523         match = get_match( features[i], mtype );
00524         if( ! match )
00525           fatal_error( "feature does not have match of type %d, %s line %d",
00526                        mtype, __FILE__, __LINE__ );
00527         pt = features[i]->img_pt;
00528         mpt = match->img_pt;
00529         err = err_fn( pt, mpt, M );
00530         if( err <= err_tol )
00531           _consensus[in++] = features[i];
00532       }
00533   *consensus = _consensus;
00534   return in;
00535 }
00536 
00537 
00538 
00539 /*
00540   Releases memory and reduces code size above
00541 
00542   @param pts1 an array of points
00543   @param pts2 an array of points
00544   @param features an array of pointers to features; can be NULL
00545 */
00546 static inline void release_mem( CvPoint2D64f* pts1, CvPoint2D64f* pts2,
00547                                 struct feature** features )
00548 {
00549   free( pts1 );
00550   free( pts2 );
00551   if( features )
00552     free( features );
00553 }



QVision framework. PARP research group, copyright 2007, 2008.