cvEstimateRigidTransform函数详细注解

cvEstimateRigidTransform是opencv中求取仿射变换的函数，定义在lkpyramid.cpp文件中，该函数先利用ransac算法从所有特征点中选取一定数目的特征点，选取出的这些特征点性质都较好，然后利用icvGetRTMatrix函数求取仿射变换系数，下面是cvEstimateRigidTransform函数的详细注解。

CV_IMPL int
cvEstimateRigidTransform( const CvArr* matA, const CvArr* matB, CvMat* matM, int full_affine )
{
    const int COUNT = 15;
    const int WIDTH = 160, HEIGHT = 120;
    const int RANSAC_MAX_ITERS = 500;
    const int RANSAC_SIZE0 = 3;
    const double RANSAC_GOOD_RATIO = 0.5;

    cv::Ptr<CvMat> sA, sB; //智能指针，相当于c++中的shared_ptr
    cv::AutoBuffer<CvPoint2D32f> pA, pB;
    cv::AutoBuffer<int> good_idx;
    cv::AutoBuffer<char> status;
    cv::Ptr<CvMat> gray;

    CvMat stubA, *A = cvGetMat( matA, &stubA );  //将CvArr*类型的matA转化为CvMat类型的stubA，A是1*192
    CvMat stubB, *B = cvGetMat( matB, &stubB );
    CvSize sz0, sz1;
    int cn, equal_sizes;
    int i, j, k, k1;
    int count_x, count_y, count = 0;
    double scale = 1;
    CvRNG rng = cvRNG(-1);//初始化随机数发生器
    double m[6]={0};
    CvMat M = cvMat( 2, 3, CV_64F, m );
    int good_count = 0;
    CvRect brect;

    if( !CV_IS_MAT(matM) )
        CV_Error( matM ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" );

    if( !CV_ARE_SIZES_EQ( A, B ) )
        CV_Error( CV_StsUnmatchedSizes, "Both input images must have the same size" );

    if( !CV_ARE_TYPES_EQ( A, B ) )
        CV_Error( CV_StsUnmatchedFormats, "Both input images must have the same data type" );

    if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 )  //8位无符号
    {
        cn = CV_MAT_CN(A->type);  //返回通道数
        sz0 = cvGetSize(A);
        sz1 = cvSize(WIDTH, HEIGHT); //160,120

        scale = MAX( (double)sz1.width/sz0.width, (double)sz1.height/sz0.height );
        scale = MIN( scale, 1. );  //scale需小于1
        sz1.width = cvRound( sz0.width * scale );  //sz1的宽高比与原图像的宽高比变得一致
        sz1.height = cvRound( sz0.height * scale );  

        equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height; //如果equal_sizes=1，说明窗口sz1与原图像sz0一样大

        if( !equal_sizes || cn != 1 )  //sz1与图像大小不等或者通道数不为1
        {
            sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 );
            sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 );

            if( cn != 1 )  //通道数不为1
            {
                gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 );
                cvCvtColor( A, gray, CV_BGR2GRAY ); //先转化成灰度图
                cvResize( gray, sA, CV_INTER_AREA ); //再改变图像大小为160*120
                cvCvtColor( B, gray, CV_BGR2GRAY );
                cvResize( gray, sB, CV_INTER_AREA );
                gray.release();
            }
            else
            {
                cvResize( A, sA, CV_INTER_AREA ); //不管输入图像多大，进来之后都会被改成160*120大小
                cvResize( B, sB, CV_INTER_AREA );
            }

            A = sA;
            B = sB;
        }

        count_y = COUNT;  //15
        count_x = cvRound((double)COUNT*sz1.width/sz1.height);
        count = count_x * count_y;

        pA.allocate(count);
        pB.allocate(count);
        status.allocate(count);

        for( i = 0, k = 0; i < count_y; i++ )
            for( j = 0; j < count_x; j++, k++ )
            {
                pA[k].x = (j+0.5f)*sz1.width/count_x;  //初始化
                pA[k].y = (i+0.5f)*sz1.height/count_y;
            }

        // find the corresponding points in B
        cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3,
                                status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 );

        // repack the remained points
        for( i = 0, k = 0; i < count; i++ )
            if( status[i] )   // 需要保留的点
            {
                if( i > k )
                {
                    pA[k] = pA[i];
                    pB[k] = pB[i];
                }
                k++;
            }

        count = k;
    }
    else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 )
    {
        count = A->cols*A->rows; //A是CvMat*类型,上面有A = cvGetMat( matA, &stubA );
        CvMat _pA, _pB;
        pA.allocate(count); // pA, pB是AutoBuffer<CvPoint2D32f> 类型
        pB.allocate(count);
        _pA = cvMat( A->rows, A->cols, CV_32FC2, pA ); //注意这里CV_32FC2是两个通道
        _pB = cvMat( B->rows, B->cols, CV_32FC2, pB );
        cvConvert( A, &_pA ); //#define cvConvert(src, dst ) cvConvertScale((src), (dst), 1, 0 )
        cvConvert( B, &_pB );
    }
    else
        CV_Error( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" );

    good_idx.allocate(count);

    if( count < RANSAC_SIZE0 )
        return 0;

    CvMat _pB = cvMat(1, count, CV_32FC2, pB);
    brect = cvBoundingRect(&_pB, 1);

    // RANSAC stuff:
    // 1. find the consensus
    for( k = 0; k < RANSAC_MAX_ITERS; k++ ) //如果中途出现无法选到足够的点等情况，则重新开始新一轮选点过程，因此这里有个循环
    {
        int idx[RANSAC_SIZE0];
        CvPoint2D32f a[3];
        CvPoint2D32f b[3];

        memset( a, 0, sizeof(a) ); // 将a所指向的某一块内存中的每个字节的内容全部设置为0， 块的大小由第三个参数指定，这个函数通常为新申请的内存做初始化工作， 其返回值为指向S的指针。
        memset( b, 0, sizeof(b) );

        // choose random 3 non-complanar points from A & B
        for( i = 0; i < RANSAC_SIZE0; i++ )  //每个点
        {
            for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ )  //每次选取当前点的迭代次数
            {
                idx[i] = cvRandInt(&rng) % count;  //从所有特征点中随机抽一个点的索引

                for( j = 0; j < i; j++ )  //前面已经抽好的点
                {
                    if( idx[j] == idx[i] )
                        break;
                    // check that the points are not very close one each other
                    if( fabs(pA[idx[i]].x - pA[idx[j]].x) +
                        fabs(pA[idx[i]].y - pA[idx[j]].y) < FLT_EPSILON )
                        break;
                    if( fabs(pB[idx[i]].x - pB[idx[j]].x) +
                        fabs(pB[idx[i]].y - pB[idx[j]].y) < FLT_EPSILON )
                        break;
                }

                if( j < i )   //是从上面的break跳出来的
                    continue;//当前选取的点不行，结束当前点此次的迭代

                if( i+1 == RANSAC_SIZE0 )  //最后一个点
                {
                    // additional check for non-complanar vectors不共线
                    a[0] = pA[idx[0]];
                    a[1] = pA[idx[1]];
                    a[2] = pA[idx[2]];

                    b[0] = pB[idx[0]];
                    b[1] = pB[idx[1]];
                    b[2] = pB[idx[2]];

                    double dax1 = a[1].x - a[0].x, day1 = a[1].y - a[0].y;
                    double dax2 = a[2].x - a[0].x, day2 = a[2].y - a[0].y;
                    double dbx1 = b[1].x - b[0].x, dby1 = b[1].y - b[0].y;
                    double dbx2 = b[2].x - b[0].x, dby2 = b[2].y - b[0].y;
                    const double eps = 0.01;

                    if( fabs(dax1*day2 - day1*dax2) < eps*sqrt(dax1*dax1+day1*day1)*sqrt(dax2*dax2+day2*day2) ||
                        fabs(dbx1*dby2 - dby1*dbx2) < eps*sqrt(dbx1*dbx1+dby1*dby1)*sqrt(dbx2*dbx2+dby2*dby2) )
                        continue;
                }
                break; //程序能运行到这里说明上面对当前点的要求均满足，因此当前点可用，不需再迭代寻找当前点
            }  //当前点的一次迭代结束

            if( k1 >= RANSAC_MAX_ITERS )  //说明迭代了RANSAC_MAX_ITERS次都没找到合适的第i个点
               break;  //不再继续往后找第i+1,i+2,i+3个点，而是准备新一轮的找点，即重新找第0,1,2,3....个点
        }  //当前第i个点结束

        if( i < RANSAC_SIZE0 ) //如果从if( k1 >= RANSAC_MAX_ITERS )跳出循环，即没有找到足够多的点，则会执行此句
            continue; //跳出当前的第k次迭代，准备第k+1轮迭代，即重新找第0,1,2,3....个点

        // estimate the transformation using 3 points
        icvGetRTMatrix( a, b, 3, &M, full_affine );  //函数定义在lkpyramid.cpp中，如果能执行到这里，说明找到了足够多的符合条件的点

        for( i = 0, good_count = 0; i < count; i++ )  //count是所有角点的总个数
        {
            if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) +
                fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < MAX(brect.width,brect.height)*0.05 )
                good_idx[good_count++] = i;
        }

        if( good_count >= count*RANSAC_GOOD_RATIO ) //如果第k次迭代找到的点能很好的代表所有点，则break不再迭代
            break;
    }  //第k次迭代结束

    if( k >= RANSAC_MAX_ITERS )  //所有的迭代结束都没找到合适的一组的点
        return 0; //此时直接返回，M中保留的是最后一次改写后的结果或者为全0(如果最外层的RANSAC_MAX_ITERS次迭代每次都从if( i < RANSAC_SIZE0 )行跳出循环的话)

    if( good_count < count )  //如果执行这句，则说明k < RANSAC_MAX_ITERS 
    {
        for( i = 0; i < good_count; i++ )
        {
            j = good_idx[i];
            pA[i] = pA[j];
            pB[i] = pB[j];
        }
    }

    icvGetRTMatrix( pA, pB, good_count, &M, full_affine );
    m[2] /= scale;
    m[5] /= scale;
    cvConvert( &M, matM );

    return 1;
}