/*
Determines whether a pixel is a scale-space extremum by comparing it to it's 3x3x3 pixel neighborhood.
@param dog_pyr DoG scale space pyramid
@param octv pixel's scale space octave
@param intvl pixel's within-octave interval
@param r pixel's image row
@param c pixel's image col
@return Returns 1 if the specified pixel is an extremum (max or min) among it's 3x3x3 pixel neighborhood.
*/
static int is_extremum( IplImage*** dog_pyr, int octv, int intvl, int r, int c )
{
    //调用函数pixval32f获取图像dog_pyr[octv][intvl]的第r行第c列的点的坐标值
	float val = pixval32f( dog_pyr[octv][intvl], r, c );
	int i, j, k;

    //检查是否最大值
	if( val > 0 )
	{
        for( i = -1; i <= 1; i++ )//层
            for( j = -1; j <= 1; j++ )//行
                for( k = -1; k <= 1; k++ )//列
					if( val < pixval32f( dog_pyr[octv][intvl+i], r + j, c + k ) )
						return 0;
	}
    //检查是否最小值
	else
	{
        for( i = -1; i <= 1; i++ )//层
            for( j = -1; j <= 1; j++ )//行
                for( k = -1; k <= 1; k++ )//列
					if( val > pixval32f( dog_pyr[octv][intvl+i], r + j, c + k ) )
						return 0;
	}

	return 1;
}

/*
Determines whether a pixel is a scale-space extremum by comparing it to it's
3x3x3 pixel neighborhood.

@param dog_pyr DoG scale space pyramid
@param octv pixel's scale space octave
@param intvl pixel's within-octave interval
@param r pixel's image row
@param c pixel's image col

@return Returns 1 if the specified pixel is an extremum (max or min) among
	it's 3x3x3 pixel neighborhood.
*/
int is_extremum( IplImage*** dog_pyr, int octv, int intvl, int r, int c )
{
	float val = pixval32f( dog_pyr[octv][intvl], r, c );
	int i, j, k;

	/* check for maximum */
	if( val > 0 )
	{
		for( i = -1; i <= 1; i++ )
			for( j = -1; j <= 1; j++ )
				for( k = -1; k <= 1; k++ )
					if( val < pixval32f( dog_pyr[octv][intvl+i], r + j, c + k ) )
						return 0;
	}

	/* check for minimum */
	else
	{
		for( i = -1; i <= 1; i++ )
			for( j = -1; j <= 1; j++ )
				for( k = -1; k <= 1; k++ )
					if( val > pixval32f( dog_pyr[octv][intvl+i], r + j, c + k ) )
						return 0;
	}

	return 1;
}

float get_complax(IplImage *img, int r, int c, int w, int h) {
    float sum = 0.0;

    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            if (mask[y * h + x]) {
                sum += pixval32f(img, r - w / 2 + x, c - h / 2 + y);
            }       
        }
    } 

    float avg = sum / num;
    float complaxVal = 0.0;
 
//  printf("%f %d %d\n", avg, r, c);
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            if (mask[y * h + x]) {
                float tmp = pixval32f(img, r - w / 2 + x, c - h / 2 + y) - avg; 
                complaxVal += tmp * tmp;
            }       
        }
    } 

    return complaxVal;
}

/*
Determines whether a feature is too edge like to be stable by computing the
ratio of principal curvatures at that feature.  Based on Section 4.1 of Lowe's paper.
@param dog_img image from the DoG pyramid in which feature was detected
@param r feature row
@param c feature col
@param curv_thr high threshold on ratio of principal curvatures
@return Returns 0 if the feature at (r,c) in dog_img is sufficiently corner-like or 1 otherwise.
*/
static int is_too_edge_like( IplImage* dog_img, int r, int c, int curv_thr )
{
	double d, dxx, dyy, dxy, tr, det;

    /*某点的主曲率与其海森矩阵的特征值成正比，为了避免直接计算特征值，这里只考虑特征值的比值
      可通过计算海森矩阵的迹tr(H)和行列式det(H)来计算特征值的比值
      设a是海森矩阵的较大特征值，b是较小的特征值，有a = r*b，r是大小特征值的比值
      tr(H) = a + b; det(H) = a*b;
      tr(H)^2 / det(H) = (a+b)^2 / ab = (r+1)^2/r
      r越大，越可能是边缘点；伴随r的增大，(r+1)^2/r 的值也增大，所以可通过(r+1)^2/r 判断主曲率比值是否满足条件*/
	/* principal curvatures are computed using the trace and det of Hessian */
    d = pixval32f(dog_img, r, c);//调用函数pixval32f获取图像dog_img的第r行第c列的点的坐标值

    //用差分近似代替偏导，求出海森矩阵的几个元素值
    /*  / dxx  dxy \
        \ dxy  dyy /   */
	dxx = pixval32f( dog_img, r, c+1 ) + pixval32f( dog_img, r, c-1 ) - 2 * d;
	dyy = pixval32f( dog_img, r+1, c ) + pixval32f( dog_img, r-1, c ) - 2 * d;
	dxy = ( pixval32f(dog_img, r+1, c+1) - pixval32f(dog_img, r+1, c-1) -
			pixval32f(dog_img, r-1, c+1) + pixval32f(dog_img, r-1, c-1) ) / 4.0;
    tr = dxx + dyy;//海森矩阵的迹
    det = dxx * dyy - dxy * dxy;//海森矩阵的行列式

    //若行列式为负，表明曲率有不同的符号，去除此点
	/* negative determinant -> curvatures have different signs; reject feature */
	if( det <= 0 )
        return 1;//返回1表明此点是边缘点

    //通过式子：(r+1)^2/r 判断主曲率的比值是否满足条件，若小于阈值，表明不是边缘点
	if( tr * tr / det < ( curv_thr + 1.0 )*( curv_thr + 1.0 ) / curv_thr )
        return 0;//不是边缘点
    return 1;//是边缘点
}

/*
Detects features at extrema in DoG scale space.  Bad features are discarded
based on contrast and ratio of principal curvatures.

@param dog_pyr DoG scale space pyramid
@param octvs octaves of scale space represented by dog_pyr
@param intvls intervals per octave
@param contr_thr low threshold on feature contrast
@param curv_thr high threshold on feature ratio of principal curvatures
@param storage memory storage in which to store detected features

@return Returns an array of detected features whose scales, orientations,
	and descriptors are yet to be determined.
*/
CvSeq* scale_space_extrema( IplImage*** dog_pyr, int octvs, int intvls,
			   double contr_thr, int curv_thr, CvMemStorage* storage ) {
	CvSeq* features;
	double prelim_contr_thr = 0.5 * contr_thr / intvls;
	struct feature* feat;
	struct detection_data* ddata;
	int o, i, r, c;

	features = cvCreateSeq( 0, sizeof(CvSeq), sizeof(struct feature), storage );
	for( o = 0; o < octvs; o++ )
		for( i = 1; i <= intvls; i++ )
			for(r = SIFT_IMG_BORDER; r < dog_pyr[o][0]->height-SIFT_IMG_BORDER; r++)
				for(c = SIFT_IMG_BORDER; c < dog_pyr[o][0]->width-SIFT_IMG_BORDER; c++)
					/* perform preliminary check on contrast */
					if( ABS( pixval32f( dog_pyr[o][i], r, c ) ) > prelim_contr_thr )
						if( is_extremum( dog_pyr, o, i, r, c ) ) {
							feat = interp_extremum(dog_pyr, o, i, r, c, intvls, contr_thr);
							if( feat ) {
								ddata = feat_detection_data( feat );
								if( ! is_too_edge_like( dog_pyr[ddata->octv][ddata->intvl],
									ddata->r, ddata->c, curv_thr ) ) {
									cvSeqPush( features, feat );
								}
								else
									free( ddata );
								free( feat );
							}
						}

	return features;
}

/*
Calculates the gradient magnitude and orientation at a given pixel.

@param img image
@param r pixel row
@param c pixel col
@param mag output as gradient magnitude at pixel (r,c)
@param ori output as gradient orientation at pixel (r,c)

@return Returns 1 if the specified pixel is a valid one and sets mag and
	ori accordingly; otherwise returns 0
*/
int calc_grad_mag_ori( IplImage* img, int r, int c, double* mag, double* ori )
{
	double dx, dy;

	if( r > 0  &&  r < img->height - 1  &&  c > 0  &&  c < img->width - 1 )
	{
		dx = pixval32f( img, r, c+1 ) - pixval32f( img, r, c-1 );
		dy = pixval32f( img, r-1, c ) - pixval32f( img, r+1, c );
		*mag = sqrt( dx*dx + dy*dy );
		*ori = atan2( dy, dx );
		return 1;
	}

	else
		return 0;
}

/*
Calculates the gradient magnitude and orientation at a given pixel.
@param img image
@param r pixel row
@param c pixel col
@param mag output as gradient magnitude at pixel (r,c)
@param ori output as gradient orientation at pixel (r,c)
@return Returns 1 if the specified pixel is a valid one and sets mag and
	ori accordingly; otherwise returns 0
*/
static int calc_grad_mag_ori( IplImage* img, int r, int c, double* mag, double* ori )
{
	double dx, dy;

    //对输入的坐标值进行检查
	if( r > 0  &&  r < img->height - 1  &&  c > 0  &&  c < img->width - 1 )
	{
        //用差分近似代替偏导，来求梯度的幅值和方向
        dx = pixval32f( img, r, c+1 ) - pixval32f( img, r, c-1 );//x方向偏导
        dy = pixval32f( img, r-1, c ) - pixval32f( img, r+1, c );//y方向偏导
        *mag = sqrt( dx*dx + dy*dy );//梯度的幅值，即梯度的模
        *ori = atan2( dy, dx );//梯度的方向
		return 1;
	}
    //行列坐标值不合法，返回0
	else
		return 0;
}

void print_32f_image( const IplImage *src ) {
    for ( int y = 0; y < src->height; y++ ) {
        for ( int x = 0; x < src->width; x++ ) {
            std::cout << pixval32f(src, x, y) << " ";
        }
        std::cout << endl;
    }
    std::cout << "*********************\n";
}

int calc_grad_mag_ori(const IplImage *src, int x, int y, float &mag, float &ori) {
    float dx, dy;

    if (x > 0 && x < src->width - 1 &&
            y > 0 && y < src->height - 1) {
        dy = pixval32f(src, x, y - 1) - pixval32f(src, x, y + 1);
        dx = pixval32f(src, x + 1, y) - pixval32f(src, x - 1, y);

        mag = sqrt(dx * dx + dy * dy);
        ori = atan2(dy, dx);

//      print_point(x, y - 1, pixval32f(src, x, y - 1));
//      print_point(x, y + 1, pixval32f(src, x, y + 1));
//      cout << dy << " " << dx << " " << ori << endl;
//      cout << cvRound(8 * (ori + CV_PI) / (2 * CV_PI)) << endl;
//      cout << "-------------\n";
//      cout << mag << endl;
        return 1;
    }

    return 0;
}

/*
Calculates interpolated pixel contrast.  Based on Eqn. (3) in Lowe's paper.

@param dog_pyr difference of Gaussians scale space pyramid
@param octv octave of scale space
@param intvl within-octave interval
@param r pixel row
@param c pixel column
@param xi interpolated subpixel increment to interval
@param xr interpolated subpixel increment to row
@param xc interpolated subpixel increment to col

@param Returns interpolated contrast.
*/
double interp_contr( IplImage*** dog_pyr, int octv, int intvl, int r,
					int c, double xi, double xr, double xc )
{
	CvMat* dD, X, T;
	double t[1], x[3] = { xc, xr, xi };

	cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
	cvInitMatHeader( &T, 1, 1, CV_64FC1, t, CV_AUTOSTEP );
	dD = deriv_3D( dog_pyr, octv, intvl, r, c );
	cvGEMM( dD, &X, 1, NULL, 0, &T,  CV_GEMM_A_T );
	cvReleaseMat( &dD );

	return pixval32f( dog_pyr[octv][intvl], r, c ) + t[0] * 0.5;
}

/*
Determines whether a feature is too edge like to be stable by computing the
ratio of principal curvatures at that feature.  Based on Section 4.1 of
Lowe's paper.

@param dog_img image from the DoG pyramid in which feature was detected
@param r feature row
@param c feature col
@param curv_thr high threshold on ratio of principal curvatures

@return Returns 0 if the feature at (r,c) in dog_img is sufficiently
	corner-like or 1 otherwise.
*/
int is_too_edge_like( IplImage* dog_img, int r, int c, int curv_thr )
{
	double d, dxx, dyy, dxy, tr, det;

	/* principal curvatures are computed using the trace and det of Hessian */
	d = pixval32f(dog_img, r, c);
	dxx = pixval32f( dog_img, r, c+1 ) + pixval32f( dog_img, r, c-1 ) - 2 * d;
	dyy = pixval32f( dog_img, r+1, c ) + pixval32f( dog_img, r-1, c ) - 2 * d;
	dxy = ( pixval32f(dog_img, r+1, c+1) - pixval32f(dog_img, r+1, c-1) -
			pixval32f(dog_img, r-1, c+1) + pixval32f(dog_img, r-1, c-1) ) / 4.0;
	tr = dxx + dyy;
	det = dxx * dyy - dxy * dxy;

	/* negative determinant -> curvatures have different signs; reject feature */
	if( det <= 0 )
		return 1;

	if( tr * tr / det < ( curv_thr + 1.0 )*( curv_thr + 1.0 ) / curv_thr )
		return 0;
	return 1;
}

/*
Calculates interpolated pixel contrast.  Based on Eqn. (3) in Lowe's paper.

@param dog_pyr difference of Gaussians scale space pyramid
@param octv octave of scale space
@param intvl within-octave interval
@param r pixel row
@param c pixel column
@param xi interpolated subpixel increment to interval
@param xr interpolated subpixel increment to row
@param xc interpolated subpixel increment to col

@param Returns interpolated contrast.
*/
 float SiftGPU::InterpContr( IplImage*** dog_pyr, int octv, int intvl, int r,
							int c, float xi, float xr, float xc )
{
	CvMat* dD, X, T;
	float t[1], x[3] = { xc, xr, xi };

	cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
	cvInitMatHeader( &T, 1, 1, CV_64FC1, t, CV_AUTOSTEP );
	dD = Deriv3D( dog_pyr, octv, intvl, r, c );

	//cvGEMM( dD, &X, 1, NULL, 0, &T,  CV_GEMM_A_T );
	t[0] = cvGetReal2D(dD, 0, 0) * x[0] + cvGetReal2D(dD, 1, 0) * x[1] + cvGetReal2D(dD, 2, 0) * x[2];

	cvReleaseMat( &dD );

	return pixval32f( dog_pyr[octv][intvl], r, c ) + t[0] * 0.5;
}

/*
Detects features at extrema in DoG scale space.  Bad features are discarded
based on contrast and ratio of principal curvatures.
@param dog_pyr DoG scale space pyramid
@param octvs octaves of scale space represented by dog_pyr
@param intvls intervals per octave
@param contr_thr low threshold on feature contrast
@param curv_thr high threshold on feature ratio of principal curvatures
@param storage memory storage in which to store detected features
@return Returns an array of detected features whose scales, orientations,
	and descriptors are yet to be determined.
*/
static CvSeq* scale_space_extrema( IplImage*** dog_pyr, int octvs, int intvls,
								   double contr_thr, int curv_thr, CvMemStorage* storage )
{
    CvSeq* features;//特征点序列
    double prelim_contr_thr = 0.5 * contr_thr / intvls;//像素的对比度阈值
	struct feature* feat;
	struct detection_data* ddata;
	int o, i, r, c;

    //在存储器storage上创建存储极值点的序列，其中存储feature结构类型的数据
	features = cvCreateSeq( 0, sizeof(CvSeq), sizeof(struct feature), storage );

    /*遍历高斯差分金字塔，检测极值点*/
    //SIFT_IMG_BORDER指明边界宽度，只检测边界线以内的极值点
    for( o = 0; o < octvs; o++ )//第o组
        for( i = 1; i <= intvls; i++ )//遍i层
            for(r = SIFT_IMG_BORDER; r < dog_pyr[o][0]->height-SIFT_IMG_BORDER; r++)//第r行
                for(c = SIFT_IMG_BORDER; c < dog_pyr[o][0]->width-SIFT_IMG_BORDER; c++)//第c列
                    //进行初步的对比度检查，只有当归一化后的像素值大于对比度阈值prelim_contr_thr时才继续检测此像素点是否可能是极值
                    //调用函数pixval32f获取图像dog_pyr[o][i]的第r行第c列的点的坐标值，然后调用ABS宏求其绝对值
					if( ABS( pixval32f( dog_pyr[o][i], r, c ) ) > prelim_contr_thr )
                        //通过在尺度空间中将一个像素点的值与其周围3*3*3邻域内的点比较来决定此点是否极值点(极大值或极小都行)
                        if( is_extremum( dog_pyr, o, i, r, c ) )//若是极值点
						{
                            //由于极值点的检测是在离散空间中进行的，所以检测到的极值点并不一定是真正意义上的极值点
                            //因为真正的极值点可能位于两个像素之间，而在离散空间中只能精确到坐标点精度上
                            //通过亚像素级插值进行极值点精确定位(修正极值点坐标)，并去除低对比度的极值点，将修正后的特征点组成feature结构返回
							feat = interp_extremum(dog_pyr, o, i, r, c, intvls, contr_thr);
                            //返回值非空，表明此点已被成功修正
                            if( feat )
							{
                                //调用宏feat_detection_data来提取参数feat中的feature_data成员并转换为detection_data类型的指针
								ddata = feat_detection_data( feat );
                                //去除边缘响应，即通过计算主曲率比值判断某点是否边缘点,返回值为0表示不是边缘点，可做特征点
                                if( ! is_too_edge_like( dog_pyr[ddata->octv][ddata->intvl], ddata->r, ddata->c, curv_thr ) )
								{
                                    cvSeqPush( features, feat );//向特征点序列features末尾插入新检测到的特征点feat
								}
								else
									free( ddata );
								free( feat );
							}
						}

    return features;//返回特征点序列
}

float *get_image_dct( const IplImage *src, const int nSub, int &featLen ) {
    IplImage *img = get_gray( src ); 
    IplImage *subImage = resize_image( img, nSub ); 
    IplImage *dctImage = cvCloneImage( subImage );

    cvDCT( subImage, dctImage, CV_DXT_FORWARD );
//  print_32f_image( dctImage );

//  float *data = ( float * )( dctImage->imageData );
//  qsort( data + 1, nSub * nSub - 1, sizeof( float ), cmp );
//  print_32f_image( dctImage );
    
    int subDct = 6;
    featLen = subDct * subDct - 1;
    int index = 0;
    float *feature = new float[ featLen ];
    /*copy only 35 points of dcts*/
    for ( int y = 0; y < subDct; y++ ) {
        for ( int x = 0; x < subDct; x++ ) {
            if ( x == 0 && y == 0 ) 
                continue;
            feature[ index++ ] = pixval32f( dctImage, x, y );
        }
    }

    /*print the feature without sort*/
//  print_dct_feature( feature, subDct );

    qsort( feature, featLen, sizeof( float ), cmp );
    //sort( feature, feature + featLen );
    /*print the feature after sort*/
    //print_dct_feature( feature, subDct );
    for ( int i = 0; i < featLen; i++ ) {
        printf( "%.3f\t", feature[ i ] );
    }
    std::cout << endl;

    show_image( img, "gray" );

    cvReleaseImage( &img );
    cvReleaseImage( &subImage );
    cvReleaseImage( &dctImage );

    return feature;
}

/*
Calculates interpolated pixel contrast.  Based on Eqn. (3) in Lowe's paper.
@param dog_pyr difference of Gaussians scale space pyramid
@param octv octave of scale space
@param intvl within-octave interval
@param r pixel row
@param c pixel column
@param xi interpolated subpixel increment to interval
@param xr interpolated subpixel increment to row
@param xc interpolated subpixel increment to col
@param Returns interpolated contrast.
*/
static double interp_contr( IplImage*** dog_pyr, int octv, int intvl, int r,
							int c, double xi, double xr, double xc )
{
	CvMat* dD, X, T;
	double t[1], x[3] = { xc, xr, xi };

    //偏移量组成的列向量X，其中是x，y，σ三方向上的偏移量
	cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
    //矩阵乘法的结果T，是一个数值
	cvInitMatHeader( &T, 1, 1, CV_64FC1, t, CV_AUTOSTEP );
    //在DoG金字塔中计算某点的x方向、y方向以及尺度方向上的偏导数，结果存放在列向量dD中
	dD = deriv_3D( dog_pyr, octv, intvl, r, c );
    //矩阵乘法：T = dD^T * X
	cvGEMM( dD, &X, 1, NULL, 0, &T,  CV_GEMM_A_T );
	cvReleaseMat( &dD );

    //返回计算出的对比度值：D + 0.5 * dD^T * X (具体公式推导见SIFT算法说明)
	return pixval32f( dog_pyr[octv][intvl], r, c ) + t[0] * 0.5;
}

/*
Computes the partial derivatives in x, y, and scale of a pixel in the DoG
scale space pyramid.

@param dog_pyr DoG scale space pyramid
@param octv pixel's octave in dog_pyr
@param intvl pixel's interval in octv
@param r pixel's image row
@param c pixel's image col

@return Returns the vector of partial derivatives for pixel I
	{ dI/dx, dI/dy, dI/ds }^T as a CvMat*
*/
CvMat* deriv_3D( IplImage*** dog_pyr, int octv, int intvl, int r, int c )
{
	CvMat* dI;
	double dx, dy, ds;

	dx = ( pixval32f( dog_pyr[octv][intvl], r, c+1 ) -
		pixval32f( dog_pyr[octv][intvl], r, c-1 ) ) / 2.0;
	dy = ( pixval32f( dog_pyr[octv][intvl], r+1, c ) -
		pixval32f( dog_pyr[octv][intvl], r-1, c ) ) / 2.0;
	ds = ( pixval32f( dog_pyr[octv][intvl+1], r, c ) -
		pixval32f( dog_pyr[octv][intvl-1], r, c ) ) / 2.0;

	dI = cvCreateMat( 3, 1, CV_64FC1 );
	cvmSet( dI, 0, 0, dx );
	cvmSet( dI, 1, 0, dy );
	cvmSet( dI, 2, 0, ds );

	return dI;
}

/*
Computes the partial derivatives in x, y, and scale of a pixel in the DoG scale space pyramid.
@param dog_pyr DoG scale space pyramid
@param octv pixel's octave in dog_pyr
@param intvl pixel's interval in octv
@param r pixel's image row
@param c pixel's image col
@return Returns the vector of partial derivatives for pixel I
	{ dI/dx, dI/dy, dI/ds }^T as a CvMat*
*/
static CvMat* deriv_3D( IplImage*** dog_pyr, int octv, int intvl, int r, int c )
{
	CvMat* dI;
	double dx, dy, ds;

    //求差分来代替偏导，这里是用的隔行求差取中值的梯度计算方法
    //求x方向上的差分来近似代替偏导数
	dx = ( pixval32f( dog_pyr[octv][intvl], r, c+1 ) -
		pixval32f( dog_pyr[octv][intvl], r, c-1 ) ) / 2.0;
    //求y方向上的差分来近似代替偏导数
	dy = ( pixval32f( dog_pyr[octv][intvl], r+1, c ) -
		pixval32f( dog_pyr[octv][intvl], r-1, c ) ) / 2.0;
    //求层间的差分来近似代替尺度方向上的偏导数
	ds = ( pixval32f( dog_pyr[octv][intvl+1], r, c ) -
		pixval32f( dog_pyr[octv][intvl-1], r, c ) ) / 2.0;

    //组成列向量
	dI = cvCreateMat( 3, 1, CV_64FC1 );
	cvmSet( dI, 0, 0, dx );
	cvmSet( dI, 1, 0, dy );
	cvmSet( dI, 2, 0, ds );

	return dI;
}

int _harris(IplImage *src, float threshold, float *_cims) {
    IplImage *deriX = derivateX(src);
    IplImage *deriY = derivateY(src);
    IplImage *deriXY = cvCloneImage(deriX);
    //
    cvMul(deriX, deriY, deriXY);
    cvMul(deriX, deriX, deriX);
    cvMul(deriY, deriY, deriY);

    cvSmooth(deriX, deriX, CV_GAUSSIAN, 5);
    cvSmooth(deriY, deriY, CV_GAUSSIAN, 5);
    cvSmooth(deriXY, deriXY, CV_GAUSSIAN, 5);

    int w = src->width;
    int h = src->height;
    float *cims = _cims;
    float k = 0.06;
    float *vals = new float[w * h];
    memset(vals, 0, w * h);

    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            float Ix = pixval32f(deriX, x, y);
            float Iy = pixval32f(deriY, x, y);
            float Ixy = pixval32f(deriXY, x, y);

            float det = Ix * Iy - Ixy * Ixy;
            float tr = Ix + Iy;

            float cim = det - k * tr * tr;
//          if (cim > threshold) {
//              cims[y * w + x] = cim;
//          } else
//              cims[y * w + x] = 0.0;
            cims[y * w + x] = cim;
            vals[y * w + x] = cim;
        }
    }

    sort(vals, vals + w * h);
    int num =  w * h > 500 ? 500 : w * h * 3 / 4;
//  float thres = vals[w * h - 500];
    float thres = 5000;

    for (int y = filterSize; y < h - filterSize; y++) {
        for (int x = filterSize; x < w- filterSize; x++) {
//          if (cims[y * w + x] >= thres && is_extremun(cims, x, y, w, h, 10)) {
//          }
//          if (cims[y * w + x] < thres) {
//              cims[y * w + x] = 0;
//          }
        }
    }

    delete [] vals;
    cvReleaseImage(&deriX);
    cvReleaseImage(&deriY);
    cvReleaseImage(&deriXY);

    return 0;
}

/*
Computes the 3D Hessian matrix for a pixel in the DoG scale space pyramid.
@param dog_pyr DoG scale space pyramid
@param octv pixel's octave in dog_pyr
@param intvl pixel's interval in octv
@param r pixel's image row
@param c pixel's image col
@return Returns the Hessian matrix (below) for pixel I as a CvMat*

    / Ixx  Ixy  Ixs \
    | Ixy  Iyy  Iys |
	\ Ixs  Iys  Iss /
*/
static CvMat* hessian_3D( IplImage*** dog_pyr, int octv, int intvl, int r, int c )
{
	CvMat* H;
	double v, dxx, dyy, dss, dxy, dxs, dys;

    v = pixval32f( dog_pyr[octv][intvl], r, c );//该点的像素值

    //用差分近似代替倒数(具体公式见各种梯度的求法)
    //dxx = f(i+1,j) - 2f(i,j) + f(i-1,j)
    //dyy = f(i,j+1) - 2f(i,j) + f(i,j-1)
	dxx = ( pixval32f( dog_pyr[octv][intvl], r, c+1 ) + 
			pixval32f( dog_pyr[octv][intvl], r, c-1 ) - 2 * v );
	dyy = ( pixval32f( dog_pyr[octv][intvl], r+1, c ) +
			pixval32f( dog_pyr[octv][intvl], r-1, c ) - 2 * v );
	dss = ( pixval32f( dog_pyr[octv][intvl+1], r, c ) +
			pixval32f( dog_pyr[octv][intvl-1], r, c ) - 2 * v );
	dxy = ( pixval32f( dog_pyr[octv][intvl], r+1, c+1 ) -
			pixval32f( dog_pyr[octv][intvl], r+1, c-1 ) -
			pixval32f( dog_pyr[octv][intvl], r-1, c+1 ) +
			pixval32f( dog_pyr[octv][intvl], r-1, c-1 ) ) / 4.0;
	dxs = ( pixval32f( dog_pyr[octv][intvl+1], r, c+1 ) -
			pixval32f( dog_pyr[octv][intvl+1], r, c-1 ) -
			pixval32f( dog_pyr[octv][intvl-1], r, c+1 ) +
			pixval32f( dog_pyr[octv][intvl-1], r, c-1 ) ) / 4.0;
	dys = ( pixval32f( dog_pyr[octv][intvl+1], r+1, c ) -
			pixval32f( dog_pyr[octv][intvl+1], r-1, c ) -
			pixval32f( dog_pyr[octv][intvl-1], r+1, c ) +
			pixval32f( dog_pyr[octv][intvl-1], r-1, c ) ) / 4.0;

    //组成海森矩阵
	H = cvCreateMat( 3, 3, CV_64FC1 );
	cvmSet( H, 0, 0, dxx );
	cvmSet( H, 0, 1, dxy );
	cvmSet( H, 0, 2, dxs );
	cvmSet( H, 1, 0, dxy );
	cvmSet( H, 1, 1, dyy );
	cvmSet( H, 1, 2, dys );
	cvmSet( H, 2, 0, dxs );
	cvmSet( H, 2, 1, dys );
	cvmSet( H, 2, 2, dss );

	return H;
}

IplImage *harris_center(IplImage *img, float threshold) {
    IplImage *src = get_gray(img);
    IplImage *deriX = derivateX(src);
    IplImage *deriY = derivateY(src);
    IplImage *deriXY = cvCloneImage(src);

    cvMul(deriX, deriY, deriXY);
    cvMul(deriX, deriX, deriX);
    cvMul(deriY, deriY, deriY);

    cvSmooth(deriX, deriX, CV_GAUSSIAN, 5);
    cvSmooth(deriY, deriY, CV_GAUSSIAN, 5);
    cvSmooth(deriXY, deriXY, CV_GAUSSIAN, 5);

    int w = src->width;
    int h = src->height;

    int centerX = w / 2;
    int centerY = h / 2;
    int ctrSize = w / 3;

    float *cims = new float[(ctrSize + 1) * (ctrSize + 1)];
    float *vals = new float[(ctrSize + 1) * (ctrSize + 1)];
    memset(vals, 0, sizeof(vals));

    int sx = centerX - ctrSize / 2;
    int sy = centerY - ctrSize / 2;
    float k = 0.06;
    for (int i = 0; i < ctrSize; i++) {
        for (int j = 0; j < ctrSize; j++) {
            int x = sx + j;
            int y = sy + i;

            float Ix = pixval32f(deriX, x, y);
            float Iy = pixval32f(deriY, x, y);
            float Ixy = pixval32f(deriXY, x, y);

            float det = Ix * Iy - Ixy * Ixy;
            float tr = Ix + Iy;
            float cim = det - k * tr * tr;

            cims[i * ctrSize + j] = cim;
            vals[i * ctrSize + j] = cim;
        }
    }

    sort(vals, vals + ctrSize * ctrSize);
    float thres = vals[ctrSize * ctrSize - 10];

    IplImage *printImg = cvCloneImage(img);
    for (int i = 0; i < ctrSize; i++) {
        for (int j = 0; j < ctrSize; j++) {
            if (cims[i * ctrSize + j] >= thres) {
                drawPoint(printImg, sx + j, sy + i);
            }
        }
    }

    show_image(printImg, "center");

    cvReleaseImage(&src);
    cvReleaseImage(&deriX);
    cvReleaseImage(&deriY);
    cvReleaseImage(&deriXY);

    return printImg;
}