harris corner detection 实现

关于harris corner的原理，参照  http://en.wikipedia.org/wiki/Corner_detection

以下为自己的实现，并于opencv进行了比较

//
// implement harris corner detection algorithm by my self and then compare it with the API in opencv
/
/**
 * @brief myCornerHarris
 * @param src_ input image, only has one channel
 * @param dst all pixels' reponse R = det(M) - k*trace(M)^2 ,M is auto correclation matrix;
 * @param blockSize
 * @param kSize
 * @param k
 * @param borderType
 */
void myCornerHarris(const Mat& src_, Mat& dst, int blockSize, int kSize, double k)
{
    Mat src;
    src_.copyTo(src);
    dst.create(src.size(),CV_32F);

    int depth = src.depth();
    double scale = (double)(1 << ((kSize > 0 ? kSize : 3) - 1)) * blockSize;
    if( depth == CV_8U )
        scale *= 255.;
    scale = 1./scale;

    assert(src.type() == CV_8UC1 || src.type() == CV_32FC1);

    Mat dx,dy;
    Sobel(src,dx,CV_32F,1,0,kSize,scale,0);
    Sobel(src,dy,CV_32F,0,1,kSize,scale,0);

    Size size = src.size();
    Mat cov(size,CV_32FC3);
    int i,j;
    for(i = 0;i < size.height;i++){
        float *covData = (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];

            covData[3*j] = dx_*dx_;
            covData[3*j+1] = dx_*dy_;
            covData[3*j+2] = dy_*dy_;
        }
    }

    // compute the sum of blocksize window
    boxFilter(cov,cov,cov.depth(),Size(blockSize,blockSize),Point(-1,-1),false);

    if(cov.isContinuous() && dst.isContinuous()){
        size.width *= size.height;
        size.height = 1;
    }
    else
        size = dst.size();

    for(i = 0;i < size.height;i++){
        float *dstData = (float*)(dst.data + i*dst.step);
        const float *covData = (const float*)(cov.data + i*cov.step);

        for(j = 0;j < size.width;j++){
            float a = covData[3*j];
            float b = covData[3*j+1];
            float c = covData[3*j+2];

            dstData[j] = a*c - b*b - k*(a+c)*(a+c);
        }
    }
}

void testCornerHarris(int argc, char* argv[]){
    if(argc != 2){
        PRINT "ERROR : program image\n";
        return;
    }

    if(!csg::isFileExsit(argv[1])){
        PRINT "",argv[1],"does not exsit\n";
        return;
    }

    Mat src = imread(argv[1],CV_LOAD_IMAGE_COLOR);
    if(!src.data){
        PRINT "image data error\n";
        return;
    }

    Mat grayImage;
    cvtColor(src,grayImage,CV_BGR2GRAY);

   //compare the difference of my cornerHarris and opencv cornerHarris
    Mat myHarrisCorner, harrisCorner;
    int blockSize = 3;
    int kSize = 3;
    double k = 0.04;
    cornerHarris(grayImage,harrisCorner,blockSize,kSize,k);
    myCornerHarris(grayImage,myHarrisCorner,blockSize,kSize,k);

    double error = 0.0;
    error = norm(myHarrisCorner,harrisCorner,NORM_L2);

    PRINT "error : ",error;
}