經典算法研究系列:九之再續：教你一步一步用c語言實現sift算法、下

 作者：July、二零一一年三月十二日
出處：http://blog.csdn.net/v_JULY_v。
參考：Rob Hess維護的sift 庫
環境：windows xp+vc6.0
條件：c語言實現。
說明：本BLOG內會陸續一一實現所有經典算法。
------------------------

本文接上，教你一步一步用c語言實現sift算法、上，而來：
函數編寫
    ok，接上文，咱們一個一個的來編寫main函數中所涉及到所有函數，這也是本文的關鍵部分：
//下采樣原來的圖像，返回縮小2倍尺寸的圖像
CvMat * halfSizeImage(CvMat * im)
{
 unsigned int i,j;
 int w = im->cols/2;
 int h = im->rows/2;
 CvMat *imnew = cvCreateMat(h, w, CV_32FC1);
 
#define Im(ROW,COL) ((float *)(im->data.fl + im->step/sizeof(float) *(ROW)))[(COL)]
#define Imnew(ROW,COL) ((float *)(imnew->data.fl + imnew->step/sizeof(float) *(ROW)))[(COL)]
 for ( j = 0; j < h; j++)
  for ( i = 0; i < w; i++)
   Imnew(j,i)=Im(j*2, i*2);
  return imnew;
}

//上采樣原來的圖像，返回放大2倍尺寸的圖像
CvMat * doubleSizeImage(CvMat * im)
{
 unsigned int i,j;
 int w = im->cols*2;
 int h = im->rows*2;
 CvMat *imnew = cvCreateMat(h, w, CV_32FC1);
 
#define Im(ROW,COL) ((float *)(im->data.fl + im->step/sizeof(float) *(ROW)))[(COL)]
#define Imnew(ROW,COL) ((float *)(imnew->data.fl + imnew->step/sizeof(float) *(ROW)))[(COL)]
 
 for ( j = 0; j < h; j++)
  for ( i = 0; i < w; i++)
   Imnew(j,i)=Im(j/2, i/2);
 
  return imnew;
}

//上采樣原來的圖像，返回放大2倍尺寸的線性插值圖像
CvMat * doubleSizeImage2(CvMat * im)
{
 unsigned int i,j;
 int w = im->cols*2;
 int h = im->rows*2;
 CvMat *imnew = cvCreateMat(h, w, CV_32FC1);
 
#define Im(ROW,COL) ((float *)(im->data.fl + im->step/sizeof(float) *(ROW)))[(COL)]
#define Imnew(ROW,COL) ((float *)(imnew->data.fl + imnew->step/sizeof(float) *(ROW)))[(COL)]
 
 // fill every pixel so we dont have to worry about skipping pixels later
 for ( j = 0; j < h; j++)
 {
  for ( i = 0; i < w; i++)
  {
   Imnew(j,i)=Im(j/2, i/2);
  }
 }
 /*
 A B C
 E F G
 H I J
 pixels A C H J are pixels from original image
 pixels B E G I F are interpolated pixels
 */
 // interpolate pixels B and I
 for ( j = 0; j < h; j += 2)
  for ( i = 1; i < w - 1; i += 2)
   Imnew(j,i)=0.5*(Im(j/2, i/2)+Im(j/2, i/2+1));
  // interpolate pixels E and G
  for ( j = 1; j < h - 1; j += 2)
   for ( i = 0; i < w; i += 2)
    Imnew(j,i)=0.5*(Im(j/2, i/2)+Im(j/2+1, i/2));
   // interpolate pixel F
   for ( j = 1; j < h - 1; j += 2)
    for ( i = 1; i < w - 1; i += 2)
     Imnew(j,i)=0.25*(Im(j/2, i/2)+Im(j/2+1, i/2)+Im(j/2, i/2+1)+Im(j/2+1, i/2+1));
    return imnew;
}

//雙線性插值，返回像素間的灰度值
float getPixelBI(CvMat * im, float col, float row)
{
 int irow, icol;
 float rfrac, cfrac;
 float row1 = 0, row2 = 0;
 int width=im->cols;
 int height=im->rows;
#define ImMat(ROW,COL) ((float *)(im->data.fl + im->step/sizeof(float) *(ROW)))[(COL)]
 
 irow = (int) row;
 icol = (int) col;
 
 if (irow < 0 || irow >= height
  || icol < 0 || icol >= width)
  return 0;
 if (row > height - 1)
  row = height - 1;
 if (col > width - 1)
  col = width - 1;
 rfrac = 1.0 - (row - (float) irow);
 cfrac = 1.0 - (col - (float) icol);
 if (cfrac < 1)
 {
  row1 = cfrac * ImMat(irow,icol) + (1.0 - cfrac) * ImMat(irow,icol+1);
 }
 else
 {
  row1 = ImMat(irow,icol);
 }
 if (rfrac < 1)
 {
  if (cfrac < 1)
  {
   row2 = cfrac * ImMat(irow+1,icol) + (1.0 - cfrac) * ImMat(irow+1,icol+1);
  } else
  {
   row2 = ImMat(irow+1,icol);
  }
 }
 return rfrac * row1 + (1.0 - rfrac) * row2;
}

//矩陣歸一化
void normalizeMat(CvMat* mat)
{
#define Mat(ROW,COL) ((float *)(mat->data.fl + mat->step/sizeof(float) *(ROW)))[(COL)]
 float sum = 0;
 
 for (unsigned int j = 0; j < mat->rows; j++)
  for (unsigned int i = 0; i < mat->cols; i++)
   sum += Mat(j,i);
  for ( j = 0; j < mat->rows; j++)
   for (unsigned int i = 0; i < mat->rows; i++)
    Mat(j,i) /= sum;
}

//向量歸一化
void normalizeVec(float* vec, int dim)
{
    unsigned int i;
 float sum = 0;
 for ( i = 0; i < dim; i++)
  sum += vec[i];
 for ( i = 0; i < dim; i++)
  vec[i] /= sum;
}

//得到向量的歐式長度，2-范數
float GetVecNorm( float* vec, int dim )
{
 float sum=0.0;
 for (unsigned int i=0;i<dim;i++)
  sum+=vec[i]*vec[i];
    return sqrt(sum);
}

//產生1D高斯核
float* GaussianKernel1D(float sigma, int dim)
{
 
 unsigned int i;
 //printf("GaussianKernel1D(): Creating 1x%d vector for sigma=%.3f gaussian kernel ", dim, sigma);
 
 float *kern=(float*)malloc( dim*sizeof(float) );
 float s2 = sigma * sigma;
 int c = dim / 2;
 float m= 1.0/(sqrt(2.0 * CV_PI) * sigma);
    double v;
 for ( i = 0; i < (dim + 1) / 2; i++)
 {
  v = m * exp(-(1.0*i*i)/(2.0 * s2)) ;
  kern[c+i] = v;
  kern[c-i] = v;
 }
 //   normalizeVec(kern, dim);
 // for ( i = 0; i < dim; i++)
 //  printf("%f  ", kern[i]);
 //  printf(" ");
 return kern;
}

//產生2D高斯核矩陣
CvMat* GaussianKernel2D(float sigma)
{
 // int dim = (int) max(3.0f, GAUSSKERN * sigma);
    int dim = (int) max(3.0f, 2.0 * GAUSSKERN *sigma + 1.0f);
 // make dim odd
 if (dim % 2 == 0)
  dim++;
 //printf("GaussianKernel(): Creating %dx%d matrix for sigma=%.3f gaussian ", dim, dim, sigma);
 CvMat* mat=cvCreateMat(dim, dim, CV_32FC1);
#define Mat(ROW,COL) ((float *)(mat->data.fl + mat->step/sizeof(float) *(ROW)))[(COL)]
 float s2 = sigma * sigma;
 int c = dim / 2;
 //printf("%d %d ", mat.size(), mat[0].size());
    float m= 1.0/(sqrt(2.0 * CV_PI) * sigma);
 for (int i = 0; i < (dim + 1) / 2; i++)
 {
  for (int j = 0; j < (dim + 1) / 2; j++)
  {
   //printf("%d %d %d ", c, i, j);
   float v = m * exp(-(1.0*i*i + 1.0*j*j) / (2.0 * s2));
   Mat(c+i,c+j) =v;
   Mat(c-i,c+j) =v;
   Mat(c+i,c-j) =v;
   Mat(c-i,c-j) =v;
  }
 }
 // normalizeMat(mat);
 return mat;
}

//x方向像素處作卷積
float ConvolveLocWidth(float* kernel, int dim, CvMat * src, int x, int y)
{
#define Src(ROW,COL) ((float *)(src->data.fl + src->step/sizeof(float) *(ROW)))[(COL)]
    unsigned int i;
 float pixel = 0;
    int col;
 int cen = dim / 2;
 //printf("ConvolveLoc(): Applying convoluation at location (%d, %d) ", x, y);
 for ( i = 0; i < dim; i++)
 {
  col = x + (i - cen);
  if (col &