图像拼接算法

#ifndef MY_SIFT
#define MY_SIFT
#define SIGMA_INIT 1.6
#define SIFT_INIT_SIGMA 0.5
#define CURVATURE_THRESHOLD 10.0
#define CONTRAST_THRESHOLD0.04 // in terms of 255
#define M_PI3.1415926535897932384626433832795
#define NUM_BINS36
#define FEATURE_WINDOW_SIZE16
#define DESC_NUM_BINS8
#define FVSIZE128
#defineFV_THRESHOLD0.2
#define flt at<float>
#include <opencv2\opencv.hpp>
using namespace cv;
struct Keypoint{
float x,y;//关键点的坐标
int oct;//组数
int intv;//检测到关键点的层
float thetai;
float scl;
float ori;
Keypoint()
{
}
Keypoint(int o,int i,float x,float y,float ti,float scl)
{
this->oct = o;
this->intv = i;
this->x = x;
this->y = y;
this->thetai = ti;
this->scl = scl;
}
void setVal(int o,int i,float x,float y,float ti,float scl)
{
this->oct = o;
this->intv = i;
this->x = x;
this->y = y;
this->thetai = ti;
this->scl = scl;
}
void setOri(float ori)
{
this->ori = ori ;
}
};
class MySIFT
{
public:
MySIFT()
{  this->isEmpty = true;    }
MySIFT(Mat img, int octaves=4, int intervals=3);
MySIFT(const char* filename, int octaves=4, int intervals=3);
~MySIFT();
void DoSift();
void ShowKeypoints();
void operator()(Mat img,Mat& desp,int oct=4,int intv=3);
vector<Keypoint> pKeypoints;
Mat m_keyDescs;// 描述符
private:
void GenerateLists();
void BuildScaleSpace();
void DetectExtrema();
void AssignOrientations();
void ExtractKeypointDescriptors();
void  GetdD(int o,int i,int r,int c,Mat& dD);
inline void  GetHessian(int o,int i,int r,int c,Mat& H);
int is_extremum( Mat** dog_pyr, int octv, int intvl, int r, int c );
bool Interp(int o,int i,int r,int c,Mat& X);
bool calc_grad(Mat img,int r,int c,float* mag,float* ori);
void ori_hist(Mat img,int r,int c,int rad,float sigma,vector<float>& ohist);
void smooth_ori_hist( vector<float>& hist );
void descr_hist(Mat img,int r,int c,float ori,float scl,Mat& des);
private:
bool isEmpty;
void release(void );
private:
Mat m_srcImage;//原始图像
unsigned int m_numOctaves;//组数
unsigned int m_numIntervals;// 每一组的层数
Mat**m_gList;// GOP
Mat**   m_dogList;// DOG
vector<Keypoint> m_keyPoints;// 特征点
};
#endif
#include "MySIFT.h"
#include <iostream>
using namespace std;
MySIFT::MySIFT(Mat img, int octaves, int intervals)
{
this->m_srcImage = img.clone();
m_numOctaves = octaves;
m_numIntervals = intervals;
DoSift();
}
MySIFT::MySIFT(const char* filename, int octaves, int intervals)
{
this->m_srcImage = imread(filename);
m_numOctaves = octaves;
m_numIntervals = intervals;
double t = (double)getTickCount();
DoSift();
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "Times passed in seconds: " << t << endl;
}
//初始化内部数据结构
void MySIFT::GenerateLists()
{
this->isEmpty = true;
unsigned int i=0;
// 高斯金字塔
m_gList = new Mat*[m_numOctaves];
for(i=0;i<m_numOctaves;i++)
m_gList[i] = new Mat[m_numIntervals+3];
// 高斯差分金字塔
m_dogList = new Mat*[m_numOctaves];
for(i=0;i<m_numOctaves;i++)
m_dogList[i] = new Mat[m_numIntervals+2];
this->isEmpty = false;
}
//回收内存垃圾
MySIFT::~MySIFT()
{
release();
}
void MySIFT::release(void)
{
if(this->isEmpty == true)
return;
int i;
for(i=0;i<m_numOctaves;i++){
delete [] m_gList[i];
delete [] m_dogList[i];
}
    delete [] m_gList;
    delete [] m_dogList;
this->isEmpty = true;
}
//使用仿函数处理图像
void MySIFT::operator()(Mat img,Mat& desp,int octs,int intvs)
{
this->release();//释放之前使用的内存
this->m_srcImage.release();//释放源图像
this->m_numIntervals = intvs;
this->m_numOctaves = octs;
this->pKeypoints.clear();//释放特征点
this->m_keyDescs.release();//清空描述符矩阵
this->m_keyPoints.clear();//释放内部特征点
this->m_srcImage = img.clone();
DoSift();
this->m_keyDescs.copyTo(desp);
}
void MySIFT::DoSift()
{
if(this->m_srcImage.empty()){
std::cout<<"Error---- source img is empty!"<<std::endl;
system("pause");
exit(0);
}
GenerateLists();
BuildScaleSpace();
DetectExtrema();
AssignOrientations();
ExtractKeypointDescriptors();
}
void CreateBaseImg(Mat src,Mat& out,double sigma)
{
Mat gray32;
// 生成浮点图像... 把 0..255 转换成 0..1
Mat gray8;
if(src.channels() == 3)
cv::cvtColor(src, gray8 ,CV_BGR2GRAY);
else
gray8 = src;
gray8.convertTo(gray32,CV_32FC1,1.0f/255.0);
double sig= sqrt( sigma * sigma -  SIFT_INIT_SIGMA * SIFT_INIT_SIGMA * 4);
cv::resize(gray32,out,Size(0,0),2,2,CV_INTER_CUBIC);
cv::GaussianBlur(out,out,Size(0,0),sig);
}
void MySIFT::BuildScaleSpace()
{
//printf("Generating scale space...\n");
CreateBaseImg(this->m_srcImage,this->m_gList[0][0],SIGMA_INIT);
// 预先滤波并且放大图像一倍,保存在高斯金字塔的底层（为了增加特征点的数目）
const int _intv  = this->m_numIntervals;
vector<double> sig( _intv + 3);
double sig_total, sig_prev;
sig[0] = SIGMA_INIT;
//创建高斯金字塔
double k = pow( 2.0, 1.0 / _intv );
    for(int i = 1; i < _intv + 3; i++ )   {
sig_prev = pow( k, i - 1)* sig[0];
sig_total = sig_prev * k;
sig[i] = sqrt( sig_total * sig_total - sig_prev * sig_prev );
    }
for( int  o = 0; o < this->m_numOctaves;   o++ ){
for( int  i = 0; i < this->m_numIntervals + 3;  i++ ){
if( o == 0  &&  i == 0 )
continue;
else if( i == 0 ){
cv::pyrDown( this->m_gList[o-1][_intv],this->m_gList[o][i] );
//cv::resize( this->m_gList[o-1][_intv],this->m_gList[o][i],Size(0,0),0.5,0.5,CV_INTER_NN);
}else
cv::GaussianBlur(this->m_gList[o][i-1],this->m_gList[o][i],Size(0,0),sig[i]);
}
}
//创建DOG金字塔
for(int o=0;o<this->m_numOctaves;++o){
for( int i=0; i< _intv + 2; ++i ){
cv::subtract(this->m_gList[o][i+1] ,this->m_gList[o][i],this->m_dogList[o][i]);
}
}
}
inline int MySIFT::is_extremum( Mat** dog_pyr, int octv, int intvl, int r, int c )
{
float val =  dog_pyr[octv][intvl].flt( 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 <  dog_pyr[octv][intvl+i].flt( 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 >  dog_pyr[octv][intvl+i].flt( r + j, c + k ) )
return 0;
}
return 1;
}
inline void MySIFT::GetdD(int o,int i,int r,int c,Mat& dD)
{
float dx,dy,ds;
dx = 0.5 * ( m_dogList[o][i].flt(r,c+1) - m_dogList[o][i].flt(r,c-1) );
dy = 0.5 * ( m_dogList[o][i].flt(r+1,c) - m_dogList[o][i].flt(r+1,c)  );
ds = 0.5 * ( m_dogList[o][i+1].flt(r,c) - m_dogList[o][i-1].flt(r,c)  );
dD = Mat(3,1,CV_32FC1);
dD.flt(0,0)=dx;
dD.flt(1,0)=dy;
dD.flt(2,0)=ds;
}
inline void MySIFT:: GetHessian(int o,int i,int r,int c,Mat& H)
{
float dxx,dyy,dss,dxs,dxy,dys;
Mat mid,up,down;
mid = this->m_dogList[o][i];
up = this->m_dogList[o][i+1];
down = this->m_dogList[o][i-1];
dxx = mid.flt(r,c+1) + mid.flt(r,c-1) - 2*mid.flt(r,c);
dyy = mid.flt(r+1,c) + mid.flt(r-1,c) - 2*mid.flt(r,c);
dss = down.flt(r,c) + up.flt(r,c) - 2*mid.flt(r,c);
dxy = ( mid.flt(r+1,c+1) + mid.flt(r-1,c-1) - mid.flt(r+1,c-1)-mid.flt(r-1,c+1) )/4.0;
dxs = ( up.flt(r,c+1) + down.flt(r,c-1) - up.flt(r,c-1) - down.flt(r,c+1) )/4.0;
dys = ( up.flt(r+1,c) + down.flt(r-1,c) - up.flt(r-1,c) - down.flt(r+1,c) )/4.0;
H = Mat::zeros(3,3,CV_32FC1);
H.flt(0,0) = dxx; H.flt(0,1) = dxy; H.flt(0,2) = dxs;
H.flt(1,0) = dxy; H.flt(1,1) = dyy; H.flt(1,2) = dys;
H.flt(2,0) = dxs; H.flt(2,1) = dys; H.flt(2,2) = dss;
}
bool MySIFT::Interp(int oct,int intv,int r,int c,Mat& X )
{
int ii=0;
Mat H,dD;
while(ii< 5){
this->GetdD(oct,intv,r,c,dD);
this->GetHessian(oct,intv,r,c,H);
X = -1.0f * H.inv() * dD;//偏移
if( abs(X.flt(0)) <0.5 && abs( X.flt(1) ) <0.5 && abs( X.flt(2) ) <0.5 ){
break;
}
r += cvRound( X.flt(1) );
c += cvRound( X.flt(0) );
intv += cvRound( X.flt(2) );
if( intv<1 || intv>this->m_numIntervals || r<5 || c<5 || r > this->m_dogList[oct][0].rows-5 || c > this->m_dogList[oct][0].cols-5)
return false;
++ii;
}
if(ii>=5) return false;
this->GetdD(oct,intv,r,c,dD);
this->GetHessian(oct,intv,r,c,H);
Mat T = dD.t() * X;  //插值
//cout<<"T"<<T<<endl; system("pause");
float t = 0.5f * T.flt(0,0) + this->m_dogList[oct][intv].flt(r,c) ; 
//检测对比度
if( abs( t ) < (CONTRAST_THRESHOLD / this->m_numIntervals) ) return false;
//检测是否边缘
float trH = H.flt(0,0) + H.flt(1,1);//dxx + dyy
float detH = H.flt(0,0) * H.flt(1,1) - H.flt(0,1) * H.flt(1,0);//dxx * dyy - dxy^2;
if(detH <= 0 || ( trH*trH/detH >= ( (CURVATURE_THRESHOLD+1)*(CURVATURE_THRESHOLD+1) / CURVATURE_THRESHOLD ) )){
return false;//是边缘则返回false
}
this->m_keyPoints.push_back(
Keypoint(oct, //组
intv, //层
(r + X.flt(1,0) )*pow(2.0f, oct - 1), //坐标 
(c + X.flt(0,0) )*pow(2.0f, oct - 1),
X.flt(2,0),
SIGMA_INIT * pow(2.0f,(intv + X.flt(2) )/this->m_numIntervals) //尺度因子
)
);
return true;
}
//检测极值点
void MySIFT::DetectExtrema()
{
//
//printf("detecting keypoints....\n");
double prelim_contr_thr = 0.5 * CONTRAST_THRESHOLD / this->m_numIntervals;
for(int o=0;o<this->m_numOctaves;++o)
for(int i=1;i<this->m_numIntervals+1;++i)
for(int r=5;r<this->m_dogList[o][i].rows-5;++r)
for(int c=5;c<this->m_dogList[o][i].cols-5;++c)
if( abs( this->m_dogList[o][i].flt(r,c) ) > prelim_contr_thr)
{
//检测是否是极值点
if( is_extremum( this->m_dogList, o, i, r, c ) )
{
//首先对其进行插值处理
Mat X=Mat::zeros(3,1,CV_32FC1); 
//拟合的偏差X
this->Interp(o,i,r,c,X);
}
}
}
bool MySIFT::calc_grad(Mat img,int r,int c,float* mag,float* ori)
{
if(  r>0 && r< img.rows-1 && c>0 && c< img.cols-1)
{
double dx = img.flt(r,c+1) - img.flt(r,c-1) ;
double dy= img.flt(r-1,c) - img.flt(r+1,c) ;
*mag = sqrt( dx*dx + dy*dy );
*ori = atan2(dy,dx);
return true;
}else{
return false;
}
}
 void MySIFT::ori_hist(Mat img,int r,int c,int rad,float sigma,vector<float>& ohist)
{
ohist.resize(NUM_BINS);
float theta = 2.0 * sigma * sigma;
float mag,ori;
for( int i= -rad;i<rad;++i)
for(int j=-rad;j<rad;++j){
if(calc_grad(img,r + i,c + j,&mag,&ori))
{
float w = exp(-( i*i + j*j ) / theta );
int bin = cvRound( (M_PI+ori)*NUM_BINS/(M_PI*2) );
bin = bin < NUM_BINS ? bin : 0; 
ohist[bin] += w * mag; 
}
}
}
 void MySIFT::smooth_ori_hist( vector<float>& hist )
{
  float prev, tmp, h0 = hist[0];
  int n = hist.size();
  prev = hist[n-1];
  for( int i = 0; i < n; i++ ){
  tmp = hist[i];
  hist[i] = 0.25 * prev + 0.5 * hist[i] +
0.25 * ( ( i+1 == n )? h0 : hist[i+1] );
  prev = tmp;
  }
}
void MySIFT::AssignOrientations()
{
vector<Keypoint>::iterator it = this->m_keyPoints.begin();
vector<Keypoint>::iterator end = this->m_keyPoints.end();
for( ;it != end;++ it)
{
Mat img = this->m_gList[ it->oct ][ it->intv ];
float sigma = it->scl;
vector<float> hist;
ori_hist(img,
it->x,it->y,
cvRound( 3.0 * 1.5 * sigma),1.5 * sigma,hist);//计算直方图
this->smooth_ori_hist(hist);//对直方图进行平滑
this->smooth_ori_hist(hist);
float omax = hist[0];//找到最大值
for(int ii=1;ii < hist.size();++ ii){
if( omax < hist[ii] )   omax = hist[ii];
}
//将大于最大值80%的方向存入特征点；
int l,r;
int n = hist.size();
for(int ii=0;ii< n; ++ii)
{
l = (ii==0 ? n-1:ii-1);  r = ( ii + 1 ) % n;
if( hist[ii] > hist[l]  &&  hist[ii] > hist[r]  &&  hist[ii] >= 0.8 * omax )
{
float bin = ii +  0.5 * ( hist[l] - hist[r] ) / ( hist[l] + hist[r] - 2.0 * hist[ii] );//插值 x* = -dx/dxx
bin = ( bin < 0 )? (n + bin) : (  ( bin >= n )? ( bin - n ): bin  );
float ori =  ( 2.0 * M_PI * bin ) / n  - M_PI;
it->setOri(ori);
this->pKeypoints.push_back( *it );
}
    }
}
}
//--------------------------4-抽取描述符------------------------------------------------------------
static void interp_hist_entry( float*** hist, double rbin, double cbin,
       double obin, double mag, int d, int n )
{
  double d_r, d_c, d_o, v_r, v_c, v_o;
  float** row, * h;
  int r0, c0, o0, rb, cb, ob, r, c, o;
  r0 = cvFloor( rbin );
  c0 = cvFloor( cbin );
  o0 = cvFloor( obin );
  d_r = rbin - r0;
  d_c = cbin - c0;
  d_o = obin - o0;
  /*
   加权
  */
  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 ) % n;
v_o = v_c * ( ( o == 0 )? 1.0 - d_o : d_o );
h[ob] += v_o;
  }
  }
   }
}
     }
}
 void MySIFT::descr_hist(Mat img,int r,int c,float ori,float scl,Mat& des)
{
float cos_t, sin_t, hist_width, exp_denom, r_rot, c_rot, grad_mag,
    grad_ori, w, rbin, cbin, obin, bins_per_rad, PI2 = 2.0 * CV_PI;
int radius, i, j;
float*** hist;
//分配内存
hist = new float**[4];
for( i = 0; i < 4; i++ )
    {
        hist[i] = new float*[4];
        for( j = 0; j < 4; j++ )
hist[i][j] = new float[8];
    }
hist_width = 3 * scl;
cos_t = cos( ori );
    sin_t = sin( ori );
    bins_per_rad = 8 / PI2;//每个区的直方图是8维的
int d = 4;
    exp_denom = d * d * 0.5;
radius = hist_width * sqrt(2.0f) * ( d + 1.0 ) * 0.5 + 0.5;
for( i = -radius; i <= radius; i++ )
for( j = -radius; j <= radius; j++ )
{
c_rot = ( j * cos_t - i * sin_t ) / hist_width;
r_rot = ( j * sin_t + i * cos_t ) / hist_width;
rbin = r_rot + d / 2 - 0.5;
cbin = c_rot + d / 2 - 0.5;
if( rbin > -1.0  &&  rbin < d  &&  cbin > -1.0  &&  cbin < d )
if( this->calc_grad( img, r + i, c + j, &grad_mag, &grad_ori ) )
{
grad_ori -= ori;
while( grad_ori < 0.0 )
grad_ori += PI2;
while( grad_ori >= PI2 )
grad_ori -= PI2;
      
obin = grad_ori * bins_per_rad;
w = exp( -(c_rot * c_rot + r_rot * r_rot) / exp_denom );
interp_hist_entry( hist, rbin, cbin, obin, grad_mag * w, 4, 8 );
}
}
//将数组转换成矩阵
int kk=0;
des = Mat::zeros(1,128,CV_32FC1);
for( i=0;i<4;++i){
for( j=0;j<4;++j){
for( int z =0 ;z<8;++z)
des.flt(0,kk++) = hist[i][j][z];
}
}
//清空内存
for( i = 0; i < 4; i++ ){
  for( j = 0; j < 4; j++ ){
delete [] hist[i][j];
  }
  delete [] hist[i];
    }
delete [] hist;
}
void MySIFT::ExtractKeypointDescriptors( )
{
//printf("抽取特征描述符.....\n");
vector<Keypoint>::iterator it=this->pKeypoints.begin();
vector<Keypoint>::iterator end=this->pKeypoints.end();
Mat outDes;
outDes.create(0,128,CV_32FC1);
for(;it!=end;++it)
{
Mat img = this->m_gList[it->oct][it->intv];
Mat des;
descr_hist( img,it->x,it->y,it->ori,it->scl,des);
outDes.push_back(des);
}
cv::normalize(outDes,this->m_keyDescs);//归一化，去除光照影响
}
void MySIFT::ShowKeypoints()
{
int cnt = 0;
Mat temp = this->m_srcImage.clone();
vector<Keypoint>::iterator it = this->pKeypoints.begin();
vector<Keypoint>::iterator end = this->pKeypoints.end();
for(;it != end; ++it)
{
Point pt = Point( cvRound( it->y ),cvRound( it->x) );
float len = 4.5 * it->scl * pow(2.0,it->oct-1);
circle(temp,pt,len,Scalar(2),1);
int x = len * cosf(it->ori);
int y = -1.0*len * sinf(it->ori);
line(temp,pt,pt+Point(x,y),Scalar(2));
}
printf("keypoint size = %d个\n",this->pKeypoints.size());
imshow("Keypoint",temp);
waitKey(0);
}