/*This file contains definitions for functions to compute transforms fromimage feature correspondencesCopyright (C) 2006  Rob Hess <hess@eecs.oregonstate.edu>@version 1.1.1-20070330*/#include "xform.h"#include "imgfeatures.h"#include "utils.h"#include <cxcore.h>#include <gsl/gsl_sf.h>#include <gsl/gsl_rng.h>#include <gsl/gsl_randist.h>#include <time.h>
/************************* Local Function Prototypes *************************/static __inline struct feature* get_match( struct feature*, int );int get_matched_features( struct feature*, int, int, struct feature*** );int calc_min_inliers( int, int, double, double );struct feature** draw_ransac_sample( struct feature**, int, int, gsl_rng* );void extract_corresp_pts( struct feature**, int, int, CvPoint2D64f**,						 CvPoint2D64f** );int find_consensus( struct feature**, int, int, CvMat*, ransac_err_fn,				   double, struct feature*** );static __inline void release_mem( CvPoint2D64f*, CvPoint2D64f*,struct feature** );/********************** Functions prototyped in xform.h **********************//*Calculates a best-fit image transform from image feature correspondences
using RANSAC.

For more information refer to:

Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
model fitting with applications to image analysis and automated cartography.
<EM>Communications of the ACM, 24</EM>, 6 (1981), pp. 381--395.

@param features an array of features; only features with a non-NULL match
	of type mtype are used in homography computation
@param n number of features in feat
@param mtype determines which of each feature's match fields to use
	for model computation; should be one of FEATURE_FWD_MATCH,
	FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
	correspondences are assumed to be between a feature's img_pt field
	and its match's mdl_pt field, otherwise correspondences are assumed to
	be between the the feature's img_pt field and its match's img_pt field
@param xform_fn pointer to the function used to compute the desired
	transformation from feature correspondences
@param m minimum number of correspondences necessary to instantiate the
	model computed by xform_fn
@param p_badxform desired probability that the final transformation
	returned by RANSAC is corrupted by outliers (i.e. the probability that
	no samples of all inliers were drawn)
@param err_fn pointer to the function used to compute a measure of error
	between putative correspondences and a computed model
@param err_tol correspondences within this distance of a computed model are
	considered as inliers
@param inliers if not NULL, output as an array of pointers to the final
	set of inliers
@param n_in if not NULL and \a inliers is not NULL, output as the final
	number of inliers

@return Returns a transformation matrix computed using RANSAC or NULL
	on error or if an acceptable transform could not be computed.*/CvMat* ransac_xform( struct feature* features, int n, int mtype,					ransac_xform_fn xform_fn, int m, double p_badxform,					ransac_err_fn err_fn, double err_tol,struct feature*** inliers, int* n_in ){	struct feature** matched, ** sample, ** consensus, ** consensus_max = NULL;	struct ransac_data* rdata;	CvPoint2D64f* pts, * mpts;	CvMat* M = NULL;	gsl_rng* rng;	double p, in_frac = RANSAC_INLIER_FRAC_EST;	int i, nm, in, in_min, in_max = 0, k = 0;	nm = get_matched_features( features, n, mtype, &matched );	if( nm < m )	{		fprintf( stderr, "Warning: not enough matches to compute xform, %s" \			" line %d\n", __FILE__, __LINE__ );		goto end;	}	/* 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 )	{		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 );		if( inliers )		{			*inliers = consensus;			consensus = NULL;		}		if( n_in )			*n_in = in;		release_mem( pts, mpts, consensus );	}	else if( consensus_max )	{		if( inliers )			*inliers = NULL;		if( n_in )			*n_in = 0;		free( consensus_max );	}	gsl_rng_free( rng );end:	for( i = 0; i < nm; i++ )	{		rdata = feat_ransac_data( matched[i] );		matched[i]->feature_data = rdata->orig_feat_data;		free( rdata );	}	free( matched );	return M;}/*Calculates a least-squares planar homography from point correspondeces.@param pts array of points@param mpts array of corresponding points; each pts[i], i=0..n-1, corresponds	to mpts[i]@param n number of points in both pts and mpts; must be at least 4@return Returns the 3 x 3 least-squares planar homography matrix that	transforms points in pts to their corresponding points in mpts or NULL if	fewer than 4 correspondences were provided*/CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n ){	CvMat* H, * A, * B, X;
	double x[9];	int i;

	if( n < 4 )	{		fprintf( stderr, "Warning: too few points in lsq_homog(), %s line %d\n",			__FILE__, __LINE__ );		return NULL;	}	/* set up matrices so we can unstack homography into X; AX = B */	A = cvCreateMat( 2*n, 8, CV_64FC1 );	B = cvCreateMat( 2*n, 1, CV_64FC1 );	X = cvMat( 8, 1, CV_64FC1, x );	H = cvCreateMat(3, 3, CV_64FC1);	cvZero( A );	for( i = 0; i < n; i++ )	{		cvmSet( A, i, 0, pts[i].x );		cvmSet( A, i+n, 3, pts[i].x );		cvmSet( A, i, 1, pts[i].y );		cvmSet( A, i+n, 4, pts[i].y );		cvmSet( A, i, 2, 1.0 );		cvmSet( A, i+n, 5, 1.0 );		cvmSet( A, i, 6, -pts[i].x * mpts[i].x );		cvmSet( A, i, 7, -pts[i].y * mpts[i].x );		cvmSet( A, i+n, 6, -pts[i].x * mpts[i].y );		cvmSet( A, i+n, 7, -pts[i].y * mpts[i].y );		cvmSet( B, i, 0, mpts[i].x );		cvmSet( B, i+n, 0, mpts[i].y );	}	cvSolve( A, B, &X, CV_SVD );	x[8] = 1.0;	X = cvMat( 3, 3, CV_64FC1, x );	cvConvert( &X, H );
	cvReleaseMat( &A );
	cvReleaseMat( &B );
	return H;}/*Calculates the transfer error between a point and its correspondence fora given homography, i.e. for a point x, it's correspondence x', andhomography H, computes d(x', Hx)^2.@param pt a point@param mpt pt's correspondence@param H a homography matrix@return Returns the transfer error between pt and mpt given H*/double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H ){	CvPoint2D64f xpt = persp_xform_pt( pt, H );	return sqrt( dist_sq_2D( xpt, mpt ) );}/*Performs a perspective transformation on a single point.  That is, for apoint (x, y) and a 3 x 3 matrix T this function returns the point(u, v), where[x' y' w']^T = T * [x y 1]^T,and(u, v) = (x'/w', y'/w').Note that affine transforms are a subset of perspective transforms.@param pt a 2D point@param T a perspective transformation matrix@return Returns the point (u, v) as above.*/CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T ){	CvMat XY, UV;	double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 };	CvPoint2D64f rslt;	cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP );	cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP );	cvMatMul( T, &XY, &UV );	rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] );	return rslt;