sift算法2

参考：http://blog.csdn.net/v_july_v/article/details/6246213

http://blog.csdn.net/pi9nc/article/details/23302075

http://blog.csdn.net/manji_lee/article/details/8922509

http://blog.csdn.net/masibuaa/article/details/9204297

http://blog.csdn.net/xiaowei_cqu/article/details/8069548
http://blog.csdn.net/xiaowei_cqu/article/details/8087239
http://blog.csdn.net/xiaowei_cqu/article/details/8096072
http://blog.csdn.net/xiaowei_cqu/article/details/8113565
http://blog.csdn.net/pi9nc/article/details/23302075
http://blog.csdn.net/v_july_v/article/details/6246213

http://wenku.baidu.com/link?url=Dg4V22OCHFAnMmoHp6-v3Du6zq9TDWKwHMR9S51gMKKEi8sZ5H4YxAXANHOXHRY2mT3SNG6oCcDRFOV7Iw7lhyXMuk7fuRmG3L4fj0-WJNe

http://www.cnblogs.com/cfantaisie/archive/2011/06/14/2080917.html

#include "stdafx.h"

#include<iostream>
#include<Windows.h>
#include"highgui.h"
#include"cv.h"
#include"math.h" 
#include<vector>
#include<string>
#include<cor.h>
//#include<precomp.hpp>
#include<stdarg.h>
//#include<core.h>
using namespace std;
//使用floor函数。floor(x)返回的是小于或等于x的最大整数。
#define pi 3.1415
#define S   3 //高斯差分金字塔的组内层数
//#define dem_gauss 3 
//高斯模板的维数
//高斯模板
#define SIGMA 1.6
#define SIFT_ORT_SIG_FCTR 1.5
#define SIFT_ORT_RADIUS 3*SIFT_ORT_SIG_FCTR
static const int SIFT_FIXPT_SCALE = 1;
static const int edgeThreshold = 10;
vector<CvMat*> Image_Hash;   //高斯金字塔
vector <CvMat*> Sub_gausshash;//高斯差分金字塔
struct node
{
int x;
int y;
int order;
int oct;
int otc_num;
int size;
double angle;
double dist[128];
};
typedef struct node Pnode;
typedef Pnode *ppnode;
vector<Pnode*> mmpnodelist;
map<int, vector<Pnode*>*> map_list;
typedef struct point
{
double  x;
double   y;
double  octave;
double  size;
}Kpoint ;
double** gauss_model(double sigma)


{
//int dem_gauss = ceil(6 * sigma + 1);//使用ceil函数。ceil(x)返回的是大于x的最小整数。
int dem_gauss = int(6 * sigma + 1);//使用ceil函数。ceil(x)返回的是大于x的最小整数。
if (dem_gauss % 2 == 0)
{
dem_gauss++;
}
int mid = dem_gauss / 2;
double guiyi_total=0;
double** template_su = new double*[dem_gauss];
for (int i = 0; i < dem_gauss; i++)
{
template_su[i] = new double[dem_gauss];
}
for (int m = 0; m < dem_gauss; m++)
{
for (int n = 0; n < dem_gauss; n++)
{
template_su[m][n] = 1 / (2 * pi*sigma*sigma);
template_su[m][n] = template_su[m][n] * exp((-pow((mid - m), 2) - pow((mid - n), 2)) / (2 * pow(sigma, 2)));
guiyi_total = guiyi_total + template_su[m][n];
}
}
//cout<<endl;
//归一化
for (int i = 0; i < dem_gauss; i++)
{
for (int j = 0; j < dem_gauss; j++)
{
template_su[i][j] = template_su[i][j] / guiyi_total;
}
}
//for (int k = 0; k < dem_gauss; k++)
//{ 
// for (int v = 0; v < dem_gauss; v++)
// {
// cout<<'\t' << &template_su[k][v] <<",";
// }
// cout << endl;
//}
return template_su;
}
//图像的灰度化
CvMat* grey_img(IplImage* image)
{
IplImage  *GrayImage;
CvMat* pGrayMat = NULL;
GrayImage = cvCreateImage(cvGetSize(image), 8, 1);
pGrayMat = cvCreateMat(image->height, image->width, CV_64FC1);
BYTE data_b;   //BYTE类型的头文件为windows.h，相当于unsigned char
BYTE data_g;
BYTE data_r;
BYTE data_gray;
for (int j = 0; j<image->height; j++)
{
for (int i = 0; i<image->width; i++)
{
data_b = (BYTE)image->imageData[j*image->widthStep + i * 3];     //B分量  
data_g = (BYTE)image->imageData[j*image->widthStep + i * 3 + 1]; //G分量  
data_r = (BYTE)image->imageData[j*image->widthStep + i * 3 + 2]; //R分量
//Gray= 0.072169B+ 0.715160G+ 0.212671R
data_gray = (BYTE)(0.072169*data_b + 0.715160*data_g + 0.212671*data_r);
cvmSet(pGrayMat, j, i, data_gray);
}
}
cvConvert(pGrayMat, GrayImage);
cvNamedWindow("Image", CV_WINDOW_AUTOSIZE);
cvShowImage("Image", GrayImage);
cvWaitKey(0);
//cvWaitKey(0);
//cvReleaseImage(&GrayImage);
//cvDestroyWindow("Image_1");
return pGrayMat;
}
 //图像高斯化
CvMat* gauss_image(CvMat *Gray_array, double sigma_float)
{
IplImage  *GrayImage;
CvMat* pGrayMat = NULL;
GrayImage = cvCreateImage(cvGetSize(Gray_array), 8, 1);
pGrayMat = cvCreateMat(Gray_array->height, Gray_array->width, CV_64FC1);


int dem_gauss = int(6 * sigma_float+1);//使用ceil函数。ceil(x)返回的是大于x的最小整数。
if (dem_gauss%2==0)
{
dem_gauss++;
}
int mid_num = dem_gauss / 2;
double** template_gauss = NULL;
//double** template_gauss = new double*[dem_gauss];
//for (int i = 0; i < dem_gauss; i++)
//{
// template_gauss[i] = new double[dem_gauss];
//}/


template_gauss=gauss_model(sigma_float);
//cout << template_gauss[0][0] << endl;
//cout << &template_gauss;
// for (int k = 0; k < dem_gauss; k++)
// {
// for (int v = 0; v < dem_gauss; v++)
// {
// cout << '\t' << template_gauss[k][v] << ",";
// }
// cout << endl;
// }
int gray_height = Gray_array->height;
int gray_width = Gray_array->width;
//double filter_num = 0.0;
//double filter_sum = 0.0;
for (int i = 0; i < gray_height; i++)
{
for (int j = 0; j < gray_width; j++)
{
double dFilter = 0.0;
double dSum = 0.0;
int a1, a2;
for (int x = -mid_num; x <= mid_num; x++)
{
for (int y = -mid_num; y <= mid_num; y++)
{
if ((j + x) >= 0 && (j + x)<gray_width && (i + y) >= 0 && (i + y)<gray_height)  //判断边缘  
{
double Imgaedata = cvmGet(Gray_array, i + y, j + x);//访问矩阵元素:
//cout << Imgaedata << endl;
///int y1 = (y + mid_num)*dem_gauss;
//int x1=x + mid_num;
//cout << template_gauss[y1][x1];
//cout << Imgaedata << endl;           dem_gauss
//a1=*(*(template_gauss+(y + mid_num))+(x + mid_num));
//a2 = Imgaedata;
//dFilter += a1*a2;
dFilter += Imgaedata *template_gauss[(y + mid_num) ][x + mid_num];
dSum += template_gauss[(y + mid_num)][x + mid_num];
}
}
}
cvmSet(pGrayMat, i, j, dFilter / dSum);
}

}
//cvConvert(pGrayMat, GrayImage);
//cvNamedWindow("Image_2", CV_WINDOW_AUTOSIZE);
//cvShowImage("Image_2", GrayImage);
//cvWaitKey(0);
//cvReleaseImage(&GrayImage);
//cvDestroyWindow("Image_2");
//释放内存
for (int b = 0; b < dem_gauss; b++)
{
delete[] template_gauss[b];
}
delete[] template_gauss;
return pGrayMat;
}
//构建高斯金字塔
CvMat* convert(CvMat*  Gray_array)
{
CvMat* pGrayMat = NULL;
pGrayMat = cvCreateMat(Gray_array->height, Gray_array->width, CV_64FC1);
int gray_height = Gray_array->height;
int gray_width = Gray_array->width;

for (int i = 0; i < gray_height; i++)
{
for (int j = 0; j < gray_width; j++)
{
int  v = cvmGet(Gray_array, i, j);
cvmSet(pGrayMat, i, j, v);
}
}


return pGrayMat;
}
//图像降采样函数
CvMat* downsample(CvMat* pMat)
{
int dowidth = (int)(pMat->width/2);
int doheight = (int)(pMat->height/2);
  CvMat* cpMat = NULL;
cpMat = cvCreateMat(doheight, dowidth, CV_64FC1);
for (int i = 0; i < doheight; i++)
{
for (int j = 0; j < dowidth; j++)
{
int  y = 2 * i;
int x = 2 * j;
cvmSet(cpMat, i, j, cvmGet(pMat, y, x));
}
}
return cpMat;
}
void Hashgausss(CvMat*  Gray_array, int minSize, double sigma_float)
{

int nheight = Gray_array->height;
int nwidth = Gray_array->width;
int nsmall = nheight < nwidth ? nheight : nwidth;
int octave = log(nsmall) / log(2) - log(minSize) / log(2);
//cout << endl;
cout << octave<<endl;
double* sigma_array = new double[S + 3];
sigma_array[0] = sigma_float;
//int v = pow(2.0, 1.0 / S);
for (int k = 1; k < S + 3; k++)
{
double sigma_pre = pow(2,(double)(k - 1)*1.0/S)*sigma_float;
double sigma_tai = sigma_pre*pow(2,(double)(1.0/S));
sigma_array[k] = (double)sqrt((double)abs(sigma_pre *sigma_pre - sigma_tai*sigma_tai));
cout << sigma_array[k] << endl;
}
CvMat* pMat = NULL;
pMat = convert(Gray_array);
for (int m = 0; m < octave; m++)//组数，金字塔层数
{
for (int n = 0; n < S + 3; n++)
{
pMat = gauss_image(pMat, sigma_array[n]);
Image_Hash.push_back(pMat);
}
vector<CvMat*>::iterator iter = Image_Hash.begin();
pMat = *(iter+(Image_Hash.size() - 3));
pMat=convert(pMat);
//降采样
pMat = downsample(pMat);
}
//vector<CvMat*>::iterator ster = Image_Hash.begin();
/*
char um = 0;
for (; ster != Image_Hash.end(); ster++)
{

char str[] = "Image_";
char srr[1];
itoa(um, srr, 10);               //整数变成字符串
strcat(str, srr);
um++;
//CvMat* bb = *ster;
//CvSize size = cvSize(bb->width, bb->height);
IplImage  *GrayImage=NULL;
GrayImage = cvCreateImage(cvGetSize(*ster), 8, 1);
cvConvert(*ster, GrayImage);
cvNamedWindow(str, CV_WINDOW_AUTOSIZE);

cvShowImage(str, GrayImage);
cvWaitKey(0);
}
*/

}
//高斯差分近似高斯拉普拉斯函数，来获取非常稳定的极大极小值 相关理论证明看论文
void Subhashgauss()
{
vector<CvMat*>::iterator ster = Image_Hash.begin();
int num = 0;
int bt = 0;
for (; ster != Image_Hash.end();ster++)
{
bt = num % (S + 3);
if (num %(S + 3)  == 0|| bt < S + 2 )
{
CvMat* ImageMat_down = NULL;
CvMat* ImageMat_up = NULL;
CvMat* ImageMat_t = NULL;
ImageMat_down = *ster;
ImageMat_up = *(ster + 1);
ImageMat_t = cvCreateMat(ImageMat_down->height, ImageMat_down->width, CV_64FC1);
   for (int su_height = 0; su_height < ImageMat_down->height; su_height++)
{
for (int su_width = 0; su_width < ImageMat_down->width; su_width++)
  {
int dig_down = cvmGet(ImageMat_down, su_height, su_width);
int dig_up = cvmGet(ImageMat_up, su_height, su_width);
cvmSet(ImageMat_t, su_height, su_width, dig_up - dig_down);
}
}
Sub_gausshash.push_back(ImageMat_t);
}
num++;
}
//代码检测
//vector<CvMat*>::iterator iter = Sub_gausshash.begin();
//for (; iter != Sub_gausshash.end(); iter++)
//{
// cout << *(iter) << endl;
//}


}
void paintimage(CvMat* m_Mat, int y, int x)
{


}
//关键点的定位   可以用来精确求解角点位置（二阶泰勒级数拟合），并过滤掉边缘点。下面阐述该函数的原理。
//map<int, vector<Pnode*>*> mao_DOG;
bool adjustLocalExtrema(ppnode& kpoint, int oct, int& id, int& y, int& x)
{
//cout << id << endl;


double contrastThreshold = 0.4;//默认的过滤掉弱特征的阈值
double img_scale = 1.f / (255 * SIFT_FIXPT_SCALE);//
double deriv_scale = img_scale*0.5f;
double second_deriv_scale = img_scale;
double cross_deriv_scale = img_scale*0.25f;
double xi = 0, xr = 0, xc = 0, contr = 0;
int i = 0;
int id_id = oct*(S + 2) + id;
vector<CvMat*>::iterator iterr = Sub_gausshash.begin();
for (; i < 5; i++)
{
CvMat* prev = *(id_id + iterr - 1);
CvMat* next = *(id_id + iterr + 1);
CvMat* cons = *(id_id + iterr);
double dx = (cvmGet(cons, y, x + 1) - cvmGet(cons, y, x - 1))*deriv_scale;
double dy = (cvmGet(cons, y + 1, x) - cvmGet(cons, y - 1, x))*deriv_scale;
double dg = (cvmGet(next, y, x) - cvmGet(prev, y, x))*deriv_scale;
double vv = (double)cvmGet(cons, y, x)*2.0;
double dxx = (cvmGet(cons, y, x + 1) + cvmGet(cons, y, x - 1) - vv)*second_deriv_scale;
double dyy = (cvmGet(cons, y + 1, x) + cvmGet(cons, y - 1, x) - vv)*second_deriv_scale;
double dgg = (cvmGet(next, y, x) + cvmGet(prev, y, x) - vv)*second_deriv_scale;
double dxy = (cvmGet(cons, y + 1, x + 1) + cvmGet(cons, y - 1, x - 1) - cvmGet(cons, y - 1, x + 1) - cvmGet(cons, y + 1, x - 1))*cross_deriv_scale;
double dxg = (cvmGet(next, y, x + 1) - cvmGet(next, y, x - 1) - cvmGet(prev, y, x + 1) + cvmGet(prev, y, x - 1))*cross_deriv_scale;
double dyg = (cvmGet(next, y + 1, x) - cvmGet(next, y - 1, x) - cvmGet(prev, y + 1, x) + cvmGet(prev, y - 1, x))*cross_deriv_scale;
//cvReleaseMat(&prev);
//cvReleaseMat(&next);
//cvReleaseMat(&cons);


CvMat* dI = cvCreateMat(3, 1, CV_64FC1);
cvmSet(dI, 0, 0, dx);
cvmSet(dI, 1, 0, dy);
cvmSet(dI, 2, 0, dg);
CvMat* H;
CvMat X;
double  xx[3] = { 0 };
H = cvCreateMat(3, 3, CV_64FC1);
cvmSet(H, 0, 0, dxx);
cvmSet(H, 0, 1, dxy);
cvmSet(H, 0, 2, dxg);
cvmSet(H, 1, 0, dxy);
cvmSet(H, 1, 1, dyy);
cvmSet(H, 1, 2, dyg);
cvmSet(H, 2, 0, dxg);
cvmSet(H, 2, 1, dyg);
cvmSet(H, 2, 2, dgg);
CvMat* H_inv = cvCreateMat(3, 3, CV_64FC1);
cvInvert(H, H_inv, CV_LU);
cvInitMatHeader(&X, 3, 1, CV_64FC1, xx, CV_AUTOSTEP);
cvGEMM(H_inv, dI, -1, NULL, 0, &X, 0);
cvReleaseMat(&dI);
cvReleaseMat(&H);
cvReleaseMat(&H_inv);
xi = xx[2];
xr = xx[1];
xc = xx[0];
//abs(xi) < 0.5;
//abs(xr) < 0.5;
if (abs(xi) < 0.5&&abs(xr) < 0.5&&abs(xc) < 0.5)
break;
id += cvRound(xi);
x += cvRound(xr);
y += cvRound(xc);
if (id<1 || id>S || x < 1 || x >= cons->width - 1 || y < 1 || y >= cons->height-1)
return false;
}
if (i >= 5) //插值最大步数，避免插值不收敛，程序中默认为5
return false;


//消除边缘响应
//vector<CvMat*>::iterator iterr = Sub_gausshash.begin();
int id_id_id = oct*(S + 2) + id;
CvMat* img = *(id_id_id + iterr);
CvMat* img_pre = *(id_id_id + iterr-1);
CvMat* img_nex = *(id_id_id + iterr+1);
//cout << cvmGet(img, y, x + 1);
//cout << cvmGet(img, y, x - 1);
double dx_e = (cvmGet(img, y, x + 1) - cvmGet(img, y, x - 1))*deriv_scale;
double dy_e = (cvmGet(img, y + 1, x) - cvmGet(img, y - 1, x))*deriv_scale;
double dg_e = (cvmGet(img_nex, y, x) - cvmGet(img_pre, y, x))*deriv_scale;
//cvReleaseMat(&img);
//cvReleaseMat(&img_pre);
//cvReleaseMat(&img_nex);
CvMat* dI_e = cvCreateMat(3, 1, CV_64FC1);
cvmSet(dI_e, 0, 0, dx_e);
cvmSet(dI_e, 1, 0, dy_e);
cvmSet(dI_e, 2, 0, dg_e);
//点乘
double t = dx_e*xc + dy_e*xr + dg_e*xi;
double contrr = cvmGet(img, y, x)*img_scale + t*0.5;
if (abs(contrr)*S < 0.01)  //?????????????????????????????????
return false;
double v2 = cvmGet(img, y, x)*2.0;
double dxx = (cvmGet(img, y, x + 1) + cvmGet(img, y, x - 1) - v2)*second_deriv_scale;
double dyy = (cvmGet(img, y + 1, x) + cvmGet(img, y - 1, x) - v2)*second_deriv_scale;
double dxy = (cvmGet(img, y + 1, x + 1) + cvmGet(img, y - 1, x - 1) - cvmGet(img, y - 1, x + 1) - cvmGet(img, y + 1, x - 1))*cross_deriv_scale;
double tr = dxx + dyy;
double det = dxx*dxx - dyy*dyy;
if (det <= 0 || tr*tr*edgeThreshold >= (edgeThreshold + 1)*(edgeThreshold + 1)*det)
return false;

kpoint->x = (x + xc)*(1 << oct);


kpoint->y = (y + xr)*(1 << oct);
//kpoint.oct = oct + (id << 8) + (cvRound((xi + 0.5) * 255) << 16);
kpoint->oct = oct;
kpoint->otc_num = id;
kpoint->size = SIGMA*pow(2.0,(id + xi) / S)*(1 << oct) * 2;

cout <<"第"<< kpoint->oct<<"组" << endl;
cout << "第" << kpoint->otc_num << "层" << "(" << kpoint->x <<","<< kpoint->y<<")"<< endl;
   //cout <<"阈值"<<kpoint->size << endl;
//cout << endl;
//cvReleaseMat(&img);
cvReleaseMat(&dI_e);
return true;
}


static const float atan2_p1 = 0.9997878412794807f*(float)(180/CV_PI);
static const float atan2_p3 = -0.3258083974640975f*(float)(180/CV_PI);
static const float atan2_p5 = 0.1555786518463281f*(float)(180/CV_PI);
static const float atan2_p7 = -0.04432655554792128f*(float)(180/CV_PI);


double  fastAtan2( double  y, double  x )
{
  double  ax = std::abs(x), ay = std::abs(y);//首先不分象限，求得一个锐角角度
  double  a, c, c2;
  if( ax >= ay )
 {
   c = ay/(ax + (double )DBL_EPSILON);
   c2 = c*c;
   a = (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
  }
   else
 {
   c = ax/(ay + (double)DBL_EPSILON);
   c2 = c*c;
   a = 90.f - (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
  }
    if( x < 0 )//锐角求出后，根据x和y的正负性确定向量的方向，即角度。
    a = 180.f - a;
    if( y < 0 )
   a = 360.f - a;
   return a;
}


double  calcOrientationHist(CvMat* h_p,int x,int y,int radius,double sigma_r,double* histt)
{


int i, j, k, len = pow((radius * 2 + 1), 2);
//cout <<"长度"<< len << endl;
double xepf_scale = -1.0 / (2.0*sigma_r*sigma_r);
double*  buf = new double [len * 4 + 36 + 4];
double *X = buf, *Y = X + len, *Mag = X, *Ori = Y + len, *W = Ori + len;
double* temphist = W + len + 2;
for (i = 0; i < 36; i++)
{
temphist[i] = 0.0;
}
for (i = -radius, k = 0; i <= radius; i++)
{
int y0 = y + i;
if (y0 <= 0 || y0 >= h_p->height - 1)
{
continue;
}
for (j = -radius; j <= radius; j++)
{
int x0 = x + j;
if (x0 <= 0 || x0 >= h_p->width - 1)
{
continue;
}
//cout <<"像素"<< cvmGet(h_p, y, x + 1) << endl;
//cout << "像素" << cvmGet(h_p, y, x - 1) << endl;
double dx = (double)(cvmGet(h_p, y, x + 1) - cvmGet(h_p, y, x - 1));
double dy = (double)(cvmGet(h_p, y + 1, x) - cvmGet(h_p, y - 1, x));
X[k] = dx; Y[k] = dy;
W[k] = (i*i + j*j)*xepf_scale;
k++;
}
}
len = k;
for (int v = 0; v < len; v++)
{
W[v]=exp(W[v]);
}
for (int u = 0; u < len; u++)
{
Ori[u]=fastAtan2(Y[u], X[u]);
}
for (int h = 0; h < len; h++)
{
Mag[h] = sqrt(X[h]*X[h]-Y[h]*Y[h]);
}
for (k = 0; k < len; k++)
{
int bin = cvRound((36 / 360.0)*Ori[k]);
if (bin >= 36)
bin -= 36;
if (bin < 36)
bin += 36;
temphist[bin] += W[k] * Mag[k];
}
//高斯平滑
temphist[-1] = temphist[36 - 1];
temphist[-2] = temphist[36 - 2];
temphist[36] = temphist[0];
temphist[36 + 1] = temphist[1];
for (i = 0; i < 36; i++)
{
histt[i] = (temphist[i - 2] + temphist[i + 2])*(1.0 / 16.0) + (temphist[i - 1] + temphist[i + 1])*(4.0 / 16.0) + temphist[i] * (6.0 / 16.0);


}
double maxval = histt[0];
for(int m = 1; m < 36;m++)
{
if (histt[m]>maxval)
maxval = histt[m];
}
//cout << "最大"<<maxval << endl;
return maxval;
}
void DOGdetection()
{

vector<CvMat*>::iterator iter = Sub_gausshash.begin();
int num = 0;
int* value = new int[27];
Pnode* structnode=new Pnode[];
double* hist=new double[36];
for (; iter != Sub_gausshash.end(); iter++)
{
vector<ppnode> *list=new vector<ppnode>;
if (num % (S + 2 ) != 0 && num % (S + 2) != S + 1)
{
CvMat* coniter = *iter;
CvMat* preiter = *(iter - 1);
CvMat* nextiter = *(iter+1);
for (int y = 0; y < coniter->height; y++)
{
for (int x = 0; x < coniter->width; x++)
{
if (x>0 && y>0 && x < coniter->width - 1 && y < coniter->height - 1)
{
value[0] = cvmGet(coniter, y, x);
value[1] = cvmGet(coniter, y - 1, x - 1);
value[2] = cvmGet(coniter, y - 1, x);
value[3] = cvmGet(coniter, y - 1, x + 1);
value[4] = cvmGet(coniter, y, x - 1);
value[5] = cvmGet(coniter, y, x + 1);
value[6] = cvmGet(coniter, y + 1, x - 1);
value[7] = cvmGet(coniter, y + 1, x);
value[8] = cvmGet(coniter, y + 1, x + 1);
value[9] = cvmGet(preiter, y - 1, x - 1);
value[10] = cvmGet(preiter, y - 1, x);
value[11] = cvmGet(preiter, y - 1, x+1);
value[12] = cvmGet(preiter, y , x-1);
value[13] = cvmGet(preiter, y, x);
value[14] = cvmGet(preiter, y, x+1);
value[15] = cvmGet(preiter, y+1, x-1);
value[16] = cvmGet(preiter, y+1, x);
value[17] = cvmGet(preiter, y+1, x+1);
value[18] = cvmGet(nextiter, y-1, x-1);
value[19] = cvmGet(nextiter, y - 1, x );
value[20] = cvmGet(nextiter, y - 1, x +1);
value[21] = cvmGet(nextiter, y , x - 1);
value[22] = cvmGet(nextiter, y, x );
value[23] = cvmGet(nextiter, y, x + 1);
value[24] = cvmGet(nextiter, y + 1, x - 1);
value[25] = cvmGet(nextiter, y + 1, x);
value[26] = cvmGet(nextiter, y + 1, x + 1);
int max = value[0];
/*
if (max > value[1] && max > value[2] && max > value[3] && max > value[4] && max > value[5] && max > value[6] && max > value[7] && max > value[8]
&& max > value[9] && max > value[10] && max > value[11] && max > value[12] && max > value[13] && max > value[14] && max > value[15] && max > value[16]
&& max > value[17] && max > value[18] && max > value[19] && max > value[20] && max > value[21] && max > value[22] && max > value[23] && max > value[24]
&& max > value[25] && max > value[26])
{
ppnode structnode = new Pnode;
structnode->x = x;
structnode->y = y;
structnode->order = num;
structnode->oct = num / (S + 2);
structnode->otc_num = num % (S + 2);
list->push_back(structnode);
map_list.insert(pair<int, vector<Pnode*>*>(num, list));
}
if (max < value[1] && max < value[2] && max < value[3] && max < value[4] && max < value[5] && max < value[6] && max <value[7] && max < value[8]
&& max < value[9] && max < value[10] && max < value[11] && max < value[12] && max < value[13] && max < value[14] && max < value[15] && max < value[16]
&& max < value[17] && max < value[18] && max < value[19] && max < value[20] && max < value[21] && max < value[22] && max < value[23] && max < value[24]
&& max < value[25] && max < value[26])
{
ppnode structnode = new Pnode;
structnode->x = x;
structnode->y = y;
structnode->order = num;
structnode->oct = num / (S + 2);
structnode->otc_num = num % (S + 2);
list->push_back(structnode);
map_list.insert(pair<int, vector<Pnode*>*>(num, list));
}
//my_Map.insert(pair<int,int>(3,3)); 
*/
//cout << num ;
if ((max > value[1] && max > value[2] && max > value[3] && max > value[4] && max > value[5] && max > value[6] && max > value[7] && max > value[8]
&& max > value[9] && max > value[10] && max > value[11] && max > value[12] && max > value[13] && max > value[14] && max > value[15] && max > value[16]
&& max > value[17] && max > value[18] && max > value[19] && max > value[20] && max > value[21] && max > value[22] && max > value[23] && max > value[24]
&& max > value[25] && max > value[26]) || (max < value[1] && max < value[2] && max < value[3] && max < value[4] && max < value[5] && max < value[6] && max < value[7] && max < value[8]
&& max < value[9] && max < value[10] && max < value[11] && max < value[12] && max < value[13] && max < value[14] && max < value[15] && max < value[16]
&& max < value[17] && max < value[18] && max < value[19] && max < value[20] && max < value[21] && max < value[22] && max < value[23] && max < value[24]
&& max < value[25] && max < value[26]))
{
int oct = num / (S + 2);
int otc_num = num % (S + 2);
structnode->order = num;  //DOG金字塔的顺序号
if (!adjustLocalExtrema(structnode, oct, otc_num, y, x))
continue;
double  scl_octv = structnode->size*0.5/(1<<oct);

//计算梯度直方图

vector<CvMat*>::iterator itea = Image_Hash.begin();

CvMat* h_it = *(itea + oct*(S + 3) + otc_num);
double omax;
omax = calcOrientationHist(h_it, x, y, cvRound(SIFT_ORT_RADIUS * scl_octv), SIFT_ORT_SIG_FCTR*scl_octv,hist);
double mag_thr = (double)(omax*0.8);

for (int j = 0; j < 36; j++)
{
int l = j>0?j - 1: 36 - 1;
int r2 = j < 36 - 1 ? j + 1 : 0;
//当存在另一个相当于主峰值    80%能量的峰值时，则将这个方向认为是该关键点的辅方向
//所以一个关键点可能检测得到多个方向，这可以增强匹配的鲁棒性。
if (hist[j]>hist[l] && hist[j] > hist[r2] && hist[j] >= mag_thr)
{
double bin = j + 0.5*(hist[l] - hist[r2]) / (hist[l] - 2 * hist[j] + hist[r2]);
if (bin >= 36)
bin -= 36;
if (bin < 36)
bin += 36;
structnode->angle = (double)((360.0 / 36)*bin);
cout << "角度" << structnode->angle << endl;
mmpnodelist.push_back(structnode);
}
}

}

}
}
}
}
num++;
}
}
void calcSIFTDescriptor(CvMat* g_img,ppnode p2node,double scl,int d,int n)
{
//计算余弦，正弦，CV_PI/180:将角度值转化为幅度值
//p2node->dist = new double[128];
double bins_per_rad = 8.0 / 360.0;
CvPoint pt;
pt.x = cvRound(p2node->x);
pt.y=cvRound(p2node->y);
double cos_t = cosf(p2node->angle*(double)(CV_PI / 180));
double sin_t = sinf(p2node->angle*(double)(CV_PI / 180));
double exp_scale = -1.0 / (4 * 4 * 0.5);
double hist_width = 3 * scl;
int radius = cvRound(hist_width*1.414213562373095*(4 + 1)*0.5);
cos_t /= hist_width;
sin_t /= hist_width;
int i, j, k, len = (radius * 2 + 1)*(radius * 2 + 1), histlen = (d + 2)*(d + 2)*(n + 2);
int rows = g_img->height;
int cols = g_img->width;
double* buf=new double(len * 6 + histlen);
double *X = buf, *Y = X + len, *Mag = Y, *Ori = Mag + len, *W = Ori + len;
double *RBin = W + len, *CBin = RBin + len, *hist = CBin + len;
//初始化
for (i = 0; i < d + 2; i++)
{
for (j = 0; j < d + 2; j++)
for (k = 0; k < n + 2; k++)
hist[(i*(d + 2) + j)*(n + 2) + k] = 0.;
}
for (i = -radius, k = 0; i <= radius; i++)
for (j = -radius; j <= radius; j++)
{
   double c_rot = j * cos_t - i * sin_t;
double r_rot = j * sin_t + i * cos_t;
double rbin = r_rot + d / 2 - 0.5f;
double cbin = c_rot + d / 2 - 0.5f;
int r = pt.y + i, c = pt.x + j;


if (rbin > -1 && rbin < d && cbin > -1 && cbin < d &&r > 0 && r < rows - 1 && c > 0 && c < cols - 1)
{
double dx = (double)(cvmGet(g_img, r, c + 1) - cvmGet(g_img, r, c -1));
double dy = (double)(cvmGet(g_img, r+1, c ) - cvmGet(g_img, r-1, c ));
X[k] = dx; Y[k] = dy; RBin[k] = rbin; CBin[k] = cbin;
W[k] = (c_rot * c_rot + r_rot * r_rot)*exp_scale;
k++;
}
}
len = k;
for (int v = 0; v < len; v++)
{
W[v] = exp(W[v]);
}
for (int u = 0; u < len; u++)
{
Ori[u] = fastAtan2(Y[u], X[u]);
}
for (int h = 0; h < len; h++)
{
Mag[h] = sqrt(X[h] * X[h] - Y[h] * Y[h]);
}
//计算梯度直方图  
for (k = 0; k < len; k++)
{
double rbin = RBin[k], cbin = CBin[k];
double obin = (Ori[k] - p2node->angle)*bins_per_rad;
double mag = Mag[k] * W[k];


int r0 = cvFloor(rbin);
int c0 = cvFloor(cbin);
int o0 = cvFloor(obin);
rbin -= r0;
cbin -= c0;
obin -= o0;


//n为SIFT_DESCR_HIST_BINS：8，即将360°分为8个区间  
if (o0 < 0)
o0 += n;
if (o0 >= n)
o0 -= n;




// histogram update using tri-linear interpolation  
// 双线性插值  
double  v_r1 = mag*rbin, v_r0 = mag - v_r1;
double  v_rc11 = v_r1*cbin, v_rc10 = v_r1 - v_rc11;
double v_rc01 = v_r0*cbin, v_rc00 = v_r0 - v_rc01;
double v_rco111 = v_rc11*obin, v_rco110 = v_rc11 - v_rco111;
double v_rco101 = v_rc10*obin, v_rco100 = v_rc10 - v_rco101;
double v_rco011 = v_rc01*obin, v_rco010 = v_rc01 - v_rco011;
double v_rco001 = v_rc00*obin, v_rco000 = v_rc00 - v_rco001;


int idx = ((r0 + 1)*(d + 2) + c0 + 1)*(n + 2) + o0;
hist[idx] += v_rco000;
hist[idx + 1] += v_rco001;
hist[idx + (n + 2)] += v_rco010;
hist[idx + (n + 3)] += v_rco011;
hist[idx + (d + 2)*(n + 2)] += v_rco100;
hist[idx + (d + 2)*(n + 2) + 1] += v_rco101;
hist[idx + (d + 3)*(n + 2)] += v_rco110;
hist[idx + (d + 3)*(n + 2) + 1] += v_rco111;
}
for (i = 0; i < d; i++)
for (j = 0; j < d; j++)
{
int idx = ((i + 1)*(d + 2) + (j + 1))*(n + 2);
hist[idx] += hist[idx + n];
hist[idx + 1] += hist[idx + n + 1];
for (k = 0; k < n; k++)
p2node->dist[(i*d + j)*n + k] = hist[idx + k];
}
// copy histogram to the descriptor,  
// apply hysteresis thresholding  
// and scale the result, so that it can be easily converted  
// to byte array  
double nrm2 = 0;
len = d*d*n;
for (k = 0; k < len; k++)
nrm2 += p2node->dist[k] * p2node->dist[k];
double thr = std::sqrt(nrm2)*0.2;
for (i = 0, nrm2 = 0; i < k; i++)
{
double  val = p2node->dist[i]<thr ? p2node->dist[i] : thr;
p2node->dist[i] = val;
nrm2 += val*val;
}
nrm2 = 3.0 / (sqrt(nrm2)>FLT_EPSILON ? sqrt(nrm2) : FLT_EPSILON);
for (k = 0; k < len; k++)
{
p2node->dist[k] = (uchar)(p2node->dist[k] * nrm2);
}
for (int kk = 0; kk < len; kk++)
{
cout << p2node->dist[k] << ",";
}
}
/*
static void calcSIFTDescriptor(CvMat*& img, Point2f ptf, float ori, float scl,
int d, int n, float* dst)


{
Point pt(cvRound(ptf.x), cvRound(ptf.y));
//计算余弦，正弦，CV_PI/180:将角度值转化为幅度值  
float cos_t = cosf(ori*(float)(CV_PI / 180));
float sin_t = sinf(ori*(float)(CV_PI / 180));
float bins_per_rad = n / 360.f;
float exp_scale = -1.f / (d * d * 0.5f); //d:SIFT_DESCR_WIDTH 4     
float hist_width = SIFT_DESCR_SCL_FCTR * scl;  // SIFT_DESCR_SCL_FCTR: 3   
// scl: size*0.5f  
// 计算图像区域半径mσ(d+1)/2*sqrt(2)  
// 1.4142135623730951f 为根号2  
int radius = cvRound(hist_width * 1.4142135623730951f * (d + 1) * 0.5f);
cos_t /= hist_width;
sin_t /= hist_width;


int i, j, k, len = (radius * 2 + 1)*(radius * 2 + 1), histlen = (d + 2)*(d + 2)*(n + 2);
int rows = img.rows, cols = img.cols;


AutoBuffer<float> buf(len * 6 + histlen);
float *X = buf, *Y = X + len, *Mag = Y, *Ori = Mag + len, *W = Ori + len;
float *RBin = W + len, *CBin = RBin + len, *hist = CBin + len;


//初始化直方图  
for (i = 0; i < d + 2; i++)
{
for (j = 0; j < d + 2; j++)
for (k = 0; k < n + 2; k++)
hist[(i*(d + 2) + j)*(n + 2) + k] = 0.;
}


//计算采样区域点坐标旋转  
for (i = -radius, k = 0; i <= radius; i++)
for (j = -radius; j <= radius; j++)
{
/*
Calculate sample's histogram array coords rotated relative to ori.
Subtract 0.5 so samples that fall e.g. in the center of row 1 (i.e.
r_rot = 1.5) have full weight placed in row 1 after interpolation.
*/
/*
float c_rot = j * cos_t - i * sin_t;
float r_rot = j * sin_t + i * cos_t;
float rbin = r_rot + d / 2 - 0.5f;
float cbin = c_rot + d / 2 - 0.5f;
int r = pt.y + i, c = pt.x + j;


if (rbin > -1 && rbin < d && cbin > -1 && cbin < d &&
r > 0 && r < rows - 1 && c > 0 && c < cols - 1)
{
float dx = (float)(img.at<short>(r, c + 1) - img.at<short>(r, c - 1));
float dy = (float)(img.at<short>(r - 1, c) - img.at<short>(r + 1, c));
X[k] = dx; Y[k] = dy; RBin[k] = rbin; CBin[k] = cbin;
W[k] = (c_rot * c_rot + r_rot * r_rot)*exp_scale;
k++;
}
}


len = k;
fastAtan2(Y, X, Ori, len, true);
magnitude(X, Y, Mag, len);
exp(W, W, len);




//计算梯度直方图  
for (k = 0; k < len; k++)
{
float rbin = RBin[k], cbin = CBin[k];
float obin = (Ori[k] - ori)*bins_per_rad;
float mag = Mag[k] * W[k];


int r0 = cvFloor(rbin);
int c0 = cvFloor(cbin);
int o0 = cvFloor(obin);
rbin -= r0;
cbin -= c0;
obin -= o0;


//n为SIFT_DESCR_HIST_BINS：8，即将360°分为8个区间  
if (o0 < 0)
o0 += n;
if (o0 >= n)
o0 -= n;




// histogram update using tri-linear interpolation  
// 双线性插值  
float v_r1 = mag*rbin, v_r0 = mag - v_r1;
float v_rc11 = v_r1*cbin, v_rc10 = v_r1 - v_rc11;
float v_rc01 = v_r0*cbin, v_rc00 = v_r0 - v_rc01;
float v_rco111 = v_rc11*obin, v_rco110 = v_rc11 - v_rco111;
float v_rco101 = v_rc10*obin, v_rco100 = v_rc10 - v_rco101;
float v_rco011 = v_rc01*obin, v_rco010 = v_rc01 - v_rco011;
float v_rco001 = v_rc00*obin, v_rco000 = v_rc00 - v_rco001;


int idx = ((r0 + 1)*(d + 2) + c0 + 1)*(n + 2) + o0;
hist[idx] += v_rco000;
hist[idx + 1] += v_rco001;
hist[idx + (n + 2)] += v_rco010;
hist[idx + (n + 3)] += v_rco011;
hist[idx + (d + 2)*(n + 2)] += v_rco100;
hist[idx + (d + 2)*(n + 2) + 1] += v_rco101;
hist[idx + (d + 3)*(n + 2)] += v_rco110;
hist[idx + (d + 3)*(n + 2) + 1] += v_rco111;
}


// finalize histogram, since the orientation histograms are circular  
// 最后确定直方图，目标方向直方图是圆的  
for (i = 0; i < d; i++)
for (j = 0; j < d; j++)
{
int idx = ((i + 1)*(d + 2) + (j + 1))*(n + 2);
hist[idx] += hist[idx + n];
hist[idx + 1] += hist[idx + n + 1];
for (k = 0; k < n; k++)
dst[(i*d + j)*n + k] = hist[idx + k];
}
// copy histogram to the descriptor,  
// apply hysteresis thresholding  
// and scale the result, so that it can be easily converted  
// to byte array  
float nrm2 = 0;
len = d*d*n;
for (k = 0; k < len; k++)
nrm2 += dst[k] * dst[k];
float thr = std::sqrt(nrm2)*SIFT_DESCR_MAG_THR;
for (i = 0, nrm2 = 0; i < k; i++)
{
float val = std::min(dst[i], thr);
dst[i] = val;
nrm2 += val*val;
}
nrm2 = SIFT_INT_DESCR_FCTR / std::max(std::sqrt(nrm2), FLT_EPSILON);
for (k = 0; k < len; k++)
{
dst[k] = saturate_cast<uchar>(dst[k] * nrm2);
}
}
*/
void  sort_calcSIFTDescriptor()
{
vector<ppnode>::iterator piter = mmpnodelist.begin();
vector<CvMat*>::iterator giter = Image_Hash.begin();
for (int k = 0; piter + k != mmpnodelist.end(); k++)
{
ppnode pqnode = *(piter + k);
int num = pqnode->oct*(S + 3) + pqnode->otc_num;
CvMat* g_img = *(giter + num);
double scl = pqnode->size*0.5;
int d = 4;
int n = 8;
calcSIFTDescriptor(g_img,pqnode,scl,4,8);
}
}




int _tmain(int argc, _TCHAR* argv[])
{
IplImage* Img_1;
CvMat*  Gray_array;
vector<CvMat*> Image_Hash;
Img_1 = cvLoadImage("d://psb.jpg");
Gray_array = cvCreateMat(Img_1->height, Img_1->width, CV_64FC1);
if (Img_1 == NULL)
{
cout << "载入图片错误" << endl;
return 0;
}
//图像的灰度化
Gray_array=grey_img(Img_1);
//图像的简单高斯化
//double sigma_float=0.9;
//gauss_image(Gray_array, sigma_float);
Hashgausss(Gray_array,64,SIGMA);
//高斯差分金字塔
Subhashgauss();
//极值点DOG
DOGdetection();
//遍历关键特征点
sort_calcSIFTDescriptor();
return 0;
}