00001
00002
00003
00004
00005
00006
00007
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
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
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
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
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
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
00172
00173
00174
00175
00176
00177
00178
00179
00180
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
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
00230
00231
00232
00233
00234
00235
00236
00237
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
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
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
00282
00283
00284
00285
00286
00287
00288
00289
00290
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
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
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
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
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
00383
00384
00385
00386
00387
00388
00389
00390
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
00426
00427
00428
00429
00430
00431
00432
00433
00434
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
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
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
00541
00542
00543
00544
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 }