SURF(Speed-Up Robust Features)的一个OpenCV实现

File: cvsurf.cpp
/**/ /*
 * An OpenCV Implementation of SURF
 * Further Information Refer to "SURF: Speed-Up Robust Feature"
 * Author: Liu Liu
 * liuliu.1987+opencv@gmail.com
 *
 * There are still serveral lacks for this experimental implementation:
 * 1.The interpolation of sub-pixel mentioned in article was not implemented yet;
 * 2.A comparision with original libSurf.so shows that the hessian detector is not a 100% match to their implementation;
 * 3.Due to above reasons, I recommanded the original one for study and reuse;
 *
 * However, the speed of this implementation is something comparable to original one.
 */

#include  " cvsurf.h "
#include  < iostream >

#define  ScanOctave (3)
#define  FilterScale (4)
#define  SamplingStep (1)

CV_INLINE CvSURFPoint cvSURFPoint(  int  x,  int  y,  int  laplacian,  int  size,  int  octave,  int  scale )
... {
    CvSURFPoint p;
    p.x = x;
    p.y = y;
    p.laplacian = laplacian;
    p.size = size;
    p.octave = octave;
    p.scale = scale;
    return p;
}

CV_INLINE  double
icvCalHaarPattern(  int *  origin,
            int *  t,
            int  widthStep )
... {
    double d = 0;
    int *p0 = 0, *p1 = 0, *p2 = 0, *p3 = 0;
    int n = t[0];
    for ( int k = 0; k < n; k++ )
    ...{
        p0 = origin+t[1]+t[2]*widthStep;
        p1 = origin+t[1]+t[4]*widthStep;
        p2 = origin+t[3]+t[2]*widthStep;
        p3 = origin+t[3]+t[4]*widthStep;
        d += (double)((*p3-*p2-*p1+*p0)*t[6])/(double)(t[5]);
        t+=6;
    }
    return d;
}

CV_INLINE  void
icvResizeHaarPattern(  int *  t_s,
               int *  t_d,
               int  OldSize,
               int  NewSize )
... {
    int n = t_d[0] = t_s[0];
    for ( int k = 0; k < n; k++ )
    ...{
        t_d[1] = t_s[1]*NewSize/OldSize;
        t_d[2] = t_s[2]*NewSize/OldSize;
        t_d[3] = t_s[3]*NewSize/OldSize;
        t_d[4] = t_s[4]*NewSize/OldSize;
        t_d[5] = (t_d[3]-t_d[1]+1)*(t_d[4]-t_d[2]+1);
        t_d[6] = t_s[6];
        t_d+=6;
        t_s+=6;
    }
}

template < typename Number >
CV_INLINE  int
icvSign( Number x )
... {
    return (( x < 0 ) ? -1 : 1);
}

CvSeq *
icvFastHessianDetector(  const  CvMat *  sum,
            CvMemStorage *  storage,
             double  quality )
... {
    double t = (double)cvGetTickCount();
    CvSeq* points = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSURFPoint), storage );
    CvMat* hessians[ScanOctave*(FilterScale+2)];
    CvMat* traces[ScanOctave*(FilterScale+2)];
    int size, size_cache[ScanOctave*(FilterScale+2)];
    int scale, scale_cache[ScanOctave*(FilterScale+2)];
    double *hessian_ptr, *hessian_ptr_cache[ScanOctave*(FilterScale+2)];
    double *trace_ptr, *trace_ptr_cache[ScanOctave*(FilterScale+2)];
    int dx_s[] = ...{ 3, 0, 2, 3, 7, 0, 1, 3, 2, 6, 7, 0, -2, 6, 2, 9, 7, 0, 1 };
    int dy_s[] = ...{ 3, 2, 0, 7, 3, 0, 1, 2, 3, 7, 6, 0, -2, 2, 6, 7, 9, 0, 1 };
    int dxy_s[] = ...{ 4, 1, 1, 4, 4, 0, 1, 5, 1, 8, 4, 0, -1, 1, 5, 4, 8, 0, -1, 5, 5, 8, 8, 0, 1 };
    int dx_t[] = ...{ 3, 0, 2, 3, 7, 0, 1, 3, 2, 6, 7, 0, -2, 6, 2, 9, 7, 0, 1 };
    int dy_t[] = ...{ 3, 2, 0, 7, 3, 0, 1, 2, 3, 7, 6, 0, -2, 2, 6, 7, 9, 0, 1 };
    int dxy_t[] = ...{ 4, 1, 1, 4, 4, 0, 1, 5, 1, 8, 4, 0, -1, 1, 5, 4, 8, 0, -1, 5, 5, 8, 8, 0, 1 };
    double dx = 0, dy = 0, dxy = 0;
    int k = 0;
    int hessian_rows, hessian_rows_cache[ScanOctave*(FilterScale+2)];
    int hessian_cols, hessian_cols_cache[ScanOctave*(FilterScale+2)];
    /**//* hessian detector */
    for ( int o = 0; o < ScanOctave; o++ )
    ...{
        t = (double)cvGetTickCount();
        for ( int s = -1; s < FilterScale+1; s++ )
        ...{
            if ( s < 0 )
                size_cache[k] = size = 7<<o; // gaussian scale 1.0;
            else
                size_cache[k] = size = (s*6+9)<<o; // gaussian scale size*1.2/9.;
            scale_cache[k] = scale = MAX( size, 9 )*SamplingStep;
            hessian_rows_cache[k] = hessian_rows = (sum->rows)*9/scale;
            hessian_cols_cache[k] = hessian_cols = (sum->cols)*9/scale;
            hessians[k] = cvCreateMat( hessian_rows, hessian_cols, CV_64FC1 );
            traces[k] = cvCreateMat( hessian_rows, hessian_cols, CV_64FC1 );
            int* sum_ptr = (int*)sum->data.ptr;
            icvResizeHaarPattern( dx_s, dx_t, 9, size );
            icvResizeHaarPattern( dy_s, dy_t, 9, size );
            icvResizeHaarPattern( dxy_s, dxy_t, 9, size );
            hessian_ptr_cache[k] = hessian_ptr = (double*)hessians[k]->data.ptr;
            trace_ptr_cache[k] = trace_ptr = (double*)traces[k]->data.ptr;
            hessian_ptr+=4/SamplingStep+(4/SamplingStep)*hessian_cols;
            trace_ptr+=4/SamplingStep+(4/SamplingStep)*hessian_cols;
            int oy = 0, y = 0;
            for ( int j = 0; j < hessian_rows-9/SamplingStep; j++ )
            ...{
                int * sum_line_ptr = sum_ptr;
                double* trace_line_ptr = trace_ptr;
                double* hessian_line_ptr = hessian_ptr;
                int ox = 0, x = 0;
                for ( int i = 0; i < hessian_cols-9/SamplingStep; i++ )
                ...{
                    dx = icvCalHaarPattern( sum_line_ptr, dx_t, sum->cols );
                    dy = icvCalHaarPattern( sum_line_ptr, dy_t, sum->cols );
                    dxy = icvCalHaarPattern( sum_line_ptr, dxy_t, sum->cols );
                    *hessian_line_ptr = (dx*dy-dxy*dxy*0.81);
                    *trace_line_ptr = dx+dy;
                    x = (i+1)*scale/9;
                    sum_line_ptr+=x-ox;
                    ox = x;
                    trace_line_ptr++;
                    hessian_line_ptr++;
                }
                y = (j+1)*scale/9;
                sum_ptr+=(y-oy)*sum->cols;
                oy = y;
                trace_ptr+=hessian_cols;
                hessian_ptr+=hessian_cols;
            }
            k++;
        }
        t = (double)cvGetTickCount()-t;
        printf( "octave time = %gms ", t/((double)cvGetTickFrequency()*1000.) );
    }
    double min_accept = quality*300;
    //t = (double)cvGetTickCount()-t;
    //printf( "hessian filter time = %gms ", t/((double)cvGetTickFrequency()*1000.) );
        t = (double)cvGetTickCount();
    k = 0;
    for ( int o = 0; o < ScanOctave; o++ )
    ...{
        k++;
        for ( int s = 0; s < FilterScale; s++ )
        ...{
            size = size_cache[k];
            scale = scale_cache[k];
            hessian_rows = hessian_rows_cache[k];
            hessian_cols = hessian_cols_cache[k];
            int margin = (5/SamplingStep)*scale_cache[k+1]/scale;
            hessian_ptr = hessian_ptr_cache[k]+margin+margin*hessian_cols;
            trace_ptr = trace_ptr_cache[k];
            for ( int j = margin; j < hessian_rows-margin; j++ )
            ...{
                double* hessian_line_ptr = hessian_ptr;
                for ( int i = margin; i < hessian_cols-margin; i++ )
                ...{
                    if ( *hessian_line_ptr > min_accept )
                    ...{
                        bool suppressed = false;
                        /**//* non-maxima suppression */
                        for ( int z = k-1; z < k+2; z++ )
                        ...{
                            double* temp_hessian_ptr = hessian_ptr_cache[z]+i*scale/scale_cache[z]-1+(j*scale/scale_cache[z]-1)*hessian_cols_cache[z];
                            for ( int y = 0; y < 3; y++ )
                            ...{
                                double* temp_hessian_line_ptr = temp_hessian_ptr;
                                for ( int x = 0; x < 3; x++ )
                                ...{
                                    if ((( z != k )||( y != 1 )||( x != 1 ))&&( *temp_hessian_line_ptr > *hessian_line_ptr ))
                                    ...{
                                        suppressed = true;
                                        break;
                                    }
                                    temp_hessian_line_ptr++;
                                }
                                if ( suppressed )
                                    break;
                                temp_hessian_ptr+=hessian_cols_cache[z];
                            }
                            if ( suppressed )
                                break;
                        }
                        if ( !suppressed )
                        ...{
                            CvSURFPoint point = cvSURFPoint( i*scale/9, j*scale/9, icvSign(trace_ptr[i+j*hessian_cols]), size_cache[k], o, s );
                            cvSeqPush( points, &point );
                        }
                    }
                    hessian_line_ptr++;
                }
                hessian_ptr+=hessian_cols;
            }
            k++;
        }
        k++;
    }
    k = 0;
    for ( int o = 0; o < ScanOctave; o++ )
        for ( int s = -1; s < FilterScale+1; s++ )
        ...{
            cvReleaseMat( &hessians[k] );
            cvReleaseMat( &traces[k] );
            k++;
        }
        t = (double)cvGetTickCount()-t;
        printf( "hessian selector time = %gms ", t/((double)cvGetTickFrequency()*1000.) );
    return points;
}

void
icvSURFGaussian( CvMat *  mat,  double  s )
... {
    int w = mat->cols;
    int h = mat->rows;
    double x, y;
    double c2 = 1./(s*s*2);
    double over_exp = 1./(3.14159*2*s*s);
    for ( int i = 0; i < w; i++ )
        for ( int j = 0; j < h; j++ )
        ...{
            x = i-w/2.;
            y = j-h/2.;
            cvmSet( mat, j, i, exp(-(x*x+y*y)*c2)*over_exp );
        }
}

CvMat *  wrap  =   0 ;
IplImage *  regions_cache[ScanOctave * FilterScale];
IplImage *  region_cache;
CvMat *  dx_cache;
CvMat *  dy_cache;
CvMat *  gauss_kernel_cache;
double  CosCache[ 3600 ];
double  SinCache[ 3600 ];

void
cvSURFInitialize()
... {
    wrap = cvCreateMat( 2, 3, CV_32FC1 );
    int k = 0;
    for ( int o = 0; o < ScanOctave; o++ )
        for ( int s = 0; s < FilterScale; s++ )
        ...{
            double scal = ((s*6+9)<<o)*1.2/9.;
            regions_cache[k] = cvCreateImage( cvSize(cvRound(21*scal), cvRound(21*scal)), 8, 1 );
            k++;
        }
    region_cache = cvCreateImage( cvSize(21, 21), 8, 1 );
    dx_cache = cvCreateMat( 20, 20, CV_64FC1 );
    dy_cache = cvCreateMat( 20, 20, CV_64FC1 );
    gauss_kernel_cache = cvCreateMat( 20, 20, CV_64FC1 );
    icvSURFGaussian( gauss_kernel_cache, 3.3 );
    for ( int i = 0; i < 3600; i++ )
    ...{
        CosCache[i] = cos(i*0.001745329);
        SinCache[i] = sin(i*0.001745329);
    }
}

void
cvSURFFinalize()
... {
    cvReleaseMat( &wrap );
    int k = 0;
    for ( int o = 0; o < ScanOctave; o++ )
        for ( int s = 0; s < FilterScale; s++ )
        ...{
            cvReleaseImage( &regions_cache[k] );
            k++;
        }
    cvReleaseImage( &region_cache );
    cvReleaseMat( &dx_cache );
    cvReleaseMat( &dy_cache );
    cvReleaseMat( &gauss_kernel_cache );
}

CvSeq *
cvSURFDescriptor(  const  CvArr *  _img,
          CvMemStorage *  storage,
           double  quality,
           int  flags )
... {
    IplImage* img = (IplImage*)_img;
    CvMat* sum = 0;
    sum = cvCreateMat( img->height+1, img->width+1, CV_32SC1 );
    cvIntegral( img, sum );
    CvMemStorage* point_storage = cvCreateChildMemStorage( storage );
    CvSeq* points = icvFastHessianDetector( sum, point_storage, quality );
        double t = (double)cvGetTickCount();
    CvSeq* descriptors = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSURFDescriptor), storage );
    int dx_s[] = ...{2, 0, 0, 2, 4, 0, -1, 2, 0, 4, 4, 0, 1};
    int dy_s[] = ...{2, 0, 0, 4, 2, 0, -1, 0, 2, 4, 4, 0, 1};
    int dx_t[] = ...{2, 0, 0, 2, 4, 0, -1, 2, 0, 4, 4, 0, 1};
    int dy_t[] = ...{2, 0, 0, 4, 2, 0, -1, 0, 2, 4, 4, 0, 1};
    double x[81], *iter_x;
    double y[81], *iter_y;
    double angle[81], *iter_angle;
    double sumx, sumy;
    double temp_mod;
    int angle_n;
    for ( int k = 0; k < points->total; k++ )
    ...{
        CvSURFPoint* point = (CvSURFPoint*)cvGetSeqElem( points, k );
        CvSURFDescriptor descriptor;
        descriptor.x = cvRound(point->x);
        descriptor.y = cvRound(point->y);
        descriptor.laplacian = point->laplacian;
        int size = point->size;
        int layer = point->octave*FilterScale+point->scale;
        descriptor.s = size*1.2/9.;
        descriptor.mod = 0;
        /**//* repeatable orientation */
        iter_x = x;
        iter_y = y;
        iter_angle = angle;
        angle_n = 0;
        icvResizeHaarPattern( dx_s, dx_t, 9, size );
        icvResizeHaarPattern( dy_s, dy_t, 9, size );
        int* sum_ptr = (int*)sum->data.ptr;
        double c2 = 1./(descriptor.s*descriptor.s*2.5*2.5*2);
        double over_exp = 1./(3.14159*2*descriptor.s*descriptor.s*2.5*2.5);
        for ( int j = -6; j <=2; j++ )
        ...{
            int y = descriptor.y+j*size/9;
            if (( y >= 0 )&&( y < sum->rows-size ))
            ...{
                double ry = j+2;
                for ( int i = -6; i <=2; i++ )
                ...{
                    int x = descriptor.x+i*size/9;
                    if (( x >= 0 )&&( x < sum->cols-size ))
                    ...{
                        double rx = j+2;
                        double radius = rx*rx+ry*ry;
                        if ( radius <= 16 )
                        ...{
                            rx*=descriptor.s;
                            ry*=descriptor.s;
                            *iter_x = icvCalHaarPattern( sum_ptr+x+y*sum->cols, dx_t, sum->cols )*exp(-radius*c2)*over_exp;
                            *iter_y = icvCalHaarPattern( sum_ptr+x+y*sum->cols, dy_t, sum->cols )*exp(-radius*c2)*over_exp;
                            *iter_angle = cvFastArctan( *iter_y, *iter_x );
                            iter_x++;
                            iter_y++;
                            iter_angle++;
                            angle_n++;
                        }
                    }
                }
            }
        }
        double bestx = 0;
        double besty = 0;
        for ( int i = 0; i < 360; i+=5 )
        ...{
            sumx = 0;
            sumy = 0;
            iter_x = x;
            iter_y = y;
            iter_angle = angle;
            for ( int j = 0; j < angle_n; j++ )
            ...{
                if ( ( ( *iter_angle < i+60 )&&( *iter_angle > i ) )||
                ( ( (*iter_angle+360) < i+60 )&&( (*iter_angle+360) > i ) ) )
                ...{
                    sumx+=*iter_x;
                    sumy+=*iter_y;
                }
                iter_x++;
                iter_y++;
                iter_angle++;
            }
            temp_mod = sumx*sumx+sumy*sumy;
            if ( temp_mod > descriptor.mod )
            ...{
                descriptor.mod = temp_mod;
                bestx = sumx;
                besty = sumy;
            }
        }
        descriptor.dir = cvFastArctan( besty, bestx );
        /**//* get sub-region (CV_INTER_AREA approximately retain the information of total image for haar feature while reduce the time consuming */
        double cos_dir = CosCache[MAX(cvRound(descriptor.dir*10)+3600, 0)%3600];
        double sin_dir = SinCache[MAX(cvRound(descriptor.dir*10)+3600, 0)%3600];
        cvmSet( wrap, 0, 0, cos_dir );
        cvmSet( wrap, 0, 1, -sin_dir );
        cvmSet( wrap, 0, 2, descriptor.x );
        cvmSet( wrap, 1, 0, sin_dir );
        cvmSet( wrap, 1, 1, cos_dir );
        cvmSet( wrap, 1, 2, descriptor.y );

        cvGetQuadrangleSubPix( img, regions_cache[layer], wrap );
        cvResize( regions_cache[layer], region_cache, CV_INTER_AREA );
        uchar* region_d;
        int region_step;
        cvGetImageRawData( region_cache, &region_d, &region_step );
        uchar* region_x = region_d+1;
        uchar* region_y = region_d+region_step;
        uchar* region_xy = region_d+1+region_step;
        region_step-=20;
        double* iter_dx = (double*)dx_cache->data.ptr;
        double* iter_dy = (double*)dy_cache->data.ptr;
        for ( int i = 0; i < 20; i++ )
        ...{
            for ( int j = 0; j < 20; j++ )
            ...{
                *iter_dx = *region_y-*region_d-*region_x+*region_xy;
                *iter_dy = *region_x-*region_d-*region_y+*region_xy;
                iter_dx++;
                iter_dy++;
                region_d++;
                region_x++;
                region_y++;
                region_xy++;
            }
            region_d+=region_step;
            region_x+=region_step;
            region_y+=region_step;
            region_xy+=region_step;
        }
        cvMul( gauss_kernel_cache, dx_cache, dx_cache );
        cvMul( gauss_kernel_cache, dy_cache, dy_cache );
        
        double tx, ty;
        double* iter_vector = descriptor.vector;
        if ( flags&CV_SURF_EXTENDED )
        ...{
            /**//* 128-bin descriptor */
            for ( int i = 0; i < 4; i++ )
                for ( int j = 0; j < 4; j++ )
                ...{
                    iter_vector[0] = 0;
                    iter_vector[1] = 0;
                    iter_vector[2] = 0;
                    iter_vector[3] = 0;
                    iter_vector[4] = 0;
                    iter_vector[5] = 0;
                    iter_vector[6] = 0;
                    iter_vector[7] = 0;
                    for ( int x = i*5; x < i*5+5; x++ )
                    ...{
                        for ( int y = j*5; y < j*5+5; y++ )
                        ...{
                            tx = cvGetReal2D( dx_cache, x, y );
                            ty = cvGetReal2D( dy_cache, x, y );
                            if ( ty >= 0 )
                            ...{
                                iter_vector[0] += tx;
                                iter_vector[1] += fabs(tx);
                            } else ...{
                                iter_vector[2] += tx;
                                iter_vector[3] += fabs(tx);
                            }
                            if ( tx >= 0 )
                            ...{
                                iter_vector[4] += ty;
                                iter_vector[5] += fabs(ty);
                            } else ...{
                                iter_vector[6] += ty;
                                iter_vector[7] += fabs(ty);
                            }
                        }
                    }
                    /**//* unit vector is essential for contrast invariant */
                    double normalize = 0;
                    for ( int k = 0; k < 8; k++ )
                        normalize+=iter_vector[k]*iter_vector[k];
                    normalize = sqrt(normalize);
                    for ( int k = 0; k < 8; k++ )
                        iter_vector[k] = iter_vector[k]/normalize;
                    iter_vector+=8;
                }
        } else ...{
            /**//* 64-bin descriptor */
            for ( int i = 0; i < 4; i++ )
                for ( int j = 0; j < 4; j++ )
                ...{
                    iter_vector[0] = 0;
                    iter_vector[1] = 0;
                    iter_vector[2] = 0;
                    iter_vector[3] = 0;
                    for ( int x = i*5; x < i*5+5; x++ )
                    ...{
                        for ( int y = j*5; y < j*5+5; y++ )
                        ...{
                            tx = cvGetReal2D( dx_cache, x, y );
                            ty = cvGetReal2D( dy_cache, x, y );
                            iter_vector[0] += tx;
                            iter_vector[1] += ty;
                            iter_vector[2] += fabs(tx);
                            iter_vector[3] += fabs(ty);
                        }
                    }
                    double normalize = 0;
                    for ( int k = 0; k < 4; k++ )
                        normalize+=iter_vector[k]*iter_vector[k];
                    normalize = sqrt(normalize);
                    for ( int k = 0; k < 4; k++ )
                        iter_vector[k] = iter_vector[k]/normalize;
                    iter_vector+=4;
                }
        }

        cvSeqPush( descriptors, &descriptor );
    }
    cvReleaseMemStorage( &point_storage );
    cvReleaseMat( &sum );
        t = (double)cvGetTickCount()-t;
        printf( "descriptor time = %gms ", t/((double)cvGetTickFrequency()*1000.) );
    return descriptors;
}

inline
double
icvCompareSURFDescriptor( CvSURFDescriptor *  descriptor1,
              CvSURFDescriptor *  descriptor2,
               double  best,
               int  length  =   64  )
... {
    double* iter_vector1 = descriptor1->vector;
    double* iter_vector2 = descriptor2->vector;
    double total_cost = 0;
    for ( int i = 0; i < length; i++ )
    ...{
        total_cost+=(*iter_vector1-*iter_vector2)*(*iter_vector1-*iter_vector2);
        if ( total_cost > best )
            break;
        iter_vector1++;
        iter_vector2++;
    }
    return total_cost;
}

inline  int
icvNaiveNearestNeighbor( CvSURFDescriptor *  descriptor,
             CvSeq *  model_descriptors,
              int  length )
... {
    int neighbor = -1;
    double d;
    double dist1 = 0xffffff, dist2 = 0xffffff;
    for ( int i = 0; i < model_descriptors->total; i++ )
    ...{
        CvSURFDescriptor* model_descriptor = (CvSURFDescriptor*)cvGetSeqElem( model_descriptors, i );
        if ( descriptor->laplacian != model_descriptor->laplacian )
            continue;
        d = icvCompareSURFDescriptor( descriptor, model_descriptor, dist2, length );
        if ( d < dist1 )
        ...{
            dist2 = dist1;
            dist1 = d;
            neighbor = i;
        } else ...{
            if ( d < dist2 )
                dist2 = d;
        }
    }
    if ( dist1 < 0.6*dist2 )
        return neighbor;
    return -1;
}

/**/ /* a rough implementation for object location */
bool
cvSURFObjectLocate( CvSeq *  ImageDescriptor,
            CvSeq *  ObjectDescriptor,
            CvPoint *  points,
             int  flags )
... {
    CvSURFDescriptor* correspond = (CvSURFDescriptor*)malloc( sizeof(CvSURFDescriptor)*ObjectDescriptor->total*2 );
    int length = ( flags&CV_SURF_EXTENDED ) ? 128 : 64;

    int chaos, correspond_total = 0;
    CvSURFDescriptor* correspond_ptr = correspond;
    for ( int i = 0; i < ObjectDescriptor->total; i++ )
    ...{
        CvSURFDescriptor* descriptor = (CvSURFDescriptor*)cvGetSeqElem( ObjectDescriptor, i );
        int nearest_neighbor = icvNaiveNearestNeighbor( descriptor, ImageDescriptor, length );
        if ( nearest_neighbor >= 0 )
        ...{
            *correspond_ptr = *descriptor;
            correspond_ptr++;
            *correspond_ptr = *(CvSURFDescriptor*)cvGetSeqElem( ImageDescriptor, nearest_neighbor );
            correspond_ptr++;
            chaos+=nearest_neighbor;
            correspond_total++;
        }
    }

    int correspond_n = correspond_total;

    if ( correspond_n < 4 )
        return 0;

    CvMat* correspond_points1;
    CvMat* correspond_points2;
    CvMat* homography_matrix;

    correspond_points1 = cvCreateMat( 4, 1, CV_32FC2 );
    correspond_points2 = cvCreateMat( 4, 1, CV_32FC2 );
    homography_matrix = cvCreateMat( 3, 3, CV_32FC1 );

    CvRNG rng = cvRNG(chaos);
    double h11, h12, h13, h21, h22, h23, h31, h32, h33;
    double x, y, z, d;
    double best_d = 0xffffff;
    int best_clan = 0;
    int clan = 0;
    CvPoint corner[4];
    for ( int k = 0; k < MIN( correspond_n*100, correspond_n*(correspond_n-1)*(correspond_n-2) ); k++ )
    ...{
        int selector[4];
        for( int i = 0; i < 4; i++ )
        ...{
            while ( selector[i] = cvRandInt(&rng)%correspond_n )
            ...{
                bool recalc = false;
                for ( int j = 0; j < i; j++ )
                    if ( selector[i] == selector[j] )
                        recalc = true;
                if ( !recalc )
                    break;
            }
            CvSURFDescriptor* correspond_descriptor1 = correspond+(selector[i]<<1);
            CvSURFDescriptor* correspond_descriptor2 = correspond_descriptor1+1;
            cvSet2D( correspond_points1, i, 0, cvScalar( (float)correspond_descriptor1->x, (float)correspond_descriptor1->y ) );
            cvSet2D( correspond_points2, i, 0, cvScalar( (float)correspond_descriptor2->x, (float)correspond_descriptor2->y ) );
        }
        cvFindHomography( correspond_points1, correspond_points2, homography_matrix );
        h11 = cvmGet( homography_matrix, 0, 0 );
        h12 = cvmGet( homography_matrix, 0, 1 );
        h13 = cvmGet( homography_matrix, 0, 2 );
        h21 = cvmGet( homography_matrix, 1, 0 );
        h22 = cvmGet( homography_matrix, 1, 1 );
        h23 = cvmGet( homography_matrix, 1, 2 );
        h31 = cvmGet( homography_matrix, 2, 0 );
        h32 = cvmGet( homography_matrix, 2, 1 );
        h33 = cvmGet( homography_matrix, 2, 2 );
        d = 0;
        clan = 0;
        for ( int i = 0; i < correspond_n; i++ )
        ...{
            CvSURFDescriptor* correspond_descriptor1 = correspond+(i<<1);
            CvSURFDescriptor* correspond_descriptor2 = correspond_descriptor1+1;
            z = 1/(h31*correspond_descriptor1->x+h32*correspond_descriptor1->y+h33);
            x = (h11*correspond_descriptor1->x+h12*correspond_descriptor1->y+h13)*z-correspond_descriptor2->x;
            y = (h21*correspond_descriptor1->x+h22*correspond_descriptor1->y+h23)*z-correspond_descriptor2->y;
            if ( x*x+y*y < 256 )
            ...{
                d+=x*x+y*y;
                clan++;
            }
        }
        if ( ( clan >= best_clan )&&( d < best_d ) )
        ...{
            for ( int i = 0; i < 4; i++ )
            ...{
                z = 1./(points[i].x*h31+points[i].y*h32+h33);
                corner[i] = cvPoint( (int)((points[i].x*h11+points[i].y*h12+h13)*z),
                             (int)((points[i].x*h21+points[i].y*h22+h23)*z) );
            }
            bool err = false;
            char dir = 0;
            for ( int i = 0; i < 4; i++ )
            ...{
                int vx1 = corner[(i+1)%4].x-corner[i%4].x;
                int vy1 = corner[(i+1)%4].y-corner[i%4].y;
                int vx2 = corner[(i+2)%4].x-corner[(i+1)%4].x;
                int vy2 = corner[(i+2)%4].y-corner[(i+1)%4].y;
                char tmp_dir = ( vx2*vy1-vx1*vy2 > 0 ) ? 1 : -1;
                if ( dir != 0 )
                ...{
                    if ( dir != tmp_dir )
                    ...{
                        err = true;
                        break;
                    }
                } else
                    dir = tmp_dir;
            }
            if ( !err )
            ...{
                best_d = d;
                best_clan = clan;
            }
        }
    }

    cvReleaseMat( &homography_matrix );
    cvReleaseMat( &correspond_points1 );
    cvReleaseMat( &correspond_points2 );
    free( correspond );

    if ( best_clan < 4 )
        return 0;

    for ( int i = 0; i < 4; i++ )
        points[i] = corner[i];

    return 1;
}

CvSeq *
cvSURFFindPair( CvSeq *  ImageDescriptor,
        CvSeq *  ObjectDescriptor,
        CvMemStorage *  storage,
         int  flags )
... {
    CvSeq* correspond = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSURFDescriptor), storage );
    int length = ( flags&CV_SURF_EXTENDED ) ? 128 : 64;
    int chaos;
    for ( int i = 0; i < ObjectDescriptor->total; i++ )
    ...{
        CvSURFDescriptor* descriptor = (CvSURFDescriptor*)cvGetSeqElem( ObjectDescriptor, i );
        int nearest_neighbor = icvNaiveNearestNeighbor( descriptor, ImageDescriptor, length );
        if ( nearest_neighbor >= 0 )
        ...{
            cvSeqPush( correspond, descriptor );
            cvSeqPush( correspond, (CvSURFDescriptor*)cvGetSeqElem( ImageDescriptor, nearest_neighbor ) );
            chaos += nearest_neighbor;
        }
    }

    return correspond;
}

File: cvsurf.h

#ifndef GUARD_cvsurf_h
#define  GUARD_cvsurf_h

#include  < cv.h >

#define  CV_SURF_EXTENDED (1)

typedef  struct  CvSURFDescriptor
... {
    int x;
    int y;
    int laplacian;
    double s;
    double dir;
    double mod;
    double vector[128];
}  CvSURFDescriptor;

typedef  struct  CvSURFPoint
... {
    int x;
    int y;
    int laplacian;
    int size;
    int octave;
    int scale;
}  CvSURFPoint;

CvSeq *  cvSURFDescriptor(  const  CvArr *  _img, CvMemStorage *  storage,  double  quality,  int  flags  =   0  );

void  cvSURFInitialize();

void  cvSURFFinalize();

bool  cvSURFObjectLocate( CvSeq *  ImageDescriptor, CvSeq *  ObjectDescriptor, CvPoint *  points,  int  flags  =   0  );

CvSeq *  cvSURFFindPair( CvSeq *  ImageDescriptor, CvSeq *  ObjectDescriptor, CvMemStorage *  storage,  int  flags );

#endif