OpenCV使用RANSAC的仿射变换估计 estimateAffine2D

OpenCV自带有findHomography这个用RANSAC随机采样求透视变换的方法,很好用，但是没有一个类似的求仿射的。

自带的getAffineTransform只是简单的使用三对点。

而estimateAffine3D使用的是三维坐标,转换起来有点不方便，而且我在使用中发现,即使把z坐标设置为0，有时候求出来的模型竟然100%都是内点，OpenCV的源码,自己提取,封装了一下.用的是SVN的Trunk,主版本2.32

有几个改动:

1.OpenCV的estimator都是继承自CvModelEstimator2,而这个父类并不是导出类,所以只能把代码都再写一遍

2.据我观察,估计时内部用的是64位浮点数,增加计算精度,我把getAffineTransform也再写了一遍,对应64位精度

//Affine2D.hpp

class Affine2DEstimator
{
public:
 Affine2DEstimator();
 int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
 bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
  CvMat* mask, double threshold,
  double confidence=0.99, int maxIters=2000 );
 bool getSubset( const CvMat* m1, const CvMat* m2,
  CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );
 bool checkSubset( const CvMat* ms1, int count );
 int findInliers( const CvMat* m1, const CvMat* m2,
  const CvMat* model, CvMat* error,
  CvMat* mask, double threshold );
 void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error );
protected:
 CvRNG rng;
 int modelPoints;
 CvSize modelSize;
 int maxBasicSolutions;
 bool checkPartialSubsets;
};

int estimateAffine2D(cv::InputArray _from, cv::InputArray _to,
 cv::OutputArray _out, cv::OutputArray _inliers,
 double param1=3, double param2=0.99);

--------------------------------------------------------------------------

int Affine2DEstimator::findInliers( const CvMat* m1, const CvMat* m2,
 const CvMat* model, CvMat* _err,
 CvMat* _mask, double threshold )
{
 int i, count = _err->rows*_err->cols, goodCount = 0;
 const float* err = _err->data.fl;
 uchar* mask = _mask->data.ptr;

computeReprojError( m1, m2, model, _err );
 threshold *= threshold;
 for( i = 0; i < count; i++ )
  goodCount += mask[i] = err[i] <= threshold;
 return goodCount;
}

void Affine2DEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error )
{
 int count = m1->rows * m1->cols;
 const CvPoint2D64f* from = reinterpret_cast<const CvPoint2D64f*>(m1->data.ptr);
 const CvPoint2D64f* to  = reinterpret_cast<const CvPoint2D64f*>(m2->data.ptr);   
 const double* F = model->data.db;
 float* err = error->data.fl;

for(int i = 0; i < count; i++ )
 {
  const CvPoint2D64f& f = from[i];
  const CvPoint2D64f& t = to[i];

double a = F[0]*f.x + F[1]*f.y + F[2] - t.x;
  double b = F[3]*f.x + F[4]*f.y + F[5] - t.y;
 
  err[i] = (float)sqrt(a*a + b*b);     
 }
}

bool Affine2DEstimator::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
 CvMat* mask0, double reprojThreshold,
 double confidence, int maxIters )
{
 bool result = false;
 cv::Ptr<CvMat> mask = cvCloneMat(mask0);
 cv::Ptr<CvMat> models, err, tmask;
 cv::Ptr<CvMat> ms1, ms2;

int iter, niters = maxIters;
 int count = m1->rows*m1->cols, maxGoodCount = 0;
 CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );

if( count < modelPoints )
  return false;

models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
 err = cvCreateMat( 1, count, CV_32FC1 );
 tmask = cvCreateMat( 1, count, CV_8UC1 );

if( count > modelPoints )
 {
  ms1 = cvCreateMat( 1, modelPoints, m1->type );
  ms2 = cvCreateMat( 1, modelPoints, m2->type );
 }
 else
 {
  niters = 1;
  ms1 = cvCloneMat(m1);
  ms2 = cvCloneMat(m2);
 }

for( iter = 0; iter < niters; iter++ )
 {
  int i, goodCount, nmodels;
  if( count > modelPoints )
  {
   bool found = getSubset( m1, m2, ms1, ms2, 300 );
   if( !found )
   {
    if( iter == 0 )
     return false;
    break;
   }
  }

nmodels = runKernel( ms1, ms2, models );
  if( nmodels <= 0 )
   continue;
  for( i = 0; i < nmodels; i++ )
  {
   CvMat model_i;
   cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
   goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );