openCV中的findHomography函式分析以及RANSAC演算法的詳解（原始碼分析）
阿新 • • 發佈：2019-01-04
本文將openCV中的RANSAC程式碼全部挑選出來，進行分析和講解，以便大家更好的理解RANSAC演算法。程式碼我都試過，可以直接執行。
在計算機視覺和影象處理等很多領域，都需要用到RANSAC演算法。openCV中也有封裝好的RANSAC演算法，以便於人們使用。關於RANSAC演算法的一些應用，可以看我的另一篇部落格：
但是前幾天師弟在使用openCV自帶的RANSAC演算法時，發現實驗的執行時間並不會隨著輸入資料的增加而增加，感覺和理論上的不太相符。所以我就花了點時間，把openCV中關於RANSAC的原始碼全部複製出來研究了一下。以便我們更加清晰的瞭解RANSAC演算法的實際執行過程。
首先看兩個類
上面的兩個類中，CvModelEstimator2是一個基類，從名字就可以看出，這個類是用來估計模型的。可以看到裡面提供了許多虛擬函式，這些函式有許多，比如runRANSAC是利用RANSAC方法計算單應矩陣，而runLMeDS是利用LMeDS方法計算單應矩陣，我們這裡僅僅講解RANSAC方法，所以其他不需要的內容我就直接註釋掉了//模型估計的基類，提供了估計矩陣的各種虛擬函式 //置信度設為0。99 迴圈次數設定為了2000 class CvModelEstimator2 { public: CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions); virtual ~CvModelEstimator2(); virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0; //virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model, // CvMat* mask, double confidence=0.99, int maxIters=2000 ); virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model, CvMat* mask, double threshold, double confidence=0.99, int maxIters=2000 ); virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; } //virtual void setSeed( int64 seed ); protected: virtual void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error ) = 0; virtual int findInliers( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error, CvMat* mask, double threshold ); virtual bool getSubset( const CvMat* m1, const CvMat* m2, CvMat* ms1, CvMat* ms2, int maxAttempts=1000 ); virtual bool checkSubset( const CvMat* ms1, int count ); CvRNG rng; int modelPoints; CvSize modelSize; int maxBasicSolutions; bool checkPartialSubsets; }; //單應矩陣估計的子類 class CvHomographyEstimator : public CvModelEstimator2 { public: CvHomographyEstimator( int modelPoints ); virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ); virtual bool refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters ); protected: virtual void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error ); };
CvHomographyEstimator繼承自CvModelEstimator2，同樣的，從名字也就可以看出，這個類使用來估計單應矩陣的。
接下來是兩個類的建構函式和解構函式，這個沒啥好說的了，基本都是預設的。
<pre name="code" class="cpp">//建構函式 CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions) { modelPoints = _modelPoints; modelSize = _modelSize; maxBasicSolutions = _maxBasicSolutions; checkPartialSubsets = true; rng = cvRNG(-1); } //解構函式 CvModelEstimator2::~CvModelEstimator2() { } CvHomographyEstimator::CvHomographyEstimator(int _modelPoints) : CvModelEstimator2(_modelPoints, cvSize(3,3), 1) { assert( _modelPoints == 4 || _modelPoints == 5 ); checkPartialSubsets = false; }
接下來到重點了。runRANSAC方法就是通過RANSAC來計算矩陣
<pre name="code" class="cpp">bool CvModelEstimator2::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; //這是迭代次數，預設最大的迭代次數為2000次
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 ) //使用RANSAC演算法時，modelPoints為4
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 );//呼叫函式，300是迴圈次數，這個函式
if( !found ) //就是為了從序列中隨機選取4組，以便
{ //以便下一步計算單應矩陣
if( iter == 0 )
return false;
break;
}
}
printf("------");
nmodels = runKernel( ms1, ms2, models );//這個函式是通過給定的4組序列計算出矩陣
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 );
//輸出看看一共迴圈了多少次
printf("%5d %5d %5d %5d\n",iter,niters,goodCount,maxGoodCount);
if( goodCount > MAX(maxGoodCount, modelPoints-1) )
{
std::swap(tmask, mask);
cvCopy( &model_i, model );
maxGoodCount = goodCount;
//迴圈的次數會發生變化，原來原因在這裡
niters = cvRANSACUpdateNumIters( confidence,
(double)(count - goodCount)/count, modelPoints, niters );
}
}
}
//printf("RANSAC演算法實際迴圈了%d次\n",niters);
if( maxGoodCount > 0 )
{
if( mask != mask0 )
cvCopy( mask, mask0 );
result = true;
}
return result;
}在這個函式引數中，輸入的m1和m2是兩個對應的序列，這兩組序列的每一對資料一一匹配，其中既有正確的匹配，也有錯誤的匹配，正確的可以稱為內點，錯誤的稱為外點，RANSAC方法就是從這些包含錯誤匹配的資料中，分離出正確的匹配，並且求得單應矩陣。model就是我們需要求解的單應矩陣，mask我們可以稱為標記矩陣，他和m1，m2的長度一樣，當一個m1和m2中的點為內點時，mask相應的標記為1，反之為0，說白了，通過mask我們最終可以知道序列中哪些是內點，哪些是外點。reprojThreshold為閾值，當某一個匹配與估計的假設小於閾值時，則被認為是一個內點，這個閾值，openCV預設給的是3，後期使用的時候自己也可以修改。confidence為置信度，其實也就是人為的規定了一個數值，這個數值可以大致表示RANSAC結果的準確性，這個具體有啥用後面咱們再說。這個值初始時被設定為0.995. maxIters為初始迭代次數，RANSAC演算法核心就是不斷的迭代，這個值就是迭代的次數，預設設為了2000
這個函式的前期，主要是設定了一些變數然後賦初值，然後轉換相應的格式等等。最關鍵的部分，是那個for迴圈。我們把這個for迴圈單獨拿出來分析一下。程式碼如下。
for( iter = 0; iter < niters; iter++ )
{
int i, goodCount, nmodels;
if( count > modelPoints )
{
bool found = getSubset( m1, m2, ms1, ms2, 300 );//呼叫函式，300是迴圈次數，這個函式
if( !found ) //就是為了從序列中隨機選取4組，以便
{ //以便下一步計算單應矩陣
if( iter == 0 )
return false;
break;
}
}
nmodels = runKernel( ms1, ms2, models );//這個函式是通過給定的4組序列計算出矩陣
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 );
//輸出看看一共迴圈了多少次
printf("%5d %5d %5d %5d\n",iter,niters,goodCount,maxGoodCount);
if( goodCount > MAX(maxGoodCount, modelPoints-1) )
{
std::swap(tmask, mask);
cvCopy( &model_i, model );
maxGoodCount = goodCount;
//迴圈的次數會發生變化，原來原因在這裡
niters = cvRANSACUpdateNumIters( confidence,
(double)(count - goodCount)/count, modelPoints, niters );
}
}
}niters最初的值為2000，這就是初始時的RANSAC演算法的迴圈次數，getSubset（）函式是從一組對應的序列中隨機的選出4組（因為要想計算出一個3X3的矩陣，至少需要4組對應的座標），m1和m2是我們輸入序列，ms1和ms2是隨機選出的對應的4組匹配。
隨機的選出4組匹配後，就應該根據這4個匹配計算相應的矩陣，所以函式runKernel（）就是根據4組匹配計算矩陣，引數裡的models就是得到的矩陣。這個矩陣只是一個假設，為了驗證這個假設，需要用其他的點去計算，看看其他的點是內點還是外點。
findInliers（）函式就是用來計算內點的。利用前面得到的矩陣，把所有的序列帶入，計算得出哪些是內點，哪些是外點，函式的返回值為goodCount，就是此次計算的內點的個數。函式中還有一個值為maxGoodCount，每次迴圈的內點個數的最大值儲存在這個值中，一個估計的矩陣如果有越多的內點，那麼這個矩陣就越有可能是正確的。所以計算內點個數以後，緊接著判斷一下goodCount和maxGoodCount的大小關係，如果goodCount>maxGoodCount，則把goodCount賦值給maxGoodCount。賦值之後的一行程式碼非常關鍵，我們單獨拿出來說一下，程式碼如下：
niters = cvRANSACUpdateNumIters( confidence,
(double)(count - goodCount)/count, modelPoints, niters );所以，們現在應該清楚為什麼輸入資料增加，而演算法執行時間不會增加了。openCV的RANSAC演算法首先把迭代的次數設定為2000，然後再迭代的過程中，動態的改變總迭代次數，無論輸入資料有多少，總的迭代次數不會增加，並且通過4個匹配計算出估計的矩陣這個時間是不變的，通過估計矩陣來計算內點，這方面的增加的時間開銷基本上可以忽略。所以導致的最終結果就是，無論輸入點有多少，運算時間基本不會有太大變化。
以上就是RANSAC演算法的核心程式碼，其中用到的一些函式，下面一一給出。
1. 轉換為齊次左邊，看上去很長，但是完成的功能就是把一般的座標轉換成齊次座標以方便以後的計算
CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
{
Ptr<CvMat> temp, denom;
int i, s_count, s_dims, d_count, d_dims;
CvMat _src, _dst, _ones;
CvMat* ones = 0;
if( !CV_IS_MAT(src) )
CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg,
"The input parameter is not a valid matrix" );
if( !CV_IS_MAT(dst) )
CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
"The output parameter is not a valid matrix" );
if( src == dst || src->data.ptr == dst->data.ptr )
{
if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
CV_Error( CV_StsBadArg, "Invalid inplace operation" );
return;
}
if( src->rows > src->cols )
{
if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->cols;
s_count = src->rows;
}
else
{
if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->rows;
s_count = src->cols;
}
if( src->rows == 1 || src->cols == 1 )
src = cvReshape( src, &_src, 1, s_count );
if( dst->rows > dst->cols )
{
if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_Error( CV_StsBadSize,
"Either the number of channels or columns or rows in the input matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->cols;
d_count = dst->rows;
}
else
{
if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_Error( CV_StsBadSize,
"Either the number of channels or columns or rows in the output matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->rows;
d_count = dst->cols;
}
if( dst->rows == 1 || dst->cols == 1 )
dst = cvReshape( dst, &_dst, 1, d_count );
if( s_count != d_count )
CV_Error( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
CV_Error( CV_StsUnsupportedFormat,
"Both matrices must be floating-point (single or double precision)" );
if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
CV_Error( CV_StsOutOfRange,
"Both input and output point dimensionality must be 2, 3 or 4" );
if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
CV_Error( CV_StsUnmatchedSizes,
"The dimensionalities of input and output point sets differ too much" );
if( s_dims == d_dims - 1 )
{
if( d_count == dst->rows )
{
ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
}
else
{
ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
}
}
if( s_dims <= d_dims )
{
if( src->rows == dst->rows && src->cols == dst->cols )
{
if( CV_ARE_TYPES_EQ( src, dst ) )
cvCopy( src, dst );
else
cvConvert( src, dst );
}
else
{
if( !CV_ARE_TYPES_EQ( src, dst ))
{
temp = cvCreateMat( src->rows, src->cols, dst->type );
cvConvert( src, temp );
src = temp;
}
cvTranspose( src, dst );
}
if( ones )
cvSet( ones, cvRealScalar(1.) );
}
else
{
int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
if( !CV_ARE_TYPES_EQ( src, dst ))
{
temp = cvCreateMat( src->rows, src->cols, dst->type );
cvConvert( src, temp );
src = temp;
}
elem_size = CV_ELEM_SIZE(src->type);
if( s_count == src->cols )
s_plane_stride = src->step / elem_size, s_stride = 1;
else
s_stride = src->step / elem_size, s_plane_stride = 1;
if( d_count == dst->cols )
d_plane_stride = dst->step / elem_size, d_stride = 1;
else
d_stride = dst->step / elem_size, d_plane_stride = 1;
denom = cvCreateMat( 1, d_count, dst->type );
if( CV_MAT_DEPTH(dst->type) == CV_32F )
{
const float* xs = src->data.fl;
const float* ys = xs + s_plane_stride;
const float* zs = 0;
const float* ws = xs + (s_dims - 1)*s_plane_stride;
float* iw = denom->data.fl;
float* xd = dst->data.fl;
float* yd = xd + d_plane_stride;
float* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
float t = *ws;
iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
else
{
const double* xs = src->data.db;
const double* ys = xs + s_plane_stride;
const double* zs = 0;
const double* ws = xs + (s_dims - 1)*s_plane_stride;
double* iw = denom->data.db;
double* xd = dst->data.db;
double* yd = xd + d_plane_stride;
double* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
double t = *ws;
iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
}
}2. 對迭代值進行更新的函式。這個函式就是對總的迭代次數進行更新，從中可以看到，迭代值以指數形式減少。最初的為2000的迭代次數，有的時候可能經過不斷的更新，最終結果成了幾十了。
CV_IMPL int
cvRANSACUpdateNumIters( double p, double ep,
int model_points, int max_iters )
{
if( model_points <= 0 )
CV_Error( CV_StsOutOfRange, "the number of model points should be positive" );
p = MAX(p, 0.);
p = MIN(p, 1.);
ep = MAX(ep, 0.);
ep = MIN(ep, 1.);
// avoid inf's & nan's
double num = MAX(1. - p, DBL_MIN);
double denom = 1. - pow(1. - ep,model_points);
if( denom < DBL_MIN )
return 0;
num = log(num);
denom = log(denom);
return denom >= 0 || -num >= max_iters*(-denom) ?
max_iters : cvRound(num/denom);
}
3. 通過4個匹配，計算單應矩陣，就是給你了4個匹配，你把和這四個匹配相符的矩陣計算出來
//通過四個匹配，計算符合要求的單應矩陣
int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
{
int i, count = m1->rows*m1->cols;
const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
double LtL[9][9], W[9][1], V[9][9];
CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
CvMat matW = cvMat( 9, 1, CV_64F, W );
CvMat matV = cvMat( 9, 9, CV_64F, V );
CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
for( i = 0; i < count; i++ )
{
cm.x += m[i].x; cm.y += m[i].y;
cM.x += M[i].x; cM.y += M[i].y;
}
cm.x /= count; cm.y /= count;
cM.x /= count; cM.y /= count;
for( i = 0; i < count; i++ )
{
sm.x += fabs(m[i].x - cm.x);
sm.y += fabs(m[i].y - cm.y);
sM.x += fabs(M[i].x - cM.x);
sM.y += fabs(M[i].y - cM.y);
}
if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )
return 0;
sm.x = count/sm.x; sm.y = count/sm.y;
sM.x = count/sM.x; sM.y = count/sM.y;
double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
cvZero( &_LtL );
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
int j, k;
for( j = 0; j < 9; j++ )
for( k = j; k < 9; k++ )
LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
}
cvCompleteSymm( &_LtL );
//cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
cvEigenVV( &_LtL, &matV, &matW );
cvMatMul( &_invHnorm, &_H0, &_Htemp );
cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
return 1;
}
4. 給定輸入序列後，從中隨機的選出4對匹配
bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
CvMat* ms1, CvMat* ms2, int maxAttempts ) //maxAttempts被設為300
{
cv::AutoBuffer<int> _idx(modelPoints);
int* idx = _idx;
int i = 0, j, k, idx_i, iters = 0;
int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
int count = m1->cols*m1->rows;
assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
elemSize /= sizeof(int);
for(; iters < maxAttempts; iters++)
{
for( i = 0; i < modelPoints && iters < maxAttempts; )
{
idx[i] = idx_i = cvRandInt(&rng) % count; //產生count以內的隨機數，count是序列長度
for( j = 0; j < i; j++ ) //保證產生的隨機數沒有重複的
if( idx_i == idx[j] )
break;
if( j < i )
continue;
for( k = 0; k < elemSize; k++ )
{
ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k]; //把隨機產生的數給了ms1和ms2
ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
}
if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 ))) //呼叫函式checkSubset
{
iters++;
continue;
}
i++;
}
if( !checkPartialSubsets && i == modelPoints &&
(!checkSubset( ms1, i ) || !checkSubset( ms2, i )))
continue;
break;
}
return i == modelPoints && iters < maxAttempts;
}
5. 對生成的4組匹配進行檢驗，觀察其是否合乎要求。
bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
{
int j, k, i, i0, i1;
CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
if( checkPartialSubsets )
i0 = i1 = count - 1;
else
i0 = 0, i1 = count - 1;
for( i = i0; i <= i1; i++ )
{
// check that the i-th selected point does not belong
// to a line connecting some previously selected points
for( j = 0; j < i; j++ )
{
double dx1 = ptr[j].x - ptr[i].x;
double dy1 = ptr[j].y - ptr[i].y;
for( k = 0; k < j; k++ )
{
double dx2 = ptr[k].x - ptr[i].x;
double dy2 = ptr[k].y - ptr[i].y;
if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
break;
}
if( k < j )
break;
}
if( j < i )
break;
}
return i >= i1;
}int CvModelEstimator2::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 ); //_err裡面是計算後的矩陣的大小，用於與閾值比較
threshold *= threshold;
for( i = 0; i < count; i++ )
goodCount += mask[i] = err[i] <= threshold;//goodCount為計算出的內點的個數
return goodCount;
}
7.上面的函式呼叫的一些函式，這些函式不難，所以下面相應的列舉一下
bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
{
CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
int i, j, k, count = m1->rows*m1->cols;
const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
cvCopy( &modelPart, solver.param );
for(;;)
{
const CvMat* _param = 0;
CvMat *_JtJ = 0, *_JtErr = 0;
double* _errNorm = 0;
if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
break;
for( i = 0; i < count; i++ )
{
const double* h = _param->data.db;
double Mx = M[i].x, My = M[i].y;
double ww = h[6]*Mx + h[7]*My + 1.;
ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0;
double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
double err[] = { _xi - m[i].x, _yi - m[i].y };
if( _JtJ || _JtErr )
{
double J[][8] =
{
{ Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
{ 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
};
for( j = 0; j < 8; j++ )
{
for( k = j; k < 8; k++ )
_JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
_JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
}
}
if( _errNorm )
*_errNorm += err[0]*err[0] + err[1]*err[1];
}
}
cvCopy( solver.param, &modelPart );
return true;
}
void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
const CvMat* model, CvMat* _err )
{
int i, count = m1->rows*m1->cols;
const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
const double* H = model->data.db;
float* err = _err->data.fl;
for( i = 0; i < count; i++ )
{
double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
err[i] = (float)(dx*dx + dy*dy);
}
}8，最後一部分是比較關鍵的。就是FindHomography函式本身。這個函式又去呼叫了cvFindHomography函式，估計就是openCV不同版本的函式吧，其實現的功能和思想都是一樣的。這個函式內部基本上也就是做一些判斷防止溢位，排查錯誤，檢驗變數以及變換格式等輔助性的內容，真正的方法性質的程式碼還是在上面的提到的CvHomographyEstimator類中。
cv::Mat findHomography( InputArray _points1, InputArray _points2,
int method, double ransacReprojThreshold, OutputArray _mask)
{
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
int npoints = points1.checkVector(2);//返回矩陣的序列個數
CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
points1.type() == points2.type()); //檢驗初始條件是否正確
Mat H(3, 3, CV_64F);
CvMat _pt1 = points1, _pt2 = points2;
CvMat matH = H, c_mask, *p_mask = 0;
if( _mask.needed() )
{
_mask.create(npoints, 1, CV_8U, -1, true);
p_mask = &(c_mask = _mask.getMat());
}
bool ok = cvFindHomography( &_pt1, &_pt2, &matH, method, ransacReprojThreshold, p_mask ) > 0; //函式呼叫
if( !ok )
H = Scalar(0);
return H;
}
CV_IMPL int
cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
CvMat* __H, int method, double ransacReprojThreshold,
CvMat* mask )
{
const double confidence = 0.995;
const int maxIters = 2000; //修改這裡來修改迭代次數
const double defaultRANSACReprojThreshold = 3;
bool result = false;
Ptr<CvMat> m, M, tempMask;
double H[9];
CvMat matH = cvMat( 3, 3, CV_64FC1, H ); //這就是單應矩陣，矩陣初始化
int count;
CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
count = MAX(imagePoints->cols, imagePoints->rows); //序列個數
CV_Assert( count >= 4 );
if( ransacReprojThreshold <= 0 )
ransacReprojThreshold = defaultRANSACReprojThreshold;
m = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( imagePoints, m ); //轉換齊次座標
M = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( objectPoints, M );
if( mask )
{
CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
(mask->rows == 1 || mask->cols == 1) &&
mask->rows*mask->cols == count );
}
if( mask || count > 4 )
tempMask = cvCreateMat( 1, count, CV_8U );
if( !tempMask.empty() )
cvSet( tempMask, cvScalarAll(1.) );
CvHomographyEstimator estimator( MIN(count, 4) ); //引數是一個小於等於4的值，只有大於4，才能用RANSAC計算
if( count == 4 )
method = 0;
if( method == CV_LMEDS )
//result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );
printf("");
else if( method == CV_RANSAC )
result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
else
result = estimator.runKernel( M, m, &matH ) > 0;
if( result && count > 4 )
{
icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count ); //壓縮，使序列緊湊
count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
M->cols = m->cols = count; //篩選過後，這個count是內點的個數
if( method == CV_RANSAC )
estimator.runKernel( M, m, &matH ); //重新計算最終的單應矩陣，matH
estimator.refine( M, m, &matH, 10 );
}
if( result )
cvConvert( &matH, __H );
if( mask && tempMask )
{
if( CV_ARE_SIZES_EQ(mask, tempMask) ) //複製這個矩陣
cvCopy( tempMask, mask );
else
cvTranspose( tempMask, mask ); //行列調換的 複製這個矩陣
}
return (int)result;
}