啊，半年来跟sift和surf的纠结终于告一段落了

#define xm
using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using ImageProcessing.Base.Imaging;

using ImageProcessing.Graph;

using ImageProcessing.Generic;

using System.Drawing;

using ImageProcessing.Base;
namespace Surf.Base.Net.FastSurf

{

/// <summary>

/// Surf金字塔

/// </summary>

public class SurfPyramid

{

public static bool InterpolateKeypoint(float[,] mat, int dx, int dy, int dscale, SurfFeature feature)

{

bool solve_ok = true;

Matrix A = new Matrix(3, 3);

Matrix X = new Matrix(3, 1);
X[0, 0] = -(mat[1, 5] - mat[1, 3]) / 2; /* Negative 1st deriv with respect to x */

X[1, 0] = -(mat[1, 7] - mat[1, 1]) / 2; /* Negative 1st deriv with respect to y */

X[2, 0] = -(mat[2, 4] - mat[0, 4]) / 2; /* Negative 1st deriv with respect to s */
A[0, 0] = mat[1, 3] - 2 * mat[1, 4] + mat[1, 5]; /* 2nd deriv x, x */

A[0, 1] = (mat[1, 8] - mat[1, 6] - mat[1, 2] + mat[1, 0]) / 4; /* 2nd deriv x, y */

A[0, 2] = (mat[2, 5] - mat[2, 3] - mat[0, 5] + mat[0, 3]) / 4; /* 2nd deriv x, s */

A[1, 0] = A[0, 1]; /* 2nd deriv y, x */

A[1, 1] = mat[1, 1] - 2 * mat[1, 4] + mat[1, 7]; /* 2nd deriv y, y */

A[1, 2] = (mat[2, 7] - mat[2, 1] - mat[0, 7] + mat[0, 1]) / 4; /* 2nd deriv y, s */

A[2, 0] = A[0, 2]; /* 2nd deriv s, x */

A[2, 1] = A[1, 2]; /* 2nd deriv s, y */

A[2, 2] = mat[0, 4] - 2 * mat[1, 4] + mat[2, 4]; /* 2nd deriv s, s */
A.SolveLinear(X);
if (X[0, 0] > 1 || X[0, 0] < -1 || X[1, 0] > 1 || X[1, 0] < -1 || X[2, 0] > 1 || X[2, 0] < -1)

solve_ok = false;

else

{

feature.pt.X += X[0, 0] * dx;

feature.pt.Y += X[1, 0] * dy;

feature.size = (int)Math.Round(feature.size + X[2, 0] * dscale);

}

return solve_ok;

}
public const int haar_size0 = 9;

public const int haar_size_inc = 6;

public static int[,] dx_s = { { 0, 2, 3, 7, 1 }, { 3, 2, 6, 7, -2 }, { 6, 2, 9, 7, 1 } };

public static int[,] dy_s = { { 2, 0, 7, 3, 1 }, { 2, 3, 7, 6, -2 }, { 2, 6, 7, 9, 1 } };

public static int[,] dxy_s = { { 1, 1, 4, 4, 1 }, { 5, 1, 8, 4, -1 }, { 1, 5, 4, 8, -1 }, { 5, 5, 8, 8, 1 } };

public static List<SurfFeature> GetKeypoints(ImageMapF iimg, int octaves, int layers, float thresh)

{

List<SurfFeature> fts = new List<SurfFeature>();

int margin, size, size0, size1;

SurfBox[][] xBoxes, yBoxes, xyBoxes;

SurfFeature ft;

int step = 1;

ImageMapF[][] spaces = new ImageMapF[octaves][];

for (int i = 0; i < octaves; i++)

{

spaces[i] = new ImageMapF[layers + 2];

for (int j = 0; j < layers + 2; j++)

spaces[i][j] = new ImageMapF(iimg.Width / step, iimg.Height / step, -1f);

step *= 2;

}

step = 1;

for (int octave = 0; octave < octaves; octave++)

{

for (int layer = 0; layer < layers + 2; layer++)

{

if (layer != 0 && layer != layers + 1)

{

size = (haar_size0 + haar_size_inc * layer) << octave;

size0 = (haar_size0 + haar_size_inc * (layer - 1)) << octave;

size1 = (haar_size0 + haar_size_inc * (layer + 1)) << octave;
margin = (((step + size1 / 2) + 1) / 2) * 2; //取偶数
xBoxes = SurfBox.Boxes(dx_s, haar_size0, iimg.Width, iimg.Height, size0, size, size1);

yBoxes = SurfBox.Boxes(dy_s, haar_size0, iimg.Width, iimg.Height, size0, size, size1);

xyBoxes = SurfBox.Boxes(dxy_s, haar_size0, iimg.Width, iimg.Height, size0, size, size1);
for (int y = margin; y < iimg.Height - 1 - margin; y += step)

for (int x = margin; x < iimg.Width - 1 - margin; x += step)

if (TryGetFeature(iimg, spaces, octave, layer, step, x, y, thresh, xBoxes, yBoxes, xyBoxes, out ft, size0, size, size1))

fts.Add(ft);

}

}

step *= 2;

}

return fts;

}
/// <summary>

/// 判断(x,y)是否是脚点

/// </summary>

static bool TryGetFeature(ImageMapF iimg,ImageMapF[][] spaces, int octave, int layer, int step, int x, int y,

float thresh, SurfBox[][] xBoxes, SurfBox[][] yBoxes, SurfBox[][] xyBoxes, out SurfFeature result, params int[] sizes)

{

result = null;

float dx, dy, dxy;

float det, db;

byte trc;
int x0 = x / step;

int y0 = y / step;

int idx = (x - sizes[1] / 2 + (y - sizes[1] / 2) * iimg.Width);
dx = SurfBox.Hears(iimg, idx, xBoxes[1]); //获取中心窗口的权值

dy = SurfBox.Hears(iimg, idx, yBoxes[1]);

dxy = SurfBox.Hears(iimg, idx, xyBoxes[1]);

det = dx * dy - 0.81f * dxy * dxy;
if (det >= thresh) //如果大于阈值,判断和邻域两个layer之间的大小关系,在该层上超过阈值再进行其他计算，防止多余的计算

{

trc = (byte)((dx + dy) >= 0 ? 1: 0);

float[,] mat = new float[3, 9];
bool ex = true;

for (int i = -1; i <= 1 && ex; i++) //layer

{

int size = sizes[i + 1]; //获取窗口尺寸

int rd = size / 2; //窗口半径
int nx, ny;

for (int j = -1; j <= 1 && ex; j++) //Y

{

ny = y + j * step - rd;

for (int k = -1; k <= 1 && ex; k++) //X

{

if (spaces[octave][layer + i][y0 + j, x0 + k] == -1) //如果此位置未被计算过,计算此位置，防止重复的计算

{

nx = x + k * step - rd;

idx = nx + ny * iimg.Width;
dx = SurfBox.Hears(iimg, idx, xBoxes[i + 1]);

dy = SurfBox.Hears(iimg, idx, yBoxes[i + 1]);

dxy = SurfBox.Hears(iimg, idx, xyBoxes[i + 1]);
db = dx * dy - 0.81f * dxy * dxy;

spaces[octave][layer + i][y0 + j, x0 + k] = db;

}

else //如果已经被计算过，那么直接赋值

db = spaces[octave][layer + i][y0 + j, x0 + k];

if (i != 0 && j != 0 && k != 0)

if (db >= det) ex = false;
mat[i + 1, 4 + j * 3 + k] = db;

}

}

}
if (ex)

{

SurfFeature ft = new SurfFeature(new PointD(x, y), trc, sizes[1], 0, det, null);

if (InterpolateKeypoint(mat, step, step, sizes[1] - sizes[0], ft))

{

result = ft;

return true;

}

}

}

return false;

}
/// <summary>

/// 角度归纳中扇形区域的总角度

/// </summary>

public const int ori_win = 60;

/// <summary>

/// 角度扫描增量

/// </summary>

public const int ori_search_inc = 5;

/// <summary>

/// 角度扫描半径

/// </summary>

public const int ori_radius = 6;

/// <summary>

/// 角度扫描尺寸

/// </summary>

public const int ori_size = (ori_radius * 2 + 1);

/// <summary>

/// 角度扫描窗口面积

/// </summary>

public const int ori_count = ori_size * ori_size;

/// <summary>

/// 描述子窗口尺寸

/// </summary>

public const int patch_sz = 20;

/// <summary>

/// haar_x窗口

/// </summary>

public static int[,] ori_dx_s = { { 0, 0, 2, 4, -1 }, { 2, 0, 4, 4, 1 } };

/// <summary>

/// haar_y窗口

/// </summary>

public static int[,] ori_dy_s = { { 0, 0, 4, 2, 1 }, { 0, 2, 4, 4, -1 } };

/// <summary>

/// 角度扫描模糊矩阵

/// </summary>

public static GaussainMatrix oriGauss = new GaussainMatrix(2.5, ori_size);

/// <summary>

/// 描述子窗口模糊矩阵

/// </summary>

public static GaussainMatrix desGauss = new GaussainMatrix(3.3, patch_sz);

/// <summary>

/// 角度扫描点

/// </summary>

public static Point[] ori_points = GetAnglePoints();

/// <summary>

/// 角度模糊系数

/// </summary>

public static float[] ori_weights = GetAngleWeights();
static float[] GetAngleWeights()

{

float[] res = new float[ori_count];

int index = 0;

for (int y = 0; y < ori_size; y++)

for (int x = 0; x < ori_size; x++)

res[index++] = (float)oriGauss[x, y];

return res;

}
static Point[] GetAnglePoints()

{

Point[] res = new Point[ori_count];

int index = 0;

for (int y = -ori_radius; y <= ori_radius; y++)

for (int x = -ori_radius; x <= ori_radius; x++)

res[index++] = new Point(x, y);

return res;

}
public static void CreateDescriptors(ImageMapF sum, GrayImage img, SurfFeature feature)

{

int i, j, x, y;

int size = feature.size;

float s = (float)size * 1.2f / 9.0f; //单位窗口尺寸

int grad_wav_size = 2 * (int)Math.Round(2 * s); //角度扫描单位窗口尺寸

int win_size = (int)((patch_sz + 1) * s);
SurfPositionBox[] xBoxes = SurfPositionBox.Boxes(ori_dx_s, 4, grad_wav_size); //X方向haar窗口

SurfPositionBox[] yBoxes = SurfPositionBox.Boxes(ori_dy_s, 4, grad_wav_size);
PointD center = feature.pt;

float[] X = new float[ori_count]; //X坐标序列

float[] Y = new float[ori_count];
int nangle = 0;

List<float> angles = new List<float>();

for (int kk = 0; kk < ori_count; kk++) //获取角度描述子向量

{

float vx, vy;

x = (int)Math.Round(center.X + ori_points[kk].X * s - (float)(grad_wav_size - 1) / 2); //当前位置

y = (int)Math.Round(center.Y + ori_points[kk].Y * s - (float)(grad_wav_size - 1) / 2);

if (y >= (sum.Height - grad_wav_size) || x >= (sum.Width - grad_wav_size))

continue;
vx = SurfPositionBox.Haars(sum, x, y, xBoxes); //求像素值

vy = SurfPositionBox.Haars(sum, x, y, yBoxes);
X[nangle] = vx * ori_weights[kk]; //边缘衰弱

Y[nangle] = vy * ori_weights[kk];
angles.Add((float)new PointD(vx, vy).Angle); //记录角度

nangle++;

}

if (nangle == 0)

{

feature.size = -1;

return;

}
float bestx = 0, besty = 0, descriptor_mod = 0;

for (i = 0; i < 360; i += ori_search_inc) //扫描最优的角度

{

float sumx = 0, sumy = 0, temp_mod;

for (j = 0; j < nangle; j++) //遍历计算出的角度

{

int d = (int)Math.Abs(Math.Round(angles[j]) - i);

if (d < ori_win / 2 || d > 360 - ori_win / 2)

//判断是否在当前角度的邻域范围内,就是在当前角度的[-ori_win / 2,ori_win / 2]范围内,ori_win为60度

{

sumx += X[j]; //向量相加,得出扇形区域的主方向

sumy += Y[j];

}

}

temp_mod = sumx * sumx + sumy * sumy; //计算向量的模

if (temp_mod > descriptor_mod) //记录模最大的向量

{

descriptor_mod = temp_mod;

bestx = sumx;

besty = sumy;

}

}

float descriptor_dir = (float)new PointD(besty, bestx).Angle;

feature.dir = descriptor_dir;
#if !xm

descriptor_dir = (float)(descriptor_dir * Math.PI / 180); //将角度转化为PI

float sin_dir = (float)Math.Sin(descriptor_dir);

float cos_dir = (float)Math.Cos(descriptor_dir);
float win_offset = -(float)(win_size - 1) / 2;

float start_x = center.x + win_offset * cos_dir + win_offset * sin_dir; //左上角旋转到主角度方向

float start_y = center.y - win_offset * sin_dir + win_offset * cos_dir;
ImageMap win = new ImageMap(win_size, win_size);

for (i = 0; i < win_size; i++, start_x += sin_dir, start_y += cos_dir) //将描述子窗口旋转为标准窗口！

{

float pixel_x = start_x;

float pixel_y = start_y;

for (j = 0; j < win_size; j++, pixel_x += cos_dir, pixel_y -= sin_dir) //Y要反向？实在不行就用像素插值

{

x = Math.Min(Math.Max(Math.Round(pixel_x), 0), img.Width - 1);

y = Math.Min(Math.Max(Math.Round(pixel_y), 0), img.Height - 1);

win[i * win_size + j] = img[x, y];

}

}
#else

PointD pd;

ImageMap win = new ImageMap(patch_sz + 1, patch_sz + 21); //要多出一个像素，保证每个像素都可以计算dx 和 dy
ImageTransform2D trans = new ImageTransform2D //逆向插值的旋转-缩放-平移变换

{

Origin = new PointD(patch_sz / 2d, patch_sz / 2d), //中心点,描述子窗口中心点

Rotate = descriptor_dir, //旋转角度,由0度到描述子方向

Scale = (double)win_size / (patch_sz + 1d), //缩放,由描述子矩阵放大到实际图形

Translation = center - new PointD(patch_sz / 2d, patch_sz / 2d) //平移,由描述子矩阵到特征点位置

};
for (y = 0; y <= patch_sz; y++)

{

for (x = 0; x <= patch_sz; x++)

{

pd = trans.Transform(new PointD(x, y)); //求得插值位置

win[x + y * win_size] = GraphHelper.box_gray_pixel(img.Buffers, img.Width, img.Height, pd.X, pd.Y); //像素插值

}

}

#endif

ImageMapF DX = new ImageMapF(patch_sz, patch_sz); //记录x梯度变化

ImageMapF DY = new ImageMapF(patch_sz, patch_sz); //记录y梯度变化
for (i = 0; i < patch_sz; i++) //计算xy方向梯度

{

for (j = 0; j < patch_sz; j++)

{

float dw = (float)desGauss[j, i];

float vx = (win[i, j + 1] - win[i, j] + win[i + 1, j + 1] - win[i + 1, j]) * dw; //边缘衰弱

float vy = (win[i + 1, j] - win[i, j] + win[i + 1, j + 1] - win[i, j + 1]) * dw;

DX[i, j] = vx;

DY[i, j] = vy;

}

}
float[] vec = new float[64];

int vec_ptr = 0;

float square_mag = 0f; //记录64个描述子的平方的总和
for (i = 0; i < 4; i++) //计算描述子 (dx,dy,|dx|,|dy|)*4*4

{

for (j = 0; j < 4; j++) //就是把20个像素高宽的描述子矩阵分成16个5X5的单位窗口

{

for (y = i * 5; y < i * 5 + 5; y++) //计算单位窗口内的梯度变化总和

{

for (x = j * 5; x < j * 5 + 5; x++)

{

float tx = DX[y, x], ty = DY[y, x];

vec[vec_ptr + 0] += tx; // dx

vec[vec_ptr + 1] += ty; // dy

vec[vec_ptr + 2] += (float)Math.Abs(tx);//|dx|

vec[vec_ptr + 3] += (float)Math.Abs(ty);//|dy|

}

}

for (int kk = 0; kk < 4; kk++)

square_mag += vec[vec_ptr + kk] * vec[vec_ptr + kk]; //累加

vec_ptr += 4; //偏移

}

}
float scale = 1f / ((float)Math.Sqrt(square_mag) + float.Epsilon);

for (i = 0; i < 64; i++)

vec[i] = vec[i] * scale;
feature.discriptors = vec;

}
public static void GetOritation(ImageMapF sum, SurfFeature feature)

{

int size = feature.size;

float s = (float)size * 1.2f / 9.0f; //单位窗口尺寸

int grad_wav_size = 2 * (int)Math.Round(2 * s); //角度扫描单位窗口尺寸

int win_size = (int)((patch_sz + 1) * s);
SurfPositionBox[] xBoxes = SurfPositionBox.Boxes(ori_dx_s, 4, grad_wav_size); //X方向haar窗口

SurfPositionBox[] yBoxes = SurfPositionBox.Boxes(ori_dy_s, 4, grad_wav_size);
PointD center = feature.pt;

int x, y;

float[] X = new float[ori_count]; //X坐标序列

float[] Y = new float[ori_count];
int nangle = 0;

List<float> angles = new List<float>();

for (int kk = 0; kk < ori_count; kk++) //获取角度描述子向量

{

float vx, vy;

x = (int)Math.Round(center.X + ori_points[kk].X * s - (float)(grad_wav_size - 1) / 2); //当前位置

y = (int)Math.Round(center.Y + ori_points[kk].Y * s - (float)(grad_wav_size - 1) / 2);

if (y >= (sum.Height - grad_wav_size) || x >= (sum.Width - grad_wav_size))

continue;
vx = SurfPositionBox.Haars(sum, x, y, xBoxes); //求像素值

vy = SurfPositionBox.Haars(sum, x, y, yBoxes);
X[nangle] = vx * ori_weights[kk]; //边缘衰弱

Y[nangle] = vy * ori_weights[kk];
angles.Add((float)new PointD(vx, vy).Angle); //记录角度

nangle++;

}

if (nangle == 0)

{

feature.size = -1;

return;

}
float bestx = 0, besty = 0, descriptor_mod = 0;

for (int i = 0; i < 360; i += ori_search_inc) //扫描最优的角度

{

float sumx = 0, sumy = 0, temp_mod;

for (int j = 0; j < nangle; j++) //遍历计算出的角度

{

int d = (int)Math.Abs(Math.Round(angles[j]) - i);

if (d < ori_win / 2 || d > 360 - ori_win / 2)

//判断是否在当前角度的邻域范围内,就是在当前角度的[-ori_win / 2,ori_win / 2]范围内,ori_win为60度

{

sumx += X[j]; //向量相加,得出扇形区域的主方向

sumy += Y[j];

}

}

temp_mod = sumx * sumx + sumy * sumy; //计算向量的模

if (temp_mod > descriptor_mod) //记录模最大的向量

{

descriptor_mod = temp_mod;

bestx = sumx;

besty = sumy;

}

}

float descriptor_dir = (float)new PointD(besty, bestx).Angle; //... ... ... ... ...
feature.dir = descriptor_dir;

}
public static ImageMapF SquareWindow(ImageMapF sum, SurfFeature feature)

{

int size = feature.size;

float s = (float)size * 1.2f / 9.0f;

int grad_wav_size = 2 * (int)Math.Round(2 * s);

int win_size = (int)((patch_sz + 1) * s);
return SquareWindow(sum, (int)Math.Round(feature.pt.X), (int)Math.Round(feature.pt.Y), win_size, patch_sz);

}
public static ImageMapF SquareWindow(ImageMapF sum, int x, int y, int size, int newSize)

{

ImageMapF res = new ImageMapF(newSize, newSize);

float scale = size / newSize;

int nx, ny;

int top = y - size / 2;

int left = x - size / 2;

int wndSize = (int)Math.Round(scale);

int area = wndSize * wndSize;
for (int ty = 0; ty < newSize; ty++)

{

ny = top + (int)Math.Round(ty * scale);

for (int tx = 0; tx < newSize; tx++)

{

nx = left + (int)Math.Round(tx * scale);

res[ty, tx] = sum.BoxIntegral(nx, ny, wndSize, wndSize) / area;

}

}

return res;

}
}

}