Harris、SHI-TOMASI角点检测opencv源码总结

       最近一个项目需要进行图像的角点检测，但是用opencv提供的各种接口效果并不是很理想，于是想探究一下它们的实现，所以看了其源码并且摘录下来，整理了一下，本想在源码的基础上再修改修改但无耐没有思路。以下使自己的整理：

#include  
#include  
#include 
#include 
#include 
#include 
#include 
#include 
#include
#include
#include
using namespace cv;
using namespace std;

#define SHI_TOMASI 0
#define HARRIS     1
template struct greaterThanPtr
{
    bool operator()(const T* a, const T* b) const { return *a > *b; }
};

void mycalcMinEigenVal( const Mat& _cov, Mat& _dst )
{
	_dst=Mat(_cov.size(),CV_32FC1);
    int i, j;
    Size size = _cov.size();
    if( _cov.isContinuous() && _dst.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
    }

    for( i = 0; i < size.height; i++ )
    {
        const float* cov = (const float*)( _cov.data + _cov.step*i);
        float* dst = (float*)( _dst.data + _dst.step*i);
        j = 0;
        for( ; j < size.width; j++ )
        {
            float a = cov[j*3]*0.5f;
            float b = cov[j*3+1];
            float c = cov[j*3+2]*0.5f;
            dst[j] = (float)((a + c) - std::sqrt((a - c)*(a - c) + b*b));
        }
    }
}
void mycalcHarris( const Mat& _cov, Mat& _dst, double k )
{
	_dst=Mat(_cov.size(),CV_32FC1);
    int i, j;
    Size size = _cov.size();
    if( _cov.isContinuous() && _dst.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
    }

    for( i = 0; i < size.height; i++ )
    {
        const float* cov = (const float*)(_cov.data + _cov.step*i);
        float* dst = (float*)(_dst.data + _dst.step*i);
        j = 0;  
        for( ; j < size.width; j++ )
        {
            float a = cov[j*3];
            float b = cov[j*3+1];
            float c = cov[j*3+2];
            dst[j] = (float)(a*c - b*b - k*(a + c)*(a + c));
        }
    }
}
void mycornerDetection(const Mat& src, Mat& eigenv, int block_size,int aperture_size, int op_type, double k,int borderType=BORDER_DEFAULT )
{
	Mat Dx, Dy;
    if( aperture_size > 0 )
    {
        Sobel( src, Dx, CV_32F, 1, 0, aperture_size, 1, 0, borderType );
        Sobel( src, Dy, CV_32F, 0, 1, aperture_size, 1, 0, borderType );
    }
    else
    {
        Scharr( src, Dx, CV_32F, 1, 0, 1, 0, borderType );
        Scharr( src, Dy, CV_32F, 0, 1, 1, 0, borderType );
    }

    Size size = src.size();
    Mat cov( size, CV_32FC3 );
    int i, j;

    for( i = 0; i < size.height; i++ )
    {
        float* cov_data = (float*)(cov.data + i*cov.step);
        const float* dxdata = (const float*)(Dx.data + i*Dx.step);
        const float* dydata = (const float*)(Dy.data + i*Dy.step);

        for( j = 0; j < size.width; j++ )
        {
            float dx = dxdata[j];
            float dy = dydata[j];

            cov_data[j*3] = dx*dx;
            cov_data[j*3+1] = dx*dy;
            cov_data[j*3+2] = dy*dy;
        }
    }
    GaussianBlur(cov, cov, Size(3, 3),0,0,borderType );
	if( op_type == SHI_TOMASI )
        mycalcMinEigenVal( cov, eigenv );
    else if( op_type == HARRIS )
        mycalcHarris( cov, eigenv, k );
}
void mygoodFeaturesToTrack( Mat image, vector &corners,int maxCorners, double qualityLevel, double minDistance,int blockSize,bool useHarrisDetector, double harrisK )
{
    Mat eig, tmp;
	mycornerDetection(image,eig,blockSize,3,useHarrisDetector,harrisK,4);
    double maxVal = 0;
    minMaxLoc( eig, 0, &maxVal, 0, 0);
    threshold( eig, eig, maxVal*qualityLevel, 0, THRESH_TOZERO );
	Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
    dilate( eig, tmp, element);

    Size imgsize = image.size();

    vector tmpCorners;

    // collect list of pointers to features - put them into temporary image
    for( int y = 1; y < imgsize.height - 1; y++ )
    {
        const float* eig_data = (const float*)eig.ptr(y);
        const float* tmp_data = (const float*)tmp.ptr(y);

        for( int x = 1; x < imgsize.width - 1; x++ )
        {
            float val = eig_data[x];
            if( val != 0 && val == tmp_data[x])
                tmpCorners.push_back(eig_data + x);
        }
    }

    sort( tmpCorners,greaterThanPtr() );
    size_t i, j, total = tmpCorners.size(), ncorners = 0;

    if(minDistance >= 1)
    {
         // Partition the image into larger grids
        int w = image.cols;
        int h = image.rows;

        const int cell_size = cvRound(minDistance);
        const int grid_width = (w + cell_size - 1) / cell_size;
        const int grid_height = (h + cell_size - 1) / cell_size;

        vector> grid(grid_width*grid_height);

        minDistance *= minDistance;

        for( i = 0; i < total; i++ )
        {
            int ofs = (int)((const uchar*)tmpCorners[i] - eig.data);
            int y = (int)(ofs / eig.step);
            int x = (int)((ofs - y*eig.step)/sizeof(float));

            bool good = true;

            int x_cell = x / cell_size;
            int y_cell = y / cell_size;

            int x1 = x_cell - 1;
            int y1 = y_cell - 1;
            int x2 = x_cell + 1;
            int y2 = y_cell + 1;

            // boundary check
            x1 = max(0, x1);
            y1 = max(0, y1);
            x2 = min(grid_width-1, x2);
            y2 = min(grid_height-1, y2);

            for( int yy = y1; yy <= y2; yy++ )
            {
                for( int xx = x1; xx <= x2; xx++ )
                {
                    vector  &m = grid[yy*grid_width + xx];

                    if( m.size() )
                    {
                        for(j = 0; j < m.size(); j++)
                        {
                            float dx = x - m[j].x;
                            float dy = y - m[j].y;

                            if( dx*dx + dy*dy < minDistance )
                            {
                                good = false;
                                goto break_out;
                            }
                        }
                    }
                }
            }

            break_out:

            if(good)
            {
                // printf("%d: %d %d -> %d %d, %d, %d -- %d %d %d %d, %d %d, c=%d\n",
                //    i,x, y, x_cell, y_cell, (int)minDistance, cell_size,x1,y1,x2,y2, grid_width,grid_height,c);
                grid[y_cell*grid_width + x_cell].push_back(Point2f((float)x, (float)y));

                corners.push_back(Point2f((float)x, (float)y));
                ++ncorners;
                if( maxCorners > 0 && (int)ncorners == maxCorners )
                    break;
            }
        }
    }
    else
    {
        for( i = 0; i < total; i++ )
        {
            int ofs = (int)((const uchar*)tmpCorners[i] - eig.data);
            int y = (int)(ofs / eig.step);
            int x = (int)((ofs - y*eig.step)/sizeof(float));

            corners.push_back(Point2f((float)x, (float)y));
            ++ncorners;
            if( maxCorners > 0 && (int)ncorners == maxCorners )
                break;
        }
    }
}
void mycvGoodFeaturesToTrack( IplImage* Image,CvPoint2D32f* Corners, int *corner_count,double quality_level, double min_distance,int block_size,bool use_harris, double harris_k )
{
    Mat image = cvarrToMat(Image);
    vector corners;
    mygoodFeaturesToTrack( image, corners, *corner_count, quality_level,min_distance, block_size, use_harris , harris_k );
    size_t i, ncorners = corners.size();
    for( i = 0; i < ncorners; i++ )
       Corners[i] = corners[i];
    *corner_count = (int)ncorners;
}

int ImageAdjust(IplImage* src, IplImage* dst, double low, double high,double bottom, double top,double gamma )
{
    if(     low<0 && low>1 && high <0 && high>1&&
        bottom<0 && bottom>1 && top<0 && top>1 && low>high)
        return -1;
    double low2 = low*255;
    double high2 = high*255;
    double bottom2 = bottom*255;
    double top2 = top*255;
    double err_in = high2 - low2;
    double err_out = top2 - bottom2;
    int x,y;
    double val;

    // intensity transform
    for( y = 0; y < src->height; y++)
    {
        for (x = 0; x < src->width; x++)
        {
            val = ((uchar*)(src->imageData + src->widthStep*y))[x]; 
            val = pow((val- low2) / err_in, gamma) * err_out + bottom2;
            if(val>255) 
                val=255;
            if(val<0)
                val=0; // Make sure src is in the range [low,high]
            ((uchar*)(dst->imageData + dst->widthStep*y))[x] = (uchar) val;
        }
    }


    return 0;
}

const int maxgamma=100;
int gamma;
IplImage *source=0;
void trackbar(int pos)
{
    IplImage *mask=cvCreateImage(cvGetSize(source),source->depth,source->nChannels);
    IplImage *temp=cvCreateImage(cvGetSize(source),source->depth,source->nChannels);
    cvZero(mask);
    double gamma1=gamma/((double)maxgamma);
    ImageAdjust(source,temp,0,1,0,1,gamma1);
    int cornerCount=6;
    CvPoint2D32f corners[6];
    double qualityLevel = 0.05;
    double minDistance = 20;
    mycvGoodFeaturesToTrack (temp, corners,&cornerCount, qualityLevel, minDistance, 2, HARRIS, 0.01);
    if(cornerCount>0)
    {
        for (int i = 0; i < cornerCount; ++i)
        {
            //cvCircle(Img, cvPoint((int)(corners[i].x+matchLoc.x), (int)(corners[i].y+matchLoc.y)), 6, CV_RGB(0,0,0), 1, CV_AA, 0);
            cvCircle(mask, cvPoint((int)(corners[i].x), (int)(corners[i].y)), 6, CV_RGB(125,125,125), 1, CV_AA, 0);
        }
    } 
    cvAdd(temp,mask,mask);
    cvShowImage("mask",mask);
    cvReleaseImage(&temp);
    cvReleaseImage(&mask);
}

void matchDetector(IplImage* Img,char* filename,vector cornersAfter,int method)
{
    IplImage* templ =cvLoadImage( filename, 0 );
    IplImage* dst=cvCreateImage(cvSize(Img->width-templ->width+1 , Img->height-templ->height+1) , IPL_DEPTH_32F,1);
    cvMatchTemplate( Img, templ, dst, method );
    double minVal; double maxVal; CvPoint minLoc; CvPoint maxLoc;
    CvPoint matchLoc;
    cvMinMaxLoc( dst, &minVal, &maxVal, &minLoc, &maxLoc,NULL );
    if(method==CV_TM_SQDIFF || method == CV_TM_SQDIFF_NORMED )
    {
        matchLoc=minLoc;
    }
    else
    {
        matchLoc=maxLoc;
    }
    //cvRectangle(img,matchLoc,cvPoint(matchLoc.x+templ->width ,matchLoc.y+templ->height),cvScalar(0),2,8,0);
    cvSetImageROI(Img,cvRect(matchLoc.x , matchLoc.y , templ->width , templ->height));
    IplImage *img=cvCreateImage(cvSize(templ->width,templ->height),Img->depth,Img->nChannels);
    cvCopy(Img,img);
    gamma=40;
    source=img;
    cvNamedWindow("mask",0);
    cvCreateTrackbar("gamma","mask",&gamma,maxgamma,trackbar);
    trackbar(0);
    cvWaitKey();
    cvResetImageROI(Img);
    cvReleaseImage(&dst);
    cvReleaseImage(&templ);
}


void main()
{
    IplImage *pImg=cvLoadImage("D://hzz//afterok/CvMFCApplication/20180321/3.jpg",0);
    vector corners;
    matchDetector(pImg,"D://hzz/afterok/CvMFCApplication/20171208/1.jpg",corners,3);
    //matchDetector(pImg,"D://hzz/afterok/CvMFCApplication/20171208/2.jpg",corners,3);
    //matchDetector(pImg,"D://hzz/afterok/CvMFCApplication/20171208/3.jpg",corners,3);
    //matchDetector(pImg,"D://hzz/afterok/CvMFCApplication/20171208/4.jpg",corners,3);
    //cvNamedWindow("pImg",0);
    //cvShowImage("pImg",pImg);
    //cvWaitKey();
}

（本博客纯属个人记录）