Opencv RANSAC 算法解析

本文将openCV中的RANSAC代码全部挑选出来，进行分析和讲解，以便大家更好的理解RANSAC算法。代码我都试过，可以直接运行。

在计算机视觉和图像处理等很多领域，都需要用到RANSAC算法。openCV中也有封装好的RANSAC算法，以便于人们使用。关于RANSAC算法的一些应用，可以看我的另一篇博客：

利用SIFT和RANSAC算法（openCV框架）实现物体的检测与定位，并求出变换矩阵（findFundamentalMat和findHomography的比较）

但是前几天师弟在使用openCV自带的RANSAC算法时，发现实验的运行时间并不会随着输入数据的增加而增加，感觉和理论上的不太相符。所以我就花了点时间，把openCV中关于RANSAC的源代码全部复制出来研究了一下。以便我们更加清晰的了解RANSAC算法的实际运行过程。

首先看两个类

[cpp]  view plain  copy

 

 

1. //模型估计的基类，提供了估计矩阵的各种虚函数  
2. //置信度设为0。99 循环次数设置为了2000  
3. class CvModelEstimator2  
4. {  
5. public:  
6.     CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions);  
7.     virtual ~CvModelEstimator2();  
8.   
9.     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0;  
10.     //virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,  
11.                           // CvMat* mask, double confidence=0.99, int maxIters=2000 );  
12.     virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,  
13.                             CvMat* mask, double threshold,  
14.                             double confidence=0.99, int maxIters=2000 );  
15.     virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; }  
16.     //virtual void setSeed( int64 seed );  
17.   
18. protected:  
19.     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,  
20.                                      const CvMat* model, CvMat* error ) = 0;  
21.     virtual int findInliers( const CvMat* m1, const CvMat* m2,  
22.                              const CvMat* model, CvMat* error,  
23.                              CvMat* mask, double threshold );  
24.     virtual bool getSubset( const CvMat* m1, const CvMat* m2,  
25.                             CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );  
26.     virtual bool checkSubset( const CvMat* ms1, int count );  
27.   
28.     CvRNG rng;  
29.     int modelPoints;  
30.     CvSize modelSize;  
31.     int maxBasicSolutions;  
32.     bool checkPartialSubsets;  
33. };  
34. //单应矩阵估计的子类  
35. class CvHomographyEstimator : public CvModelEstimator2  
36. {  
37. public:  
38.     CvHomographyEstimator( int modelPoints );  
39.   
40.     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );  
41.     virtual bool refine( const CvMat* m1, const CvMat* m2,  
42.                          CvMat* model, int maxIters );  
43.       
44.   
45. protected:  
46.     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,  
47.                                      const CvMat* model, CvMat* error );  
48. };  

上面的两个类中，CvModelEstimator2是一个基类，从名字就可以看出，这个类是用来估计模型的。可以看到里面提供了许多虚函数，这些函数有许多，比如runRANSAC是利用RANSAC方法计算单应矩阵，而runLMeDS是利用LMeDS方法计算单应矩阵，我们这里仅仅讲解RANSAC方法，所以其他不需要的内容我就直接注释掉了

CvHomographyEstimator继承自CvModelEstimator2，同样的，从名字也就可以看出，这个类使用来估计单应矩阵的。

接下来是两个类的构造函数和析构函数，这个没啥好说的了，基本都是默认的。

[html]  view plain  copy

 

 

1. <pre name="code" class="cpp">//构造函数  
2. CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)  
3. {  
4.     modelPoints = _modelPoints;  
5.     modelSize = _modelSize;  
6.     maxBasicSolutions = _maxBasicSolutions;  
7.     checkPartialSubsets = true;  
8.     rng = cvRNG(-1);  
9. }  
10. //析构函数  
11. CvModelEstimator2::~CvModelEstimator2()  
12. {  
13. }  
14.   
15. CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)  
16.     : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)  
17. {  
18.     assert( _modelPoints == 4 || _modelPoints == 5 );  
19.     checkPartialSubsets = false;  
20.   
21. }  

接下来到重点了。runRANSAC方法就是通过RANSAC来计算矩阵

[html]  view plain  copy

 

 

1. <pre name="code" class="cpp">bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,  
2.                                     CvMat* mask0, double reprojThreshold,  
3.                                     double confidence, int maxIters )  
4. {  
5.     bool result = false;  
6.     cv::Ptr<CvMat> mask = cvCloneMat(mask0);   //标记矩阵，标记内点和外点  
7.     cv::Ptr<CvMat> models, err, tmask;  
8.     cv::Ptr<CvMat> ms1, ms2;  
9.   
10.     int iter, niters = maxIters;   //这是迭代次数，默认最大的迭代次数为2000次  
11.     int count = m1->rows*m1->cols, maxGoodCount = 0;  
12.     CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );  
13.   
14.     if( count < modelPoints )  //使用RANSAC算法时，modelPoints为4  
15.         return false;  
16.   
17.     models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );  
18.     err = cvCreateMat( 1, count, CV_32FC1 );  
19.     tmask = cvCreateMat( 1, count, CV_8UC1 );  
20.       
21.     if( count > modelPoints )  
22.     {  
23.         ms1 = cvCreateMat( 1, modelPoints, m1->type );  
24.         ms2 = cvCreateMat( 1, modelPoints, m2->type );  
25.     }  
26.     else  
27.     {  
28.         niters = 1;  
29.         ms1 = cvCloneMat(m1);  
30.         ms2 = cvCloneMat(m2);  
31.     }  
32.   
33.     for( iter = 0; iter < niters; iter++ )  
34.     {  
35.         int i, goodCount, nmodels;  
36.           
37.         if( count > modelPoints )  
38.         {  
39.             bool found = getSubset( m1, m2, ms1, ms2, 300 );//调用函数，300是循环次数，这个函数  
40.             if( !found )                                    //就是为了从序列中随机选取4组，以便  
41.             {                                               //以便下一步计算单应矩阵  
42.                 if( iter == 0 )  
43.                     return false;  
44.                 break;  
45.             }  
46.         }  
47.         printf("------");  
48.            
49.         nmodels = runKernel( ms1, ms2, models );//这个函数是通过给定的4组序列计算出矩阵  
50.           
51.         if( nmodels <= 0 )  
52.             continue;  
53.         for( i = 0; i < nmodels; i++ )  
54.         {  
55.             CvMat model_i;  
56.             cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );  
57.             goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );  
58.             //输出看看一共循环了多少次  
59.             printf("%5d %5d %5d %5d\n",iter,niters,goodCount,maxGoodCount);  
60.             if( goodCount > MAX(maxGoodCount, modelPoints-1) )  
61.             {  
62.                 std::swap(tmask, mask);  
63.                 cvCopy( &model_i, model );  
64.                 maxGoodCount = goodCount;  
65.                 //循环的次数会发生变化，原来原因在这里  
66.                 niters = cvRANSACUpdateNumIters( confidence,  
67.                     (double)(count - goodCount)/count, modelPoints, niters );  
68.             }  
69.         }  
70.           
71.     }  
72.   
73.     //printf("RANSAC算法实际循环了%d次\n",niters);  
74.   
75.     if( maxGoodCount > 0 )  
76.     {  
77.         if( mask != mask0 )  
78.             cvCopy( mask, mask0 );  
79.         result = true;  
80.     }  
81.   
82.     return result;  
83. }  

在这个函数参数中，输入的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循环单独拿出来分析一下。代码如下。

[cpp]  view plain  copy

 

 

1.  for( iter = 0; iter < niters; iter++ )  
2.  {  
3.      int i, goodCount, nmodels;  
4.   
5.      if( count > modelPoints )  
6.      {  
7.          bool found = getSubset( m1, m2, ms1, ms2, 300 );//调用函数，300是循环次数，这个函数  
8.          if( !found )                                    //就是为了从序列中随机选取4组，以便  
9.          {                                               //以便下一步计算单应矩阵  
10.              if( iter == 0 )  
11.                  return false;  
12.              break;  
13.          }  
14.      }   
15.      nmodels = runKernel( ms1, ms2, models );//这个函数是通过给定的4组序列计算出矩阵  
16.   
17.      if( nmodels <= 0 )  
18.          continue;  
19.      for( i = 0; i < nmodels; i++ )  
20.      {  
21.          CvMat model_i;  
22.          cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );  
23.          goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );  
24. //输出看看一共循环了多少次  
25. printf("%5d %5d %5d %5d\n",iter,niters,goodCount,maxGoodCount);  
26.          if( goodCount > MAX(maxGoodCount, modelPoints-1) )  
27.          {  
28.              std::swap(tmask, mask);  
29.              cvCopy( &model_i, model );  
30.              maxGoodCount = goodCount;  
31.     //循环的次数会发生变化，原来原因在这里  
32.              niters = cvRANSACUpdateNumIters( confidence,  
33.                  (double)(count - goodCount)/count, modelPoints, niters );  
34.          }  
35.      }  
36.   
37.  }  

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。赋值之后的一行代码非常关键，我们单独拿出来说一下，代码如下：

[cpp]  view plain  copy

 

 

1. niters = cvRANSACUpdateNumIters( confidence,  
2.                    (double)(count - goodCount)/count, modelPoints, niters );  

niters本来是迭代的次数，也就是循环的次数。但是通过这行代码我们发现，每次循环后，都会对niters这个值进行更新，也就是每次循环后都会改变循环的总次数。cvRANSACUpdateNumIters（）函数利用confidence（置信度）count（总匹配个数）goodCount（当前内点个数）niters（当前的总迭代次数）这几个参数，来动态的改变总迭代次数的大小。该函数的中心思想就是当内点占的比例较多时，那么很有可能已经找到了正确的估计，所以就适当的减少迭代次数来节省时间。这个迭代次数的减少是以指数形式减少的，所以节省的时间开销也是非常的可观。因此最初设计的2000的迭代次数，可能最终的迭代次数只有几十。同样的，如果你自己一开始把迭代次数设置成10000或者更大，进过几次迭代后，niters又会变得非常小了。所以初始时的niters设置的再大，其实对最终的运行时间也没什么影响。我用我自己的程序简答试了一下，无论初值设为2000,10000,20000，最终的迭代次数都变成了58！！！

所以，们现在应该清楚为什么输入数据增加，而算法运行时间不会增加了。openCV的RANSAC算法首先把迭代的次数设置为2000，然后再迭代的过程中，动态的改变总迭代次数，无论输入数据有多少，总的迭代次数不会增加，并且通过4个匹配计算出估计的矩阵这个时间是不变的，通过估计矩阵来计算内点，这方面的增加的时间开销基本上可以忽略。所以导致的最终结果就是，无论输入点有多少，运算时间基本不会有太大变化。

以上就是RANSAC算法的核心代码，其中用到的一些函数，下面一一给出。

1. 转换为齐次左边，看上去很长，但是完成的功能就是把一般的坐标转换成齐次坐标以方便以后的计算

[cpp]  view plain  copy

 

 

1. CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )  
2. {  
3.     Ptr<CvMat> temp, denom;  
4.   
5.     int i, s_count, s_dims, d_count, d_dims;  
6.     CvMat _src, _dst, _ones;  
7.     CvMat* ones = 0;  
8.   
9.     if( !CV_IS_MAT(src) )  
10.         CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg,  
11.         "The input parameter is not a valid matrix" );  
12.   
13.     if( !CV_IS_MAT(dst) )  
14.         CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,  
15.         "The output parameter is not a valid matrix" );  
16.   
17.     if( src == dst || src->data.ptr == dst->data.ptr )  
18.     {  
19.         if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )  
20.             CV_Error( CV_StsBadArg, "Invalid inplace operation" );  
21.         return;  
22.     }  
23.   
24.     if( src->rows > src->cols )  
25.     {  
26.         if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )  
27.             CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );  
28.   
29.         s_dims = CV_MAT_CN(src->type)*src->cols;  
30.         s_count = src->rows;  
31.     }  
32.     else  
33.     {  
34.         if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )  
35.             CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );  
36.   
37.         s_dims = CV_MAT_CN(src->type)*src->rows;  
38.         s_count = src->cols;  
39.     }  
40.   
41.     if( src->rows == 1 || src->cols == 1 )  
42.         src = cvReshape( src, &_src, 1, s_count );  
43.   
44.     if( dst->rows > dst->cols )  
45.     {  
46.         if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )  
47.             CV_Error( CV_StsBadSize,  
48.             "Either the number of channels or columns or rows in the input matrix must be =1" );  
49.   
50.         d_dims = CV_MAT_CN(dst->type)*dst->cols;  
51.         d_count = dst->rows;  
52.     }  
53.     else  
54.     {  
55.         if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )  
56.             CV_Error( CV_StsBadSize,  
57.             "Either the number of channels or columns or rows in the output matrix must be =1" );  
58.   
59.         d_dims = CV_MAT_CN(dst->type)*dst->rows;  
60.         d_count = dst->cols;  
61.     }  
62.   
63.     if( dst->rows == 1 || dst->cols == 1 )  
64.         dst = cvReshape( dst, &_dst, 1, d_count );  
65.   
66.     if( s_count != d_count )  
67.         CV_Error( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );  
68.   
69.     if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )  
70.         CV_Error( CV_StsUnsupportedFormat,  
71.         "Both matrices must be floating-point (single or double precision)" );  
72.   
73.     if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )  
74.         CV_Error( CV_StsOutOfRange,  
75.         "Both input and output point dimensionality must be 2, 3 or 4" );  
76.   
77.     if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )  
78.         CV_Error( CV_StsUnmatchedSizes,  
79.         "The dimensionalities of input and output point sets differ too much" );  
80.   
81.     if( s_dims == d_dims - 1 )  
82.     {  
83.         if( d_count == dst->rows )  
84.         {  
85.             ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));  
86.             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));  
87.         }  
88.         else  
89.         {  
90.             ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));  
91.             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));  
92.         }  
93.     }  
94.   
95.     if( s_dims <= d_dims )  
96.     {  
97.         if( src->rows == dst->rows && src->cols == dst->cols )  
98.         {  
99.             if( CV_ARE_TYPES_EQ( src, dst ) )  
100.                 cvCopy( src, dst );  
101.             else  
102.                 cvConvert( src, dst );  
103.         }  
104.         else  
105.         {  
106.             if( !CV_ARE_TYPES_EQ( src, dst ))  
107.             {  
108.                 temp = cvCreateMat( src->rows, src->cols, dst->type );  
109.                 cvConvert( src, temp );  
110.                 src = temp;  
111.             }  
112.             cvTranspose( src, dst );  
113.         }  
114.   
115.         if( ones )  
116.             cvSet( ones, cvRealScalar(1.) );  
117.     }  
118.     else  
119.     {  
120.         int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;  
121.   
122.         if( !CV_ARE_TYPES_EQ( src, dst ))  
123.         {  
124.             temp = cvCreateMat( src->rows, src->cols, dst->type );  
125.             cvConvert( src, temp );  
126.             src = temp;  
127.         }  
128.   
129.         elem_size = CV_ELEM_SIZE(src->type);  
130.   
131.         if( s_count == src->cols )  
132.             s_plane_stride = src->step / elem_size, s_stride = 1;  
133.         else  
134.             s_stride = src->step / elem_size, s_plane_stride = 1;  
135.   
136.         if( d_count == dst->cols )  
137.             d_plane_stride = dst->step / elem_size, d_stride = 1;  
138.         else  
139.             d_stride = dst->step / elem_size, d_plane_stride = 1;  
140.   
141.         denom = cvCreateMat( 1, d_count, dst->type );  
142.   
143.         if( CV_MAT_DEPTH(dst->type) == CV_32F )  
144.         {  
145.             const float* xs = src->data.fl;  
146.             const float* ys = xs + s_plane_stride;  
147.             const float* zs = 0;  
148.             const float* ws = xs + (s_dims - 1)*s_plane_stride;  
149.   
150.             float* iw = denom->data.fl;  
151.   
152.             float* xd = dst->data.fl;  
153.             float* yd = xd + d_plane_stride;  
154.             float* zd = 0;  
155.   
156.             if( d_dims == 3 )  
157.             {  
158.                 zs = ys + s_plane_stride;  
159.                 zd = yd + d_plane_stride;  
160.             }  
161.   
162.             for( i = 0; i < d_count; i++, ws += s_stride )  
163.             {  
164.                 float t = *ws;  
165.                 iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;  
166.             }  
167.   
168.             cvDiv( 0, denom, denom );  
169.   
170.             if( d_dims == 3 )  
171.                 for( i = 0; i < d_count; i++ )  
172.                 {  
173.                     float w = iw[i];  
174.                     float x = *xs * w, y = *ys * w, z = *zs * w;  
175.                     xs += s_stride; ys += s_stride; zs += s_stride;  
176.                     *xd = x; *yd = y; *zd = z;  
177.                     xd += d_stride; yd += d_stride; zd += d_stride;  
178.                 }  
179.             else  
180.                 for( i = 0; i < d_count; i++ )  
181.                 {  
182.                     float w = iw[i];  
183.                     float x = *xs * w, y = *ys * w;  
184.                     xs += s_stride; ys += s_stride;  
185.                     *xd = x; *yd = y;  
186.                     xd += d_stride; yd += d_stride;  
187.                 }  
188.         }  
189.         else  
190.         {  
191.             const double* xs = src->data.db;  
192.             const double* ys = xs + s_plane_stride;  
193.             const double* zs = 0;  
194.             const double* ws = xs + (s_dims - 1)*s_plane_stride;  
195.   
196.             double* iw = denom->data.db;  
197.   
198.             double* xd = dst->data.db;  
199.             double* yd = xd + d_plane_stride;  
200.             double* zd = 0;  
201.   
202.             if( d_dims == 3 )  
203.             {  
204.                 zs = ys + s_plane_stride;  
205.                 zd = yd + d_plane_stride;  
206.             }  
207.   
208.             for( i = 0; i < d_count; i++, ws += s_stride )  
209.             {  
210.                 double t = *ws;  
211.                 iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;  
212.             }  
213.   
214.             cvDiv( 0, denom, denom );  
215.   
216.             if( d_dims == 3 )  
217.                 for( i = 0; i < d_count; i++ )  
218.                 {  
219.                     double w = iw[i];  
220.                     double x = *xs * w, y = *ys * w, z = *zs * w;  
221.                     xs += s_stride; ys += s_stride; zs += s_stride;  
222.                     *xd = x; *yd = y; *zd = z;  
223.                     xd += d_stride; yd += d_stride; zd += d_stride;  
224.                 }  
225.             else  
226.                 for( i = 0; i < d_count; i++ )  
227.                 {  
228.                     double w = iw[i];  
229.                     double x = *xs * w, y = *ys * w;  
230.                     xs += s_stride; ys += s_stride;  
231.                     *xd = x; *yd = y;  
232.                     xd += d_stride; yd += d_stride;  
233.                 }  
234.         }  
235.     }  
236. }  

2. 对迭代值进行更新的函数。这个函数就是对总的迭代次数进行更新，从中可以看到，迭代值以指数形式减少。最初的为2000的迭代次数，有的时候可能经过不断的更新，最终结果成了几十了。

[cpp]  view plain  copy

 

 

1. CV_IMPL int  
2. cvRANSACUpdateNumIters( double p, double ep,  
3.                         int model_points, int max_iters )  
4. {  
5.     if( model_points <= 0 )  
6.         CV_Error( CV_StsOutOfRange, "the number of model points should be positive" );  
7.   
8.     p = MAX(p, 0.);  
9.     p = MIN(p, 1.);  
10.     ep = MAX(ep, 0.);  
11.     ep = MIN(ep, 1.);  
12.   
13.     // avoid inf's & nan's  
14.     double num = MAX(1. - p, DBL_MIN);  
15.     double denom = 1. - pow(1. - ep,model_points);  
16.     if( denom < DBL_MIN )  
17.         return 0;  
18.   
19.     num = log(num);  
20.     denom = log(denom);  
21.       
22.     return denom >= 0 || -num >= max_iters*(-denom) ?  
23.         max_iters : cvRound(num/denom);  
24. }  

3. 通过4个匹配，计算单应矩阵，就是给你了4个匹配，你把和这四个匹配相符的矩阵计算出来

[cpp]  view plain  copy

 

 

1. //通过四个匹配，计算符合要求的单应矩阵  
2. int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )  
3. {  
4.     int i, count = m1->rows*m1->cols;  
5.     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;  
6.     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;  
7.   
8.     double LtL[9][9], W[9][1], V[9][9];  
9.     CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );  
10.     CvMat matW = cvMat( 9, 1, CV_64F, W );  
11.     CvMat matV = cvMat( 9, 9, CV_64F, V );  
12.     CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );  
13.     CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );  
14.     CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};  
15.   
16.     for( i = 0; i < count; i++ )  
17.     {  
18.         cm.x += m[i].x; cm.y += m[i].y;  
19.         cM.x += M[i].x; cM.y += M[i].y;  
20.     }  
21.   
22.     cm.x /= count; cm.y /= count;  
23.     cM.x /= count; cM.y /= count;  
24.   
25.     for( i = 0; i < count; i++ )  
26.     {  
27.         sm.x += fabs(m[i].x - cm.x);  
28.         sm.y += fabs(m[i].y - cm.y);  
29.         sM.x += fabs(M[i].x - cM.x);  
30.         sM.y += fabs(M[i].y - cM.y);  
31.     }  
32.   
33.     if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||  
34.         fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )  
35.         return 0;  
36.     sm.x = count/sm.x; sm.y = count/sm.y;  
37.     sM.x = count/sM.x; sM.y = count/sM.y;  
38.   
39.     double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };  
40.     double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };  
41.     CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );  
42.     CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );  
43.   
44.     cvZero( &_LtL );  
45.     for( i = 0; i < count; i++ )  
46.     {  
47.         double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;  
48.         double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;  
49.         double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };  
50.         double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };  
51.         int j, k;  
52.         for( j = 0; j < 9; j++ )  
53.             for( k = j; k < 9; k++ )  
54.                 LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];  
55.     }  
56.     cvCompleteSymm( &_LtL );  
57.   
58.     //cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );  
59.     cvEigenVV( &_LtL, &matV, &matW );  
60.     cvMatMul( &_invHnorm, &_H0, &_Htemp );  
61.     cvMatMul( &_Htemp, &_Hnorm2, &_H0 );  
62.     cvConvertScale( &_H0, H, 1./_H0.data.db[8] );  
63.   
64.     return 1;  
65. }  

4. 给定输入序列后，从中随机的选出4对匹配

[cpp]  view plain  copy

 

 

1. bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,  
2.                                    CvMat* ms1, CvMat* ms2, int maxAttempts )  //maxAttempts被设为300  
3. {  
4.     cv::AutoBuffer<int> _idx(modelPoints);  
5.     int* idx = _idx;  
6.     int i = 0, j, k, idx_i, iters = 0;  
7.     int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);  
8.     const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;  
9.     int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;  
10.     int count = m1->cols*m1->rows;  
11.   
12.     assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );  
13.     elemSize /= sizeof(int);  
14.   
15.     for(; iters < maxAttempts; iters++)  
16.     {  
17.         for( i = 0; i < modelPoints && iters < maxAttempts; )  
18.         {  
19.             idx[i] = idx_i = cvRandInt(&rng) % count;    //产生count以内的随机数，count是序列长度  
20.             for( j = 0; j < i; j++ )                    //保证产生的随机数没有重复的  
21.                 if( idx_i == idx[j] )  
22.                     break;  
23.             if( j < i )  
24.                 continue;  
25.             for( k = 0; k < elemSize; k++ )  
26.             {    
27.                 ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];   //把随机产生的数给了ms1和ms2  
28.                 ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];  
29.             }  
30.             if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))   //调用函数checkSubset  
31.             {  
32.                 iters++;  
33.                 continue;  
34.             }  
35.             i++;  
36.         }  
37.         if( !checkPartialSubsets && i == modelPoints &&  
38.             (!checkSubset( ms1, i ) || !checkSubset( ms2, i )))  
39.             continue;  
40.         break;  
41.     }  
42.   
43.     return i == modelPoints && iters < maxAttempts;  
44. }  

5. 对生成的4组匹配进行检验，观察其是否合乎要求。

[cpp]  view plain  copy

 

 

1. bool CvModelEstimator2::checkSubset( const CvMat* m, int count )  
2. {  
3.     int j, k, i, i0, i1;  
4.     CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;  
5.   
6.     assert( CV_MAT_TYPE(m->type) == CV_64FC2 );  
7.       
8.     if( checkPartialSubsets )  
9.         i0 = i1 = count - 1;  
10.     else  
11.         i0 = 0, i1 = count - 1;  
12.       
13.     for( i = i0; i <= i1; i++ )  
14.     {  
15.         // check that the i-th selected point does not belong  
16.         // to a line connecting some previously selected points  
17.         for( j = 0; j < i; j++ )  
18.         {  
19.             double dx1 = ptr[j].x - ptr[i].x;  
20.             double dy1 = ptr[j].y - ptr[i].y;  
21.             for( k = 0; k < j; k++ )  
22.             {  
23.                 double dx2 = ptr[k].x - ptr[i].x;  
24.                 double dy2 = ptr[k].y - ptr[i].y;  
25.                 if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))  
26.                     break;  
27.             }  
28.             if( k < j )  
29.                 break;  
30.         }  
31.         if( j < i )  
32.             break;  
33.     }  
34.   
35.     return i >= i1;  
36. }  

6. 计算内点的个数并且标记序列中哪些点是内点。

[cpp]  view plain  copy

 

 

1. int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,  
2.                                     const CvMat* model, CvMat* _err,  
3.                                     CvMat* _mask, double threshold )  
4. {  
5.     int i, count = _err->rows*_err->cols, goodCount = 0;  
6.     const float* err = _err->data.fl;  
7.     uchar* mask = _mask->data.ptr;  
8.   
9.     computeReprojError( m1, m2, model, _err );  //_err里面是计算后的矩阵的大小，用于与阈值比较  
10.     threshold *= threshold;  
11.     for( i = 0; i < count; i++ )  
12.         goodCount += mask[i] = err[i] <= threshold;//goodCount为计算出的内点的个数  
13.     return goodCount;  
14. }  

7.上面的函数调用的一些函数，这些函数不难，所以下面相应的列举一下

[cpp]  view plain  copy

 

 

1. bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )  
2. {  
3.     CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));  
4.     int i, j, k, count = m1->rows*m1->cols;  
5.     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;  
6.     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;  
7.     CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );  
8.     cvCopy( &modelPart, solver.param );  
9.   
10.     for(;;)  
11.     {  
12.         const CvMat* _param = 0;  
13.         CvMat *_JtJ = 0, *_JtErr = 0;  
14.         double* _errNorm = 0;  
15.   
16.         if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))  
17.             break;  
18.   
19.         for( i = 0; i < count; i++ )  
20.         {  
21.             const double* h = _param->data.db;  
22.             double Mx = M[i].x, My = M[i].y;  
23.             double ww = h[6]*Mx + h[7]*My + 1.;  
24.             ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0;  
25.             double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;  
26.             double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;  
27.             double err[] = { _xi - m[i].x, _yi - m[i].y };  
28.             if( _JtJ || _JtErr )  
29.             {  
30.                 double J[][8] =  
31.                 {  
32.                     { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },  
33.                     { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }  
34.                 };  
35.   
36.                 for( j = 0; j < 8; j++ )  
37.                 {  
38.                     for( k = j; k < 8; k++ )  
39.                         _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];  
40.                     _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];  
41.                 }  
42.             }  
43.             if( _errNorm )  
44.                 *_errNorm += err[0]*err[0] + err[1]*err[1];  
45.         }  
46.     }  
47.   
48.     cvCopy( solver.param, &modelPart );  
49.     return true;  
50. }  
51.   
52. void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,  
53.                                                 const CvMat* model, CvMat* _err )  
54. {  
55.     int i, count = m1->rows*m1->cols;  
56.     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;  
57.     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;  
58.     const double* H = model->data.db;  
59.     float* err = _err->data.fl;  
60.   
61.     for( i = 0; i < count; i++ )  
62.     {  
63.         double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);  
64.         double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;  
65.         double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;  
66.         err[i] = (float)(dx*dx + dy*dy);  
67.     }  
68. }  

8，最后一部分是比较关键的。就是FindHomography函数本身。这个函数又去调用了cvFindHomography函数，估计就是openCV不同版本的函数吧，其实现的功能和思想都是一样的。这个函数内部基本上也就是做一些判断防止溢出，排查错误，检验变量以及变换格式等辅助性的内容，真正的方法性质的代码还是在上面的提到的CvHomographyEstimator类中。

[cpp]  view plain  copy

 

 

1. cv::Mat findHomography( InputArray _points1, InputArray _points2,  
2.                             int method, double ransacReprojThreshold, OutputArray _mask)  
3. {  
4.     Mat points1 = _points1.getMat(), points2 = _points2.getMat();  
5.     int npoints = points1.checkVector(2);//返回矩阵的序列个数  
6.     CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&  
7.                points1.type() == points2.type());  //检验初始条件是否正确  
8.       
9.     Mat H(3, 3, CV_64F);  
10.     CvMat _pt1 = points1, _pt2 = points2;  
11.     CvMat matH = H, c_mask, *p_mask = 0;  
12.     if( _mask.needed() )  
13.     {  
14.         _mask.create(npoints, 1, CV_8U, -1, true);  
15.         p_mask = &(c_mask = _mask.getMat());  
16.     }  
17.     bool ok = cvFindHomography( &_pt1, &_pt2, &matH, method, ransacReprojThreshold, p_mask ) > 0;  //函数调用  
18.     if( !ok )  
19.         H = Scalar(0);  
20.     return H;  
21. }  
22.   
23.   
24.   
25.   
26.   
27. CV_IMPL int  
28. cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,  
29.                   CvMat* __H, int method, double ransacReprojThreshold,  
30.                   CvMat* mask )  
31. {  
32.     const double confidence = 0.995;  
33.     const int maxIters = 2000;                           //修改这里来修改迭代次数  
34.     const double defaultRANSACReprojThreshold = 3;  
35.     bool result = false;  
36.     Ptr<CvMat> m, M, tempMask;  
37.   
38.     double H[9];  
39.     CvMat matH = cvMat( 3, 3, CV_64FC1, H );    //这就是单应矩阵，矩阵初始化  
40.     int count;   
41.   
42.     CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );  
43.   
44.     count = MAX(imagePoints->cols, imagePoints->rows);    //序列个数  
45.     CV_Assert( count >= 4 );  
46.     if( ransacReprojThreshold <= 0 )  
47.         ransacReprojThreshold = defaultRANSACReprojThreshold;  
48.   
49.     m = cvCreateMat( 1, count, CV_64FC2 );  
50.     cvConvertPointsHomogeneous( imagePoints, m );  //转换齐次坐标  
51.   
52.     M = cvCreateMat( 1, count, CV_64FC2 );  
53.     cvConvertPointsHomogeneous( objectPoints, M );  
54.   
55.     if( mask )  
56.     {  
57.         CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&  
58.             (mask->rows == 1 || mask->cols == 1) &&  
59.             mask->rows*mask->cols == count );  
60.     }  
61.     if( mask || count > 4 )  
62.         tempMask = cvCreateMat( 1, count, CV_8U );  
63.     if( !tempMask.empty() )  
64.         cvSet( tempMask, cvScalarAll(1.) );  
65.   
66.         CvHomographyEstimator estimator( MIN(count, 4) );   //参数是一个小于等于4的值，只有大于4，才能用RANSAC计算  
67.     if( count == 4 )  
68.         method = 0;  
69.     if( method == CV_LMEDS )  
70.         //result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );  
71.         printf("");  
72.     else if( method == CV_RANSAC )  
73.         result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);  
74.     else  
75.         result = estimator.runKernel( M, m, &matH ) > 0;  
76.   
77.     if( result && count > 4 )  
78.     {  
79.         icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );  //压缩，使序列紧凑  
80.         count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );  
81.         M->cols = m->cols = count;    //筛选过后，这个count是内点的个数  
82.         if( method == CV_RANSAC )  
83.             estimator.runKernel( M, m, &matH );  //重新计算最终的单应矩阵，matH  
84.         estimator.refine( M, m, &matH, 10 );  
85.     }  
86.   
87.     if( result )  
88.         cvConvert( &matH, __H );  
89.       
90.     if( mask && tempMask )  
91.     {  
92.         if( CV_ARE_SIZES_EQ(mask, tempMask) )    //复制这个矩阵  
93.            cvCopy( tempMask, mask );  
94.         else  
95.            cvTranspose( tempMask, mask );        //行列调换的 复制这个矩阵  
96.     }  
97.   
98.     return (int)result;  
99. }