SIFT：尺度不變特徵變換匹配演算法詳解

本章將要講解的內容如下所示：

   1）SIFT演算法的簡介

   2）SIFT演算法的實現細節

   3）SIFT演算法的應用領域

   4）SIFT演算法的改進與擴充套件

一 SIFT演算法的簡介

  1）傳統的特徵提取方法

成像匹配的核心問題是：將同一目標在不同時間、不同解析度、不同光照、不同位姿情況下所成的像相對應。傳統的匹配演算法往往是直接提取角點或者邊緣，對環境中的適應能力較差，急需提出一種棒性強、能夠適應不同光照、不同位姿等情況下的有效識別目標的方法。

  2）SIFT演算法提出的意義

1999年，British Columbia大學大衛.勞伊教授總結了現有的基於不變數技術

將一副影象對映為一個區域性特徵向量集；特徵向量具有平移、縮放、旋轉不變性，同時對光照變化、仿射以及投影也具有一定的不變性。

  3）SIFT演算法的特點

1）SIFT特徵是影象的區域性特徵，其對旋轉、尺度縮放、亮度變化保持不變性，對視角變化、仿射變換、也保持一定程度的穩定性。

2）獨特性好，資訊量豐富，適用於在海量特徵資料庫中進行快速、準確的匹配。

3）可擴充套件性，可以很方便的與其他形式的特徵向量進行聯合。

  4）SIFT演算法可以解決的問題

目標的自身狀態、場景所處的環境和成像器材的成像特性等因素影響影象配準/目標識別跟蹤的效能。而SIFT演算法在一定程度上可以解決：

1）目標的旋轉、縮放、平移(RST)

2）影象仿射/投影變換

3）光照影響

4）目標遮擋

5）雜物場景

6）噪聲

5）SIFT演算法實現步驟簡述

 SIFT演算法的實質可以歸為在不同尺度空間上查詢特徵點(關鍵點)的問題。

1）原影象---->特徵點檢測------->特徵點描述----->目標的特徵點集

2）目標影象-->特徵點檢測------->特徵點描述----->目標的特徵點集

3）兩幅圖片的特徵點集合進行：特徵點匹配-------->匹配點矯正

   SIFT演算法實現物體的識別主要有三大工序：

1）提取關鍵點

2）對關鍵點附加詳細的資訊(區域性特徵)，也就是所謂的描述器

3）通過兩方的特徵點(附帶上特徵向量的關鍵點)的兩兩比較找出相互匹配的若干對特徵點，也就建立了景物間的對應關係。

6）SIFT演算法實現步驟

1）關鍵點檢測

2）關鍵點描述

3）關鍵點匹配

4）消除錯匹配點

/*************************************************************************************************************************
*檔案說明:
*        SIFT演算法的實現標頭檔案
*開發環境:
*        Win10+VS2012+OpenCv2.4.8
*時間地點:
*        陝西師範大學.文津樓 2016.12.30
*再次修改時間:
*        陝西師範大學.文津樓 2017.2.24
*作者資訊:
*        九月
**************************************************************************************************************************/
#ifndef SIFT_H
#define SIFT_H

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

using namespace cv;

typedef double pixel_t;                             //【1】畫素型別

#define INIT_SIGMA 0.5                               //【2】初始sigma
#define SIGMA 1.6
#define INTERVALS 3                                  //【3】高斯金字塔中每組影象中有三層/張圖片

#define RATIO 10                                     //【4】半徑r
#define MAX_INTERPOLATION_STEPS 5                    //【5】最大空間間隔
#define DXTHRESHOLD 0.03                             //【6】|D(x^)| < 0.03   0.03極值點太多

#define ORI_HIST_BINS 36                             //【7】bings=36
#define ORI_SIGMA_TIMES 1.5
#define ORI_WINDOW_RADIUS 3.0 * ORI_SIGMA_TIMES 
#define ORI_SMOOTH_TIMES 2
#define ORI_PEAK_RATIO 0.8
#define FEATURE_ELEMENT_LENGTH 128
#define DESCR_HIST_BINS 8
#define IMG_BORDER 5 
#define DESCR_WINDOW_WIDTH 4
#define DESCR_SCALE_ADJUST 3
#define DESCR_MAG_THR 0.2
#define INT_DESCR_FCTR 512.0
/*********************************************************************************************
*模組說明：
*        關鍵點/特徵點的結構體宣告
*注意點1：
*        在高斯金字塔構建的過程中，一幅影象可以產生好幾組(octave)影象，一組影象包含幾層(inteval)
*        影象
*********************************************************************************************/
struct Keypoint
{
	int    octave;                                        //【1】關鍵點所在組
	int    interval;                                      //【2】關鍵點所在層
	double offset_interval;                               //【3】調整後的層的增量

	int    x;                                             //【4】x,y座標,根據octave和interval可取的層內影象
	int    y;
	double scale;                                         //【5】空間尺度座標scale = sigma0*pow(2.0, o+s/S)
	
	double dx;                                            //【6】特徵點座標，該座標被縮放成原影象大小 
	double dy;

	double offset_x;
	double offset_y;

	//============================================================
	//1---高斯金字塔組內各層尺度座標，不同組的相同層的sigma值相同
	//2---關鍵點所在組的組內尺度
	//============================================================
	double octave_scale;                                  //【1】offset_i;
	double ori;                                           //【2】方向
	int    descr_length;
	double descriptor[FEATURE_ELEMENT_LENGTH];            //【3】特徵點描述符            
	double val;                                           //【4】極值
};
/*********************************************************************************
*模組說明：
*        SIFT演算法內,所有成員函式的宣告
*********************************************************************************/
void read_features(Vector<Keypoint>&features, const char*file);
void write_features(const Vector<Keypoint>&features, const char*file);

void testInverse3D();

void write_pyr(const Vector<Mat>& pyr, const char* dir);
void DrawKeyPoints(Mat &src, Vector<Keypoint>& keypoints);

const char* GetFileName(const char* dir, int i);

void ConvertToGray(const Mat& src, Mat& dst);
void DownSample(const Mat& src, Mat& dst);
void UpSample(const Mat& src, Mat& dst);

void GaussianTemplateSmooth(const Mat &src, Mat &dst, double);
void GaussianSmooth2D(const Mat &src, Mat &dst, double sigma);
void GaussianSmooth(const Mat &src, Mat &dst, double sigma);

void Sift(const Mat &src, Vector<Keypoint>& features, double sigma=SIGMA, int intervals=INTERVALS);

void CreateInitSmoothGray(const Mat &src, Mat &dst, double);
void GaussianPyramid(const Mat &src, Vector<Mat>&gauss_pyr, int octaves, int intervals, double sigma);

void Sub(const Mat& a, const Mat& b, Mat & c);

void DogPyramid(const Vector<Mat>& gauss_pyr, Vector<Mat>& dog_pyr, int octaves, int intervals);
void DetectionLocalExtrema(const Vector<Mat>& dog_pyr, Vector<Keypoint>& extrema, int octaves, int intervals);
void DrawSiftFeatures(Mat& src, Vector<Keypoint>& features);

#endif

/*************************************************************************************************************************
*檔案說明:
*        SIFT演算法的標頭檔案所對應的實現檔案
*開發環境:
*        Win10+VS2012+OpenCv2.4.8
*時間地點:
*        陝西師範大學.文津樓 2016.12.30
*再次修改時間:
*        陝西師範大學.文津樓 2017.2.24
*作者資訊:
*        九月
**************************************************************************************************************************/
#include"sift.h"
#include<fstream>
#include<iostream>

using namespace std;
using namespace cv;
/*************************************************************************************************************************
*模組說明：
*        影象灰度化函式----將彩色影象轉為灰度影象
**************************************************************************************************************************/
void ConvertToGray(const Mat& src,Mat& dst)
{
	cv::Size size = src.size();
	if(dst.empty())
		dst.create(size,CV_64F);                              //[1]利用Mat類的成員函式建立Mat容器
	
	uchar*    srcData = src.data;                             //[2]指向儲存所有畫素值的矩陣的資料區域
	pixel_t*  dstData = (pixel_t*)dst.data;                   //[3]指向dst的矩陣體資料區域
	
	int       dstStep = dst.step/sizeof(dstData[0]);         //[4]一行影象含有多少位元組數

	for(int j=0;j<src.cols;j++)
	{
		for(int i=0;i<src.rows;i++)
		{
			double b = (srcData+src.step*i+src.channels()*j)[0]/255.0;
			double g = (srcData+src.step*i+src.channels()*j)[1]/255.0;
			double r = (srcData+src.step*i+src.channels()*j)[2]/255.0;
			*(dstData+dstStep*i+dst.channels()*j) = (r+g+b)/3.0;
		}
	}
}
/*************************************************************************************************************************
*模組說明：
*        隔點取樣
**************************************************************************************************************************/
void DownSample(const Mat& src, Mat& dst)
{
	if(src.channels() != 1)
		return;

	if(src.cols <=1 || src.rows <=1)
	{
		src.copyTo(dst);
		return;
	}

	dst.create((int)(src.rows/2), (int)(src.cols/2), src.type());

	pixel_t* srcData = (pixel_t*)src.data;
	pixel_t* dstData = (pixel_t*)dst.data;

	int srcStep = src.step/sizeof(srcData[0]);
	int dstStep = dst.step/sizeof(dstData[0]);

	int m = 0, n = 0;
	for(int j = 0; j < src.cols; j+=2, n++)
	{
		m = 0;
		for(int i = 0; i < src.rows; i+=2, m++)
		{
			pixel_t sample = *(srcData + srcStep * i + src.channels() * j);
			if(m < dst.rows && n < dst.cols)
			{
				*(dstData + dstStep * m + dst.channels() * n) = sample;
			}
		}
	}

}
/*************************************************************************************************************************
*模組說明：
*        線性插值放大
**************************************************************************************************************************/
void UpSample(const Mat &src, Mat &dst)
{
	if(src.channels() != 1)
		return;
	dst.create(src.rows*2, src.cols*2, src.type());

	pixel_t* srcData = (pixel_t*)src.data;
	pixel_t* dstData = (pixel_t*)dst.data;

	int srcStep = src.step/sizeof(srcData[0]);
	int dstStep = dst.step/sizeof(dstData[0]);

	int m = 0, n = 0;
	for(int j = 0; j < src.cols-1; j++, n+=2)
	{
		m = 0;
		for(int i = 0; i < src.rows-1; i++, m+=2)
		{
			double sample = *(srcData + srcStep * i + src.channels() * j);
			*(dstData + dstStep * m + dst.channels() * n) = sample;
			
			double rs = *(srcData + srcStep * (i) + src.channels()*j)+(*(srcData + srcStep * (i+1) + src.channels()*j));
			*(dstData + dstStep * (m+1) + dst.channels() * n) = rs/2;
			double cs = *(srcData + srcStep * i + src.channels()*(j))+(*(srcData + srcStep * i + src.channels()*(j+1)));
			*(dstData + dstStep * m + dst.channels() * (n+1)) = cs/2;

			double center = (*(srcData + srcStep * (i+1) + src.channels() * j))
						+ (*(srcData + srcStep * i + src.channels() * j))
						+ (*(srcData + srcStep * (i+1) + src.channels() * (j+1)))
						+ (*(srcData + srcStep * i + src.channels() * (j+1)));

			*(dstData + dstStep * (m+1) + dst.channels() * (n+1)) = center/4;

		}

	}



	if(dst.rows < 3 || dst.cols < 3)
		return;

	//最後兩行兩列
	for(int k = dst.rows-1; k >=0; k--)
	{
		*(dstData + dstStep *(k) + dst.channels()*(dst.cols-2))=*(dstData + dstStep *(k) + dst.channels()*(dst.cols-3));
		*(dstData + dstStep *(k) + dst.channels()*(dst.cols-1))=*(dstData + dstStep *(k) + dst.channels()*(dst.cols-3));
	}
	for(int k = dst.cols-1; k >=0; k--)
	{
		*(dstData + dstStep *(dst.rows-2) + dst.channels()*(k))=*(dstData + dstStep *(dst.rows-3) + dst.channels()*(k));
		*(dstData + dstStep *(dst.rows-1) + dst.channels()*(k))=*(dstData + dstStep *(dst.rows-3) + dst.channels()*(k));
	}

}

/*************************************************************************************************************************
*模組說明：
*        OpenCv中的高斯平滑函式
**************************************************************************************************************************/
void GaussianSmooth(const Mat &src, Mat &dst, double sigma)
{ 
	GaussianBlur(src, dst, Size(0,0), sigma, sigma); 
}
/*************************************************************************************************************************
*模組說明：
*        模組1--建立初始灰度影象------初始影象先將原影象灰度化，再擴大一倍後，使用高斯模糊平滑
*功能說明：
*        在最開始建立高斯金字塔的時候，要預先模糊輸入的影象來作為第0個組的第0層影象，這時相當於丟棄了最高的空域的取樣率。因
*        此，通常的做法是先將影象的尺度擴大一倍來生成第-1組。我們假定初始的輸入影象為了抗擊混淆現象，已經對其進行了sigma=0.5
*        的高斯模糊，如果輸入影象的尺寸用雙線性插值擴大一倍，那麼相當於sigma=1.0
*這樣做的原因:
*        在檢測極值點前，對原始影象的高斯平滑以致影象丟失高頻資訊，所以Lowe建議在建立尺度空間前，首先對原始影象長寬擴充套件一倍，
*        以便保留原始影象的資訊(特別是影象的高頻資訊，比如邊緣，角點)，增加特徵點的數量。
*函式功能:
*        這個函式的作用是用於建立高斯金字塔的-1層影象
*程式碼解讀:
*        2017.2.24----陝西師範大學
**************************************************************************************************************************/
void CreateInitSmoothGray(const Mat &src, Mat &dst, double sigma = SIGMA)
{
	cv::Mat gray;                                  //[1]儲存原始影象的灰度影象        
	cv::Mat up;                                    //[2]儲存原始影象的上取樣影象
	
	ConvertToGray(src, gray);
	UpSample(gray, up);                            //[3]影象的上取樣
	                                               //[4]高斯金字塔的-1組的sigma的計算公式
	double  sigma_init = sqrt(sigma * sigma - (INIT_SIGMA * 2) * (INIT_SIGMA * 2));//[1]-1層的sigma

	GaussianSmooth(up, dst, sigma_init);           //[5]高斯平滑
}
/*************************************************************************************************************************
*模組說明：
*        模組二：3.3 影象高斯金字塔的構建
**************************************************************************************************************************/
void GaussianPyramid(const Mat &src, vector<Mat>&gauss_pyr, int octaves, int intervals = INTERVALS, double sigma = SIGMA)
{
	double *sigmas = new double[intervals+3];
	double k = pow(2.0, 1.0/intervals);

	sigmas[0] = sigma;

	double sig_prev;
	double sig_total;

	for(int i = 1; i < intervals + 3; i++ )
	{
		sig_prev = pow( k, i - 1 ) * sigma;
		sig_total = sig_prev * k;
		sigmas[i] = sqrt( sig_total * sig_total - sig_prev * sig_prev );
	}

	for(int o = 0; o < octaves; o++)
	{
		//每組多三層
		for(int i = 0; i < intervals+3; i++)
		{
			Mat mat;
			if(o == 0 && i == 0)
			{
				src.copyTo(mat);
			}
			else if(i == 0)
			{
				DownSample(gauss_pyr[(o-1)*(intervals+3)+intervals], mat);
			}
			else
			{
				GaussianSmooth(gauss_pyr[o * (intervals+3)+i-1], mat, sigmas[i]);
			}
			gauss_pyr.push_back(mat);
		}
	}
	
	delete[] sigmas;
}
/*************************************************************************************************************************
*模組說明：
*        影象的差分
**************************************************************************************************************************/
void Sub(const Mat& a, const Mat& b, Mat & c)
{
	if(a.rows != b.rows || a.cols != b.cols || a.type() != b.type())
		return;
	if(!c.empty())
		return;
	c.create(a.size(), a.type());

	pixel_t* ap = (pixel_t*)a.data;
	pixel_t* ap_end = (pixel_t*)a.dataend;
	pixel_t* bp = (pixel_t*)b.data;
	pixel_t* cp = (pixel_t*)c.data;
	int step = a.step/sizeof(pixel_t);

	while(ap != ap_end)
	{
		*cp++ = *ap++ - *bp++;
	}
}
/*************************************************************************************************************************
*模組說明：
*       模組三：3.4 高斯差分金字塔
*功能說明：
*       1--2002年，Mikolajczyk在詳細的實驗比較中發現尺度歸一化的高斯拉普拉斯函式的極大值和極小值同其他的特徵提取函式，例如：
*          梯度，Hessian或者Harris角點特徵比較，能夠產生穩定的影象特徵。
*       2--而Lindberg早在1994年發現高斯差分函式（Difference of Gaussian,簡稱DOG運算元）與尺度歸一化的拉普拉斯函式非常相似，因此，
*          利用影象間的差分代替微分。
**************************************************************************************************************************/
void DogPyramid(const Vector<Mat>& gauss_pyr, Vector<Mat>& dog_pyr, int octaves, int intervals=INTERVALS)
{
	for(int o = 0; o < octaves; o ++)
	{
		for(int i = 1; i < intervals+3; i++)
		{
			Mat mat;
			Sub(gauss_pyr[o*(intervals+3)+i], gauss_pyr[o*(intervals+3)+i-1], mat);
			dog_pyr.push_back(mat);
		}
	}
}
/*************************************************************************************************************************
*模組說明：
*       模組四的第一步:3.4-空間極值點的初步檢測(關鍵點的初步探查)
*功能說明:
*       1--在高斯差分函式之後，得到一系列的關鍵點的疑似點，我們需要對這些關鍵點的疑似點初步進行檢測和篩選
*       2--此塊程式碼所根據的原理為CSDN部落格中的：3.5空間極值點的檢測 
**************************************************************************************************************************/
bool isExtremum(int x, int y, const Vector<Mat>& dog_pyr, int index)
{
	pixel_t * data = (pixel_t *)dog_pyr[index].data;
	int      step = dog_pyr[index].step/sizeof(data[0]);
	pixel_t   val  = *(data+y*step+x);

	if(val > 0)
	{
		for(int i = -1; i <= 1; i++)
		{
			int stp = dog_pyr[index+i].step/sizeof(data[0]);
			for(int j = -1; j <= 1; j++)
			{
				for(int k = -1; k <= 1; k++)
				{
					if( val < *((pixel_t*)dog_pyr[index+i].data+stp*(y+j)+(x+k)))
					{
						return false;
					}
				}
			}
		}
	}
	else
	{
		for(int i = -1; i <= 1; i++)
		{
			int stp = dog_pyr[index+i].step/sizeof(pixel_t);
			for(int j = -1; j <= 1; j++)
			{
				for(int k = -1; k <= 1; k++)
				{
					//檢查最小極值
					if( val > *((pixel_t*)dog_pyr[index+i].data+stp*(y+j)+(x+k)))
					{
						return false;
					}
				}
			}
		}
	}
	return true;
}
/*************************************************************************************************************************
*模組說明：
*       模組四的第三步:4.2--消除邊緣響應點
*功能說明:
*       1）一個定義不好的高斯差分運算元的極值在橫跨邊緣的地方有較大的住主曲率，在垂直邊緣的方向有較小的主曲率。
*       2）DOG運算元會產生較強的邊緣響應，需要剔除不穩定的邊緣響應點，獲取特徵點處的Hessian矩陣，主曲率通過一個2*2的Hessian矩
*          陣H求出
*       3）主曲率D和Hessian矩陣的特徵值成正比，公式(r+1)*(r+1)/r的值在兩個特徵值相等時最小；這個值越大，說明兩個特徵值的比值
*          越大，即在某一個方向的梯度值越大，而在另一個方向的梯度值越小，而邊緣恰恰就是這種情況。所以，為了剔除邊緣響應點，
*          需要讓該比值小於一定的閾值，因此，為了檢測主曲率是否在某閾值r下，只需檢測。CSDN論文中的公式(4-7成立)，成立，將關
*          鍵點保留，反之，剔除。
**************************************************************************************************************************/
#define DAt(x, y) (*(data+(y)*step+(x))) 
bool passEdgeResponse(int x, int y, const Vector<Mat>& dog_pyr, int index, double r = RATIO)
{
	pixel_t *data = (pixel_t *)dog_pyr[index].data;
	int step = dog_pyr[index].step/sizeof(data[0]);
	pixel_t val = *(data+y*step+x);

	double Dxx;
	double Dyy;
	double Dxy;
	double Tr_h;                                                         //[1]Hessian矩陣的跡
	double Det_h;                                                        //[2]Hessian矩陣所對應的行列式的值
	 
	Dxx = DAt(x+1,y) + DAt(x-1,y) - 2*val;
	Dyy = DAt(x,y+1) + DAt(x,y-1) - 2*val;
	Dxy = (DAt(x+1,y+1) + DAt(x-1,y-1) - DAt(x-1,y+1) - DAt(x+1,y-1))/4.0;

	Tr_h  = Dxx + Dyy;
	Det_h = Dxx * Dyy - Dxy * Dxy;

	if(Det_h <=0)return false;
	
	if(Tr_h * Tr_h / Det_h < (r + 1) * (r + 1) / r) return true;

	return false;
}
/*************************************************************************************************************************
*模組說明：
*       有限差分求導？
**************************************************************************************************************************/
#define Hat(i, j) (*(H+(i)*3 + (j)))

double PyrAt(const Vector<Mat>& pyr, int index, int x, int y)
{
	pixel_t *data = (pixel_t*)pyr[index].data;
	int      step = pyr[index].step/sizeof(data[0]);
	pixel_t   val = *(data+y*step+x);
	
	return val;
}
/*************************************************************************************************************************
*模組說明：
*       有限差分求導？
**************************************************************************************************************************/
#define At(index, x, y) (PyrAt(dog_pyr, (index), (x), (y)))

//3維D(x)一階偏導,dx列向量
void DerivativeOf3D(int x, int y, const Vector<Mat>& dog_pyr, int index, double *dx)
{
	double Dx = (At(index, x+1, y)-At(index, x-1, y))/2.0;
	double Dy = (At(index, x, y+1)-At(index, x, y-1))/2.0;
	double Ds = (At(index+1, x, y)-At(index-1, x, y))/2.0;

	dx[0] = Dx;
	dx[1] = Dy;
	dx[2] = Ds;
}

//3維D(x)二階偏導，即Hessian矩陣
void Hessian3D(int x, int y, const Vector<Mat>& dog_pyr, int index, double *H)
{
	double val, Dxx, Dyy, Dss, Dxy, Dxs, Dys;
	
	val = At(index, x, y);

	Dxx = At(index, x+1, y) + At(index, x-1, y) - 2*val;
	Dyy = At(index, x, y+1) + At(index, x, y-1) - 2*val;
	Dss = At(index+1, x, y) + At(index-1, x, y) - 2*val;

	Dxy = (At(index, x+1, y+1) + At(index, x-1, y-1)
		 - At(index, x+1, y-1) - At(index, x-1, y+1))/4.0;

	Dxs = (At(index+1, x+1, y) + At(index-1, x-1, y)
		 - At(index-1, x+1, y) - At(index+1, x-1, y))/4.0;

	Dys = (At(index+1, x, y+1) + At(index-1, x, y-1)
		 - At(index+1, x, y-1) - At(index-1, x, y+1))/4.0;

	Hat(0, 0) = Dxx;
	Hat(1, 1) = Dyy;
	Hat(2, 2) = Dss;

	Hat(1, 0) = Hat(0, 1) = Dxy;
	Hat(2, 0) = Hat(0 ,2) = Dxs;
	Hat(2, 1) = Hat(1, 2) = Dys;
}
/*************************************************************************************************************************
*模組說明：
*       4.4 三階矩陣求逆
**************************************************************************************************************************/
#define HIat(i, j) (*(H_inve+(i)*3 + (j)))
//3*3階矩陣求逆
bool Inverse3D(const double *H, double *H_inve)
{

	double A = Hat(0, 0)*Hat(1, 1)*Hat(2, 2) 
			 + Hat(0, 1)*Hat(1, 2)*Hat(2, 0)
			 + Hat(0, 2)*Hat(1, 0)*Hat(2, 1)
			 - Hat(0, 0)*Hat(1, 2)*Hat(2, 1)
			 - Hat(0, 1)*Hat(1, 0)*Hat(2, 2)
			 - Hat(0, 2)*Hat(1, 1)*Hat(2, 0);

	if(fabs(A) < 1e-10) return false;

	HIat(0, 0) = Hat(1, 1) * Hat(2, 2) - Hat(2, 1)*Hat(1, 2);
	HIat(0, 1) = -(Hat(0, 1) * Hat(2, 2) - Hat(2, 1) * Hat(0, 2));
	HIat(0, 2) = Hat(0, 1) * Hat(1, 2) - Hat(0, 2)*Hat(1, 1);
	
	HIat(1, 0) = Hat(1, 2) * Hat(2, 0) - Hat(2, 2)*Hat(1, 0);
	HIat(1, 1) = -(Hat(0, 2) * Hat(2, 0) - Hat(0, 0) * Hat(2, 2));
	HIat(1, 2) = Hat(0, 2) * Hat(1, 0) - Hat(0, 0)*Hat(1, 2);
	
	HIat(2, 0) = Hat(1, 0) * Hat(2, 1) - Hat(1, 1)*Hat(2, 0);
	HIat(2, 1) = -(Hat(0, 0) * Hat(2, 1) - Hat(0, 1) * Hat(2, 0));
	HIat(2, 2) = Hat(0, 0) * Hat(1, 1) - Hat(0, 1)*Hat(1, 0);

	for(int i = 0; i < 9; i++)
	{
		*(H_inve+i) /= A;
	}
	return true;
}
/*************************************************************************************************************************
*模組說明：
*       
**************************************************************************************************************************/
//計算x^
void GetOffsetX(int x, int y, const Vector<Mat>& dog_pyr, int index, double *offset_x)
{
	//x^ = -H^(-1) * dx; dx = (Dx, Dy, Ds)^T
	double H[9], H_inve[9]= {0};
	Hessian3D(x, y, dog_pyr, index, H);
	Inverse3D(H, H_inve);
	double dx[3];
	DerivativeOf3D(x, y, dog_pyr, index, dx);
	
	for(int i = 0; i < 3; i ++)
	{
		offset_x[i] = 0.0;
		for(int j = 0; j < 3; j++)
		{
			offset_x[i] += H_inve[i*3 + j] * dx[j];
		}
		offset_x[i] = -offset_x[i];
	}
}

//計算|D(x^)|
double GetFabsDx(int x, int y, const Vector<Mat>& dog_pyr, int index, const double* offset_x)
{
	//|D(x^)|=D + 0.5 * dx * offset_x; dx=(Dx, Dy, Ds)^T
	double dx[3];
	DerivativeOf3D(x, y, dog_pyr, index, dx);

	double term = 0.0;
	for(int i = 0; i < 3; i++)
		term += dx[i] * offset_x[i];

	pixel_t *data = (pixel_t *)dog_pyr[index].data;
	int step = dog_pyr[index].step/sizeof(data[0]);
	pixel_t val = *(data+y*step+x);

	return fabs(val + 0.5 * term);
}
/*************************************************************************************************************************
*模組說明：
*       模組四的第二步:修正極值點，刪除不穩定的點
*功能說明:
*       1--根據高斯差分函式產生的極值點並不全都是穩定的特徵點，因為某些極值點的響應較弱，而且DOG運算元會產生較強的邊緣響應
*       2--以上方法檢測到的極值點是離散空間的極值點，下面通過擬合三維二次函式來精確定位關鍵點的位置和尺度，同時去除對比度
*          低和不穩定的邊緣響應點(因為DOG運算元會產生較強的邊緣響應)，以增強匹配的穩定性、提高抗噪聲的能力。
*       3--修正極值點，刪除不穩定點，|D(x)| < 0.03 Lowe 2004
**************************************************************************************************************************/
Keypoint* InterploationExtremum(int x, int y, const Vector<Mat>& dog_pyr, int index, int octave, int interval, double dxthreshold = DXTHRESHOLD)
{
	//計算x=(x,y,sigma)^T
	//x^ = -H^(-1) * dx; dx = (Dx, Dy, Ds)^T
	double offset_x[3]={0};

	const Mat &mat = dog_pyr[index];

	int idx   = index;
	int intvl = interval;
	int i     = 0; 

	while(i < MAX_INTERPOLATION_STEPS)
	{
		GetOffsetX(x, y, dog_pyr, idx, offset_x);
		//4. Accurate keypoint localization.  Lowe
		//如果offset_x 的任一維度大於0.5，it means that the extremum lies closer to a different sample point.
		if(fabs(offset_x[0]) < 0.5 && fabs(offset_x[1]) < 0.5 && fabs(offset_x[2]) < 0.5)
			break;

		//用周圍的點代替
		x += cvRound(offset_x[0]);
		y += cvRound(offset_x[1]);
		interval += cvRound(offset_x[2]);

		idx = index - intvl + interval;
		//此處保證檢測邊時 x+1,y+1和x-1, y-1有效
		if( interval < 1 || interval > INTERVALS ||x >= mat.cols-1 || x < 2 ||y >= mat.rows-1 || y < 2)  
		{
			return NULL;
		}

		i++;
	}

	//竄改失敗
	if(i >= MAX_INTERPOLATION_STEPS)
		return NULL;

	//rejecting unstable extrema
	//|D(x^)| < 0.03取經驗值
	if(GetFabsDx(x, y, dog_pyr, idx, offset_x) < dxthreshold/INTERVALS)
	{
		return NULL;
	}

	Keypoint *keypoint = new Keypoint;


	keypoint->x = x;
	keypoint->y = y;

	keypoint->offset_x        = offset_x[0];
	keypoint->offset_y        = offset_x[1];

	keypoint->interval        = interval;
	keypoint->offset_interval = offset_x[2];

	keypoint->octave          = octave;

	keypoint->dx              = (x + offset_x[0])*pow(2.0, octave);
	keypoint->dy              = (y + offset_x[1])*pow(2.0, octave);

	return keypoint;
}
/*************************************************************************************************************************
*模組說明：
*       模組四：3.5 空間極值點的檢測(關鍵點的初步探查)
*功能說明：
*       1--關鍵點是由DOG空間的區域性極值點組成的，關鍵點的初步探查是通過同一組內各DoG相鄰兩層影象之間的比較完成的。為了尋找DoG
*          函式的極值點，每一個畫素點都要和它所有相鄰的點比較，看其是否比它的影象域和尺度域相鄰的點大還是小。
*       2--當然這樣產生的極值點並不全都是穩定的特徵點，因為某些極值點相應較弱，而且DOG運算元會產生較強的邊緣響應。
**************************************************************************************************************************/
void DetectionLocalExtrema(const Vector<Mat>& dog_pyr, Vector<Keypoint>& extrema, int octaves, int intervals=INTERVALS)
{

	double  thresh = 0.5 * DXTHRESHOLD / intervals;

	for(int o = 0; o < octaves; o++)
	{
		//第一層和最後一層極值忽略
		for(int i = 1; i < (intervals + 2) - 1; i++)
		{
			int index    = o*(intervals+2)+i;                              //[1]圖片索引的定位
			pixel_t *data = (pixel_t *)dog_pyr[index].data;                //[2]獲取圖片的矩陣體的首地址
			int step     = dog_pyr[index].step/sizeof(data[0]);           //[3]說明矩陣在儲存空間中的儲存是以線性空間的方式存放的

	
			for(int y = IMG_BORDER; y < dog_pyr[index].rows-IMG_BORDER; y++)
			{
				for(int x = IMG_BORDER; x < dog_pyr[index].cols-IMG_BORDER; x++)
				{
					pixel_t val = *(data+y*step+x);
					if(std::fabs(val) > thresh )                           //[4]排除閾值過小的點
					{ 		
						if(isExtremum(x,y, dog_pyr, index))                //[5]判斷是否是極值
						{
							Keypoint *extrmum = InterploationExtremum(x, y, dog_pyr, index, o, i);
							if(extrmum)
							{
								if(passEdgeResponse(extrmum->x,extrmum->y, dog_pyr, index))
								{	
									extrmum->val = *(data+extrmum->y*step+extrmum->x);
									extrema.push_back(*extrmum);
								}

								delete extrmum;
	
							}//extrmum
						}//isExtremum
					}//std::fabs
				}//for x
			}//for y
		
		}
	}
}
/*************************************************************************************************************************
*模組說明：
*       模組五：
*功能說明：
*     
**************************************************************************************************************************/
void CalculateScale(Vector<Keypoint>& features, double sigma = SIGMA, int intervals = INTERVALS)
{
	double intvl = 0;
	for(int i = 0; i < features.size(); i++)
	{
		intvl                    = features[i].interval + features[i].offset_interval;
		features[i].scale        = sigma * pow(2.0, features[i].octave + intvl/intervals);
		features[i].octave_scale = sigma * pow(2.0, intvl/intervals);
	}

}

//對擴大的影象特徵縮放
void HalfFeatures(Vector<Keypoint>& features)
{
	for(int i = 0; i < features.size(); i++)
	{
		features[i].dx /= 2;
		features[i].dy /= 2;

		features[i].scale /= 2;
	}
}
/********************************************************************************************************************************
*模組說明:
*        模組六---步驟2：計算關鍵點的梯度和梯度方向
*功能說明：
*        1）計算關鍵點(x,y)處的梯度幅值和梯度方向
*        2）將所計算出來的梯度幅值和梯度方向儲存在變數mag和ori中
*********************************************************************************************************************************/
bool CalcGradMagOri(const Mat& gauss, int x, int y, double& mag, double& ori)
{
	if(x > 0 && x < gauss.cols-1 && y > 0 && y < gauss.rows -1)
	{
		pixel_t *data = (pixel_t*)gauss.data;
		int step     = gauss.step/sizeof(*data);

		double dx = *(data+step*y+(x+1))-(*(data+step*y+(x-1)));           //[1]利用X方向上的差分代替微分dx
		double dy = *(data+step*(y+1)+x)-(*(data+step*(y-1)+x));           //[2]利用Y方向上的差分代替微分dy

		mag = sqrt(dx*dx + dy*dy);                                          //[3]計算該關鍵點的梯度幅值
		ori = atan2(dy, dx);                                                //[4]計算該關鍵點的梯度方向
		return true;
	}
	else
		return false;
}
/********************************************************************************************************************************
*模組說明:
*        模組六---步驟1：計算梯度的方向直方圖
*功能說明：
*        1）直方圖以每10度為一個柱，共36個柱，柱代表的方向為為畫素點的梯度方向，柱的長短代表了梯度幅值。
*        2）根據Lowe的建議，直方圖統計採用3*1.5*sigma
*        3）在直方圖統計時，每相鄰三個畫素點採用高斯加權，根據Lowe的建議，模板採用[0.25,0.5,0.25],並且連續加權兩次
*結    論：
*        影象的關鍵點檢測完畢後，每個關鍵點就擁有三個資訊：位置、尺度、方向；同時也就使關鍵點具備平移、縮放和旋轉不變性
*********************************************************************************************************************************/
double* CalculateOrientationHistogram(const Mat& gauss, int x, int y, int bins, int radius, double sigma)
{
	double* hist = new double[bins];                           //[1]動態分配一個double型別的陣列
	for(int i = 0; i < bins; i++)                               //[2]給這個陣列初始化
		*(hist + i) = 0.0;

	double  mag;                                                //[3]關鍵點的梯度幅值                                          
	double  ori;                                                //[4]關鍵點的梯度方向
	double  weight;

	int           bin;
	const double PI2   = 2.0*CV_PI;
	double        econs = -1.0/(2.0*sigma*sigma);

	for(int i = -radius; i <= radius; i++)
	{
		for(int j = -radius; j <= radius; j++)
		{
			if(CalcGradMagOri(gauss, x+i, y+j, mag, ori))       //[5]計算該關鍵點的梯度幅值和方向
			{
				weight = exp((i*i+j*j)*econs);
				bin    = cvRound(bins * (CV_PI - ori)/PI2);     //[6]對一個double行的數進行四捨五入，返回一個整形的數
				bin    = bin < bins ? bin : 0;

				hist[bin] += mag * weight;                      //[7]統計梯度的方向直方圖
			}
		}
	}

	return hist;
}
/********************************************************************************************************************************
*模組說明:
*        模組六---步驟3：對梯度方向直方圖進行連續兩次的高斯平滑
*功能說明：
*        1）在直方圖統計時，每相鄰三個畫素點採用高斯加權，根據Lowe的建議，模板採用[0.25,0.5,0.25],並且連續加權兩次
*        2）對直方圖進行兩次平滑
*********************************************************************************************************************************/
void GaussSmoothOriHist(double *hist, int n)
{
	double prev = hist[n-1];
	double temp;
	double h0   = hist[0];

	for(int i = 0; i < n; i++)
	{
		temp    = hist[i];
		hist[i] = 0.25 * prev + 0.5 * hist[i] + 0.25 * (i+1 >= n ? h0 : hist[i+1]);//對方向直方圖進行高斯平滑
		prev    = temp;
	}
}
/********************************************************************************************************************************
*模組說明:
*        模組六---步驟4：計算方向直方圖中的主方向
*********************************************************************************************************************************/
double DominantDirection(double *hist, int n)
{
	double maxd = hist[0];
	for(int i = 1; i < n; i++)
	{
		if(hist[i] > maxd)                            //求取36個柱中的最大峰值
			maxd = hist[i];
	}
	return maxd;
}
void CopyKeypoint(const Keypoint& src, Keypoint& dst)
{
	dst.dx = src.dx;
	dst.dy = src.dy;

	dst.interval        = src.interval;
	dst.octave          = src.octave;
	dst.octave_scale    = src.octave_scale;
	dst.offset_interval = src.offset_interval;

	dst.offset_x = src.offset_x;
	dst.offset_y = src.offset_y;
	
	dst.ori      = src.ori;
	dst.scale    = src.scale;
	dst.val      = src.val;
	dst.x        = src.x;
	dst.y        = src.y;
}
/********************************************************************************************************************************
*模組說明:
*        模組六---步驟5：計算更加精確的關鍵點主方向----拋物插值
*功能說明：
*        1）方向直方圖的峰值則代表了該特徵點的方向，以直方圖中的最大值作為該關鍵點的主方向。為了增強匹配的魯棒性，只保留峰值大於主
*           方向峰值80%的方向作為改關鍵點的輔方向。因此，對於同一梯度值得多個峰值的關鍵點位置，在相同位置和尺度將會有多個關鍵點被
*           建立但方向不同。僅有15%的關鍵點被賦予多個方向，但是可以明顯的提高關鍵點的穩定性。
*        2）在實際程式設計中，就是把該關鍵點複製成多份關鍵點，並將方向值分別賦給這些複製後的關鍵點
*        3）並且，離散的梯度直方圖要進行【插值擬合處理】，來求得更加精確的方向角度值
*********************************************************************************************************************************/
#define Parabola_Interpolate(l, c, r) (0.5*((l)-(r))/((l)-2.0*(c)+(r))) 
void CalcOriFeatures(const Keypoint& keypoint, Vector<Keypoint>& features, const double *hist, int n, double mag_thr)
{
	double  bin;
	double  PI2 = CV_PI * 2.0;
	int l;
	int r;

	for(int i = 0; i < n; i++)
	{
		l = (i == 0) ? n-1 : i -1;
		r = (i+1)%n;

		//hist[i]是極值
		if(hist[i] > hist[l] && hist[i] > hist[r] && hist[i] >= mag_thr)
		{
			bin = i + Parabola_Interpolate(hist[l], hist[i], hist[r]);
			bin = (bin < 0) ? (bin + n) : (bin >=n ? (bin -n) : bin);

			Keypoint new_key;

			CopyKeypoint(keypoint, new_key);

			new_key.ori = ((PI2 * bin)/n) - CV_PI;
			features.push_back(new_key);
		}
	}
}
/********************************************************************************************************************************
*模組說明:
*        模組六：5 關鍵點方向分配
*功能說明：
*        1）為了使描述符具有旋轉不變性，需要利用影象的區域性特徵為每一個關鍵點分配一個基準方向。使用影象梯度的方法求取區域性結構的穩定
*           方向。
*        2）對於在DOG金字塔中檢測出來的關鍵點，採集其所在高斯金字塔影象3sigma鄰域視窗內畫素的梯度和方向梯度和方向特徵。
*        3）梯度的模和方向如下所示:
*        4) 在完成關鍵點的梯度計算後，使用直方圖統計鄰域內畫素的梯度和方向。梯度直方圖將0~360度的方向範圍分為36個柱，其中每柱10度，
*           如圖5.1所示，直方圖的峰值方向代表了關鍵點的主方向
*********************************************************************************************************************************/
void OrientationAssignment(Vector<Keypoint>& extrema, Vector<Keypoint>& features, const Vector<Mat>& gauss_pyr)
{
	int n = extrema.size();
	double *hist;

	for(int i = 0; i < n; i++)
	{

		hist = CalculateOrientationHistogram(gauss_pyr[extrema[i].octave*(INTERVALS+3)+extrema[i].interval],
			extrema[i].x, extrema[i].y, ORI_HIST_BINS, cvRound(ORI_WINDOW_RADIUS*extrema[i].octave_scale), 
			ORI_SIGMA_TIMES*extrema[i].octave_scale);                             //[1]計算梯度的方向直方圖
	
		for(int j = 0; j < ORI_SMOOTH_TIMES; j++)
			GaussSmoothOriHist(hist, ORI_HIST_BINS);                              //[2]對方向直方圖進行高斯平滑
		double highest_peak = DominantDirection(hist, ORI_HIST_BINS);            //[3]求取方向直方圖中的峰值
		                                                                          //[4]計算更加精確的關鍵點主方向
		CalcOriFeatures(extrema[i], features, hist, ORI_HIST_BINS, highest_peak*ORI_PEAK_RATIO);

		delete[] hist;
	
	}
}

void InterpHistEntry(double ***hist, double xbin, double ybin, double obin, double mag, int bins, int d)
{
	double d_r, d_c, d_o, v_r, v_c, v_o;
	double** row, * h;
	int r0, c0, o0, rb, cb, ob, r, c, o;

	r0 = cvFloor( ybin );
	c0 = cvFloor( xbin );
	o0 = cvFloor( obin );
	d_r = ybin - r0;
	d_c = xbin - c0;
	d_o = obin - o0;

	/*
		做插值：
		xbin,ybin,obin:種子點所在子視窗的位置和方向
		所有種子點都將落在4*4的視窗中
		r0,c0取不大於xbin，ybin的正整數
		r0,c0只能取到0,1,2
		xbin,ybin在(-1, 2)

		r0取不大於xbin的正整數時。
		r0+0 <= xbin <= r0+1
		mag在區間[r0,r1]上做插值

		obin同理
	*/

	for( r = 0; r <= 1; r++ )
	{
		rb = r0 + r;
		if( rb >= 0  &&  rb < d )
		{
			v_r = mag * ( ( r == 0 )? 1.0 - d_r : d_r );
			row = hist[rb];
			for( c = 0; c <= 1; c++ )
			{
				cb = c0 + c;
				if( cb >= 0  &&  cb < d )
				{
					v_c = v_r * ( ( c == 0 )? 1.0 - d_c : d_c );
					h = row[cb];
					for( o = 0; o <= 1; o++ )
					{
						ob = ( o0 + o ) % bins;
						v_o = v_c * ( ( o == 0 )? 1.0 - d_o : d_o );
						h[ob] += v_o;
					}
				}
			}
		}
	}

	
}
/********************************************************************************************************************************
*模組說明:
*        模組七--步驟1:計算描述子的直方圖
*功能說明：
*      
*********************************************************************************************************************************/
double*** CalculateDescrHist(const Mat& gauss, int x, int y, double octave_scale, double ori, int bins, int width)
{
	double ***hist = new double**[width];

	for(int i = 0; i < width; i++)
	{
		hist[i] = new double*[width];
		for(int j = 0; j < width; j++)
		{
			hist[i][j] = new double[bins];
		}
	}
	
	for(int r = 0; r < width; r++ )
		for(int c = 0; c < width; c++ )
			for(int o = 0; o < bins; o++ )
				hist[r][c][o]=0.0;


	double cos_ori = cos(ori);
	double sin_ori = sin(ori);

	//6.1高斯權值，sigma等於描述字視窗寬度的一半
	double sigma = 0.5 * width;
	double conste = -1.0/(2*sigma*sigma);

	double PI2 = CV_PI * 2;

	double sub_hist_width = DESCR_SCALE_ADJUST * octave_scale;

	//【1】計算描述子所需的影象領域區域的半徑
	int    radius   = (sub_hist_width*sqrt(2.0)*(width+1))/2.0 + 0.5;    //[1]0.5取四捨五入
	double grad_ori;
	double grad_mag;

	for(int i = -radius; i <= radius; i++)
	{
		for(int j =-radius; j <= radius; j++)
		{
			double rot_x = (cos_ori * j - sin_ori * i) / sub_hist_width;
			double rot_y = (sin_ori * j + cos_ori * i) / sub_hist_width;

			double xbin = rot_x + width/2 - 0.5;                         //[2]xbin,ybin為落在4*4視窗中的下標值
			double ybin = rot_y + width/2 - 0.5;

			if(xbin > -1.0 && xbin < width && ybin > -1.0 && ybin < width)
			{
				if(CalcGradMagOri(gauss, x+j, y + i, grad_mag, grad_ori)) //[3]計算關鍵點的梯度方向
				{
					grad_ori = (CV_PI-grad_ori) - ori;
					while(grad_ori < 0.0)
						grad_ori += PI2;
					while(grad_ori >= PI2)
						grad_ori -= PI2;

					double obin = grad_ori * (bins/PI2);

					double weight = exp(conste*(rot_x*rot_x + rot_y * rot_y));

					InterpHistEntry(hist, xbin, ybin, obin, grad_mag*weight, bins, width);

				}
			}
		}
	}

	return hist;
}

void NormalizeDescr(Keypoint& feat)
{
	double cur, len_inv, len_sq = 0.0;
	int i, d = feat.descr_length;

	for( i = 0; i < d; i++ )
	{
		cur = feat.descriptor[i];
		len_sq += cur*cur;
	}
	len_inv = 1.0 / sqrt( len_sq );
	for( i = 0; i < d; i++ )
		feat.descriptor[i] *= len_inv;
}
/********************************************************************************************************************************
*模組說明:
*        模組七--步驟2:直方圖到描述子的轉換
*功能說明：
*      
*********************************************************************************************************************************/
void HistToDescriptor(double ***hist, int width, int bins, Keypoint& feature)
{
	int int_val, i, r, c, o, k = 0;

	for( r = 0; r < width; r++ )
		for( c = 0; c < width; c++ )
			for( o = 0; o < bins; o++ )
			{
				feature.descriptor[k++] = hist[r][c][o];
			}

	feature.descr_length = k;
	NormalizeDescr(feature);                           //[1]描述子特徵向量歸一化

	for( i = 0; i < k; i++ )                           //[2]描述子向量門限
		if( feature.descriptor[i] > DESCR_MAG_THR )    
			feature.descriptor[i] = DESCR_MAG_THR;

	NormalizeDescr(feature);                           //[3]描述子進行最後一次的歸一化操作

	for( i = 0; i < k; i++ )                           //[4]將單精度浮點型的描述子轉換為整形的描述子
	{
		int_val = INT_DESCR_FCTR * feature.descriptor[i];
		feature.descriptor[i] = min( 255, int_val );
	}
}
/********************************************************************************************************************************
*模組說明:
*        模組七:6 關鍵點描述
*功能說明：
*        1）通過以上步驟，對於一個關鍵點，擁有三個資訊：位置、尺度、方向
*        2）接下來就是為每個關鍵點建立一個描述符，用一組向量來將這個關鍵點描述出來，使其不隨各種變化而變化，比如光照、視角變化等等
*        3）這個描述子不但包括關鍵點，也包含關鍵點周圍對其貢獻的畫素點，並且描述符應該有較高的獨特性，以便於特徵點正確的匹配概率
*        1）SIFT描述子----是關鍵點鄰域高斯影象梯度統計結果的一種表示。
*        2）通過對關鍵點周圍影象區域分塊，計算塊內梯度直方圖，生成具有獨特性的向量
*        3）這個向量是該區域影象資訊的一種表述和抽象，具有唯一性。
*Lowe論文：
*    Lowe建議描述子使用在關鍵點尺度空間內4*4的視窗中計算的8個方向的梯度資訊，共4*4*8=128維向量來表徵。具體的步驟如下所示:
*        1)確定計算描述子所需的影象區域
*        2）將座標軸旋轉為關鍵點的方向，以確保旋轉不變性，如CSDN博文中的圖6.2所示；旋轉後鄰域取樣點的新座標可以通過公式(6-2)計算
*        3）將鄰域內的取樣點分配到對應的子區域，將子區域內的梯度值分配到8個方向上，計算其權值
*        4）插值計算每個種子點八個方向的梯度
*        5）如上統計的4*4*8=128個梯度資訊即為該關鍵點的特徵向量。特徵向量形成後，為了去除光照變化的影響，需要對它們進行歸一化處理，
*           對於影象灰度值整體漂移，影象各點的梯度是鄰域畫素相減得到的，所以也能去除。得到的描述子向量為H，歸一化之後的向量為L
*        6）描述子向量門限。非線性光照，相機飽和度變化對造成某些方向的梯度值過大，而對方向的影響微弱。因此，設定門限值（向量歸一化
*           後，一般取0.2）截斷較大的梯度值。然後，在進行一次歸一化處理，提高特徵的鑑別性。
*        7）按特徵點的尺度對特徵描述向量進行排序
*        8）至此，SIFT特徵描述向量生成。
*********************************************************************************************************************************/
void DescriptorRepresentation(Vector<Keypoint>& features, const Vector<Mat>& gauss_pyr, int bins, int width)
{
	double ***hist;

	for(int i = 0; i < features.size(); i++)
	{                                                                       //[1]計算描述子的直方圖
		hist = CalculateDescrHist(gauss_pyr[features[i].octave*(INTERVALS+3)+features[i].interval],
			features[i].x, features[i].y, features[i].octave_scale, features[i].ori, bins, width);

		HistToDescriptor(hist, width, bins, features[i]);                   //[2]直方圖到描述子的轉換

		for(int j = 0; j < width; j++)
		{
			for(int k = 0; k < width; k++)
			{
				delete[] hist[j][k];
			}
			delete[] hist[j];
		}
		delete[] hist;
	}
}

bool FeatureCmp(Keypoint& f1, Keypoint& f2)
{
	return f1.scale < f2.scale;
}
/*******************************************************************************************************************
*函式說明:
*        最大的模組1：SIFT演算法模組
*函式引數說明:
*        1---const Mat &src---------------準備進行特徵點檢測的原始圖片
*        2---Vector<Keypoint>& features---用來儲存檢測出來的關鍵點
*        3---double sigma-----------------sigma值
*        4---int intervals----------------關鍵點所在的層數
********************************************************************************************************************/
void Sift(const Mat &src, Vector<Keypoint>& features, double sigma, int intervals)
{
	std::cout<<"【Step_one】Create -1 octave gaussian pyramid image"<<std::endl;
	cv::Mat          init_gray;
	CreateInitSmoothGray(src, init_gray, sigma);                                   
	int octaves = log((double)min(init_gray.rows, init_gray.cols))/log(2.0) - 2;             //計算高斯金字塔的層數
	std::cout<<"【1】The height and width of init_gray_img = "<<init_gray.rows<<"*"<<init_gray.cols<<std::endl;
	std::cout<<"【2】The octaves of the gauss pyramid      = "<<octaves<<std::endl;


	std::cout <<"【Step_two】Building gaussian pyramid ..."<<std::endl;                                                         
	std::vector<Mat> gauss_pyr;                                                  
	GaussianPyramid(init_gray, gauss_pyr, octaves, intervals, sigma);            
	write_pyr(gauss_pyr, "gausspyramid");
	

	std::cout <<"【Step_three】Building difference of gaussian pyramid..."<<std::endl;                   
	Vector<Mat> dog_pyr;                                                          
	DogPyramid(gauss_pyr, dog_pyr, octaves, intervals);                           
	write_pyr(dog_pyr, "dogpyramid");



	std::cout <<"【Step_four】Deatecting local extrema..."<<std::endl;                                                     
	Vector<Keypoint> extrema;                                                     
	DetectionLocalExtrema(dog_pyr, extrema, octaves, intervals);                  
	std::cout <<"【3】keypoints cout: "<< extrema.size()<<" --"<<std::endl;	
	std::cout <<"【4】extrema detection finished."<<std::endl;
	std::cout <<"【5】please look dir gausspyramid, dogpyramid and extrema.txt.--"<<endl;



	std::cout <<"【Step_five】CalculateScale..."<<std::endl;   
	CalculateScale(extrema, sigma, intervals);                                    
	HalfFeatures(extrema);



	std::cout << "【Step_six】Orientation assignment..."<<std::endl;                                    
	OrientationAssignment(extrema, features, gauss_pyr);                         
	std::cout << "【6】features count: "<<features.size()<<std::endl;
	  


	std::cout << "【Step_seven】DescriptorRepresentation..."<<std::endl;
	DescriptorRepresentation(features, gauss_pyr, DESCR_HIST_BINS, DESCR_WINDOW_WIDTH);
	sort(features.begin(), features.end(), FeatureCmp);                           
	cout << "finished."<<endl;
}
/*******************************************************************************************************************
*函式說明:
*        畫出SIFT特徵點的具體函式
********************************************************************************************************************/
void DrawSiftFeature(Mat& src, Keypoint& feat, CvScalar color)
{
	int len, hlen, blen, start_x, start_y, end_x, end_y, h1_x, h1_y, h2_x, h2_y;
	double scl, ori;
	double scale = 5.0;
	double hscale = 0.75;
	CvPoint start, end, h1, h2;

	/* compute points for an arrow scaled and rotated by feat's scl and ori */
	start_x = cvRound( feat.dx );
	start_y = cvRound( feat.dy );
	scl = feat.scale;
	ori = feat.ori;
	len = cvRound( scl * scale );
	hlen = cvRound( scl * hscale );
	blen = len - hlen;
	end_x = cvRound( len *  cos( ori ) ) + start_x;
	end_y = cvRound( len * -sin( ori ) ) + start_y;
	h1_x = cvRound( blen *  cos( ori + CV_PI / 18.0 ) ) + start_x;
	h1_y = cvRound( blen * -sin( ori + CV_PI / 18.0 ) ) + start_y;
	h2_x = cvRound( blen *  cos( ori - CV_PI / 18.0 ) ) + start_x;
	h2_y = cvRound( blen * -sin( ori - CV_PI / 18.0 ) ) + start_y;
	start = cvPoint( start_x, start_y );
	end = cvPoint( end_x, end_y );
	h1 = cvPoint( h1_x, h1_y );
	h2 = cvPoint( h2_x, h2_y );

	line( src, start, end, color, 1, 8, 0 );
	line( src, end, h1, color, 1, 8, 0 );
	line( src, end, h2, color, 1, 8, 0 );
}
/*******************************************************************************************************************
*函式說明:
*         最大的模組3：畫出SIFT特徵點
********************************************************************************************************************/
void DrawSiftFeatures(Mat& src, Vector<Keypoint>& features)
{
	CvScalar color = CV_RGB( 0, 255, 0 );
	for(int i = 0; i < features.size(); i++)
	{
		DrawSiftFeature(src, features[i], color);
	}
}
/*******************************************************************************************************************
*函式說明:
*         最大的模組2：畫出關鍵點KeyPoints
********************************************************************************************************************/
void DrawKeyPoints(Mat &src, Vector<Keypoint>& keypoints)
{
	int j = 0;
	for(int i = 0; i < keypoints.size(); i++)
	{
	
		CvScalar color = {255, 0 ,0};
		circle(src, Point(keypoints[i].dx, keypoints[i].dy), 3, color);
		j++;
	}
}

const char* GetFileName(const char* dir, int i)
{
	char *name = new char[50];
	sprintf(name, "%s\\%d\.jpg",dir, i);
	return name;
}

void cv64f_to_cv8U(const Mat& src, Mat& dst)
{
	double* data = (double*)src.data;
	int step = src.step/sizeof(*data);

	if(!dst.empty())
		return;
	dst.create(src.size(), CV_8U);

	uchar* dst_data = dst.data;

	for(int i = 0, m = 0; i < src.cols; i++, m++)
	{
		for(int j = 0, n = 0; j < src.rows; j++, n++)
		{
			*(dst_data+dst.step*j+i) = (uchar)(*(data+step*j+i)*255);	
		}
	}
}


//通過轉換後儲存的影象，會失真,和imshow顯示出的影象相差很大
void writecv64f(const char* filename, const Mat& mat)
{
	Mat dst;
	cv64f_to_cv8U(mat, dst);
	imwrite(filename, dst);
}

void write_pyr(const Vector<Mat>& pyr, const char* dir)
{
	for(int i = 0; i < pyr.size(); i++)
	{
				writecv64f(GetFileName(dir, i), pyr[i]);
	}
}

void read_features(Vector<Keypoint>&features, const char*file)
{
	ifstream in(file);
	int n = 0, dims = 0;
	in >> n >> dims; 
	cout <<n <<" " <<dims<<endl;
	for(int i = 0; i < n; i++)
	{
		Keypoint key;
		in >> key.dy >> key.dx  >> key.scale >> key.ori;
		for(int j = 0; j < dims; j++)
		{
			in >> key.descriptor[j];
		}
		features.push_back(key);
	}
	in.close();
}
/*******************************************************************************************************************
*函式說明:
*         最大的模組4：將特徵點寫入文字檔案
********************************************************************************************************************/
void write_features(const Vector<Keypoint>&features, const char*file)
{
	ofstream dout(file);
	dout << features.size()<<" "<< FEATURE_ELEMENT_LENGTH<<endl;
	for(int i = 0; i < features.size(); i++)
	{
		dout <<features[i].dy<< " " << features[i].dx<<" "<<features[i].scale<< " "<<features[i].ori<<endl;
		for(int j = 0; j < FEATURE_ELEMENT_LENGTH; j++)
		{
			if(j % 20 == 0)
				dout<<endl;
			dout << features[i].descriptor[j]<<" "; 
		}
		dout<<endl;
	}
	dout.close();
}

/*************************************************************************************************************************
*檔案說明:
*        SIFT演算法的實現
*開發環境:
*        Win10+VS2012+OpenCv2.4.8
*時間地點:
*        陝西師範大學.文津樓 2016.12.30
*再次修改時間:
*        陝西師範大學.文津樓 2017.2.24
*作者資訊:
*        九月
**************************************************************************************************************************/
#include <windows.h>
#include <iostream>
#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>

#include "sift.h"


using namespace std;
using namespace cv;


int main(int argc, char **argv)
{
	cv::Mat src = imread("jobs_512.jpg");

	if(src.empty())
	{
		cout << "jobs_512.jpg open error! "<<endl;
		getchar();
		return -1;
	}

	if(src.channels()!=3) return -2;

	cv::Vector<Keypoint> features;

	Sift(src, features, 1.6);                           //【1】SIFT特徵點檢測和特徵點描述
 	DrawKeyPoints(src, features);                       //【2】畫出關鍵點(特徵點)
	DrawSiftFeatures(src, features);                    //【3】畫出SIFT特徵點
	write_features(features, "descriptor.txt");         //【4】將特徵點寫入文字

	cv::imshow("src", src);
	cv::waitKey();

	return 0;
}

參考資料：

1）http://blog.csdn.net/maweifei/article/details/53932782

2）http://wenku.baidu.com/view/e10ed068561252d380eb6eec.html?re=view