啊，半年来跟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;
        }

    }
}