从图像中提取亚像素边缘点

为什么需要拟合？

梯度的连续变化

边缘处的灰度变化是连续的，不是阶跃的。梯度幅值在边缘处呈现山峰状分布：

灰度值变化：

复制代码

         _______
        /       \
       /         \
______/           \______
      ↑           ↑
    边缘位置    边缘位置
    (实际在像素之间)

梯度幅值在垂直边缘方向形成抛物线状的峰值，我们需要找到这个峰值的精确位置。

亚像素精度

复制代码

像素级精度 vs 亚像素精度

原始边缘：     |      |
像素网格：  □  □  □  □  □
Canny检测：   ↓     ↓
             1.0   2.0  ← 像素坐标（误差±0.5）
亚像素拟合： ↓        ↓
            1.23   2.17 ← 真实位置（精度±0.1）

关键优化点：

SIMD梯度计算：使用SSE指令并行处理8个像素

扫描行缓冲区：避免重复内存访问

边界镜像处理：第一行和最后一行使用镜像填充

亚像素拟合：二次曲面拟合定位边缘到亚像素精度

内存优化：按需重新分配数组

c 复制代码

#include <vector>
#include <cmath>
#include <cstring>
#include <cstdlib>
#include <immintrin.h>
#include <algorithm>

// 特征点结构
struct EdgePoint {
    float x;        // 亚像素X坐标
    float y;        // 亚像素Y坐标
    float angle;    // 梯度方向（弧度）
    float magnitude;// 梯度幅值（匹配权重）
    
    EdgePoint(float _x = 0, float _y = 0, float _angle = 0, float _mag = 0) 
        : x(_x), y(_y), angle(_angle), magnitude(_mag) {}
};

// 3x3二次曲面拟合，返回亚像素偏移
bool FitQuadraticSurface(float* gradient_mag, int width, int x, int y, float& dx, float& dy) {
    // 获取3x3梯度幅值窗口
    const float* center = &gradient_mag[y * width + x];
    const float* top = center - width;
    const float* bottom = center + width;
    
    float p[3][3] = {
        {top[-1], top[0], top[1]},
        {center[-1], center[0], center[1]},
        {bottom[-1], bottom[0], bottom[1]}
    };
    
    // 计算一阶导数（中心差分）
    float gx = (p[1][2] - p[1][0]) * 0.5f;     // ∂f/∂x
    float gy = (p[2][1] - p[0][1]) * 0.5f;     // ∂f/∂y
    
    // 计算二阶导数
    float gxx = p[1][2] - 2.0f * p[1][1] + p[1][0];  // ∂²f/∂x²
    float gyy = p[2][1] - 2.0f * p[1][1] + p[0][1];  // ∂²f/∂y²
    float gxy = (p[2][2] - p[2][0] - p[0][2] + p[0][0]) * 0.25f;  // ∂²f/∂x∂y
    
    // 构建海森矩阵 H = [[gxx, gxy], [gxy, gyy]]
    // 求解 H * [dx, dy]^T = -[gx, gy]^T
    float det = gxx * gyy - gxy * gxy;
    
    if (fabs(det) < 1e-8f) {
        dx = dy = 0.0f;
        return false;
    }
    
    // 求解线性方程组
    dx = -(gyy * gx - gxy * gy) / det;
    dy = -(-gxy * gx + gxx * gy) / det;
    
    // 限制偏移在[-0.5, 0.5]范围内
    dx = std::clamp(dx, -0.5f, 0.5f);
    dy = std::clamp(dy, -0.5f, 0.5f);
    
    return true;
}

// 计算梯度方向和幅值
void ComputeGradientAtPoint(unsigned char* img, int stride, int x, int y, 
                           float& gx, float& gy, float& mag) {
    // Sobel算子
    const unsigned char* top = img + (y-1) * stride;
    const unsigned char* center = img + y * stride;
    const unsigned char* bottom = img + (y+1) * stride;
    
    // Sobel X: [-1 0 1; -2 0 2; -1 0 1]
    gx = (top[x+1] + 2*center[x+1] + bottom[x+1]) -
         (top[x-1] + 2*center[x-1] + bottom[x-1]);
    
    // Sobel Y: [-1 -2 -1; 0 0 0; 1 2 1]
    gy = (bottom[x-1] + 2*bottom[x] + bottom[x+1]) -
         (top[x-1] + 2*top[x] + top[x+1]);
    
    // 归一化因子
    gx /= 8.0f;
    gy /= 8.0f;
    
    // 梯度幅值
    mag = sqrtf(gx * gx + gy * gy);
}

// SSE优化的梯度幅值计算
void ComputeGradientMagnitudeSSE(unsigned char* blurred_img, int width, int height, int stride,
                                 std::vector<float>& gradient_mag) {
    gradient_mag.resize(width * height, 0.0f);
    
    // 分配扫描行缓冲区
    std::vector<unsigned char> scan_buffer(3 * (width + 2));
    unsigned char* prev_line = &scan_buffer[0];
    unsigned char* curr_line = prev_line + width + 2;
    unsigned char* next_line = curr_line + width + 2;
    
    // 初始化第一行
    curr_line[0] = blurred_img[0];
    memcpy(&curr_line[1], blurred_img, width);
    curr_line[width + 1] = blurred_img[width - 1];
    memcpy(prev_line, curr_line, width + 2);
    
    // Sobel卷积核常量
    alignas(16) const int8_t sobel_x_kernel[16] = {-1, 0, 1, -2, 0, 2, -1, 0, 1};
    alignas(16) const int8_t sobel_y_kernel[16] = {-1, -2, -1, 0, 0, 0, 1, 2, 1};
    
    __m128i sobel_x = _mm_load_si128((__m128i*)sobel_x_kernel);
    __m128i sobel_y = _mm_load_si128((__m128i*)sobel_y_kernel);
    
    for (int y = 0; y < height; y++) {
        // 更新下一行
        if (y < height - 1) {
            unsigned char* src_next = blurred_img + (y + 1) * stride;
            next_line[0] = src_next[0];
            memcpy(&next_line[1], src_next, width);
            next_line[width + 1] = src_next[width - 1];
        } else {
            // 最后一行：镜像处理
            memcpy(next_line, curr_line, width + 2);
        }
        
        float* grad_row = &gradient_mag[y * width];
        
        // SIMD处理内部像素
        int x = 1;
        for (; x <= width - 8; x += 8) {
            // 加载3x3窗口
            __m128i top = _mm_loadl_epi64((__m128i*)&prev_line[x-1]);
            __m128i mid = _mm_loadl_epi64((__m128i*)&curr_line[x-1]);
            __m128i bot = _mm_loadl_epi64((__m128i*)&next_line[x-1]);
            
            // 扩展为16位
            __m128i top_16 = _mm_unpacklo_epi8(top, _mm_setzero_si128());
            __m128i mid_16 = _mm_unpacklo_epi8(mid, _mm_setzero_si128());
            __m128i bot_16 = _mm_unpacklo_epi8(bot, _mm_setzero_si128());
            
            // Sobel X: [-1 0 1; -2 0 2; -1 0 1]
            __m128i gx_left = _mm_add_epi16(_mm_add_epi16(top_16, _mm_slli_epi16(mid_16, 1)), bot_16);
            __m128i gx_right = _mm_srli_si128(gx_left, 2);  // 向右移动1个像素
            
            __m128i gx = _mm_sub_epi16(gx_right, gx_left);
            
            // Sobel Y: [-1 -2 -1; 0 0 0; 1 2 1]
            __m128i gy_top = _mm_add_epi16(_mm_slli_epi16(top_16, 1), 
                                          _mm_add_epi16(_mm_srli_si128(top_16, 1), 
                                                       _mm_srli_si128(top_16, 2)));
            __m128i gy_bot = _mm_add_epi16(_mm_slli_epi16(bot_16, 1),
                                          _mm_add_epi16(_mm_srli_si128(bot_16, 1),
                                                       _mm_srli_si128(bot_16, 2)));
            
            __m128i gy = _mm_sub_epi16(gy_bot, gy_top);
            
            // 转换为浮点数并计算幅值
            __m128 gx_f = _mm_cvtepi32_ps(_mm_srai_epi32(_mm_unpacklo_epi16(gx, gx), 0));
            __m128 gy_f = _mm_cvtepi32_ps(_mm_srai_epi32(_mm_unpacklo_epi16(gy, gy), 0));
            
            __m128 mag = _mm_sqrt_ps(_mm_add_ps(_mm_mul_ps(gx_f, gx_f), _mm_mul_ps(gy_f, gy_f)));
            
            // 存储结果
            _mm_storeu_ps(&grad_row[x], mag);
        }
        
        // 处理剩余像素
        for (; x < width - 1; x++) {
            int gx = (prev_line[x+1] + 2*curr_line[x+1] + next_line[x+1]) -
                     (prev_line[x-1] + 2*curr_line[x-1] + next_line[x-1]);
            
            int gy = (next_line[x-1] + 2*next_line[x] + next_line[x+1]) -
                     (prev_line[x-1] + 2*prev_line[x] + prev_line[x+1]);
            
            grad_row[x] = sqrtf(gx*gx + gy*gy) / 8.0f;
        }
        
        // 边界像素特殊处理
        if (y > 0) grad_row[0] = grad_row[1];
        if (y < height - 1) grad_row[width-1] = grad_row[width-2];
        
        // 滚动缓冲区
        unsigned char* temp = prev_line;
        prev_line = curr_line;
        curr_line = next_line;
        next_line = temp;
    }
}

/**
 * @brief 亚像素边缘点提取
 * 
 * @param blurred_image 高斯模糊后的图像
 * @param edge_map Canny边缘检测结果（0xFF=边缘）
 * @param width 图像宽度
 * @param height 图像高度
 * @param stride 图像行跨度
 * @param edge_points 输出：亚像素边缘点列表
 * @return int 错误码
 */
int ExtractSubpixelEdgePoints(
    unsigned char* blurred_image,
    unsigned char* edge_map,
    int width,
    int height,
    int stride,
    std::vector<EdgePoint>& edge_points) {
    
    // 参数检查
    if (!blurred_image || !edge_map) return 3;
    if (width <= 2 || height <= 2 || stride < width) return 1;
    
    edge_points.clear();
    
    // 1. 计算梯度幅值图像
    std::vector<float> gradient_mag;
    ComputeGradientMagnitudeSSE(blurred_image, width, height, stride, gradient_mag);
    
    // 2. 统计边缘点数（预分配内存）
    int edge_count = 0;
    for (int y = 0; y < height; y++) {
        unsigned char* edge_row = edge_map + y * stride;
        for (int x = 0; x < width; x++) {
            if (edge_row[x] == 0xFF) edge_count++;
        }
    }
    
    if (edge_count == 0) return 7;
    
    edge_points.reserve(edge_count);
    
    // 3. 提取亚像素边缘点
    for (int y = 1; y < height - 1; y++) {
        unsigned char* edge_row = edge_map + y * stride;
        
        for (int x = 1; x < width - 1; x++) {
            if (edge_row[x] == 0xFF) {
                // 计算亚像素偏移
                float dx, dy;
                if (!FitQuadraticSurface(gradient_mag.data(), width, x, y, dx, dy)) {
                    continue;
                }
                
                // 计算梯度方向
                float gx, gy, mag;
                ComputeGradientAtPoint(blurred_image, stride, x, y, gx, gy, mag);
                
                if (mag < 1e-6f) continue;  // 忽略梯度太小的点
                
                // 计算角度（边缘法线方向）
                // 注意：边缘方向与梯度方向垂直
                float edge_angle = atan2f(-gx, gy);  // 旋转90度
                
                // 归一化到[0, 2π)
                if (edge_angle < 0) edge_angle += 2.0f * M_PI;
                
                // 创建特征点
                EdgePoint point;
                point.x = x + dx;
                point.y = y + dy;
                point.angle = edge_angle;
                point.magnitude = mag;
                
                edge_points.push_back(point);
            }
        }
    }
    
    return 0;
}

// 简化的内存布局版本（更接近原代码）
struct EdgeFeatureArrays {
    std::vector<float> directions;  // 边缘方向
    std::vector<float> positions_x; // X坐标
    std::vector<float> positions_y; // Y坐标
    std::vector<float> magnitudes;  // 梯度幅值
};

int ExtractEdgeFeaturesToArrays(
    unsigned char* blurred_image,
    unsigned char* edge_map,
    int width,
    int height,
    int stride,
    EdgeFeatureArrays& features) {
    
    features.directions.clear();
    features.positions_x.clear();
    features.positions_y.clear();
    features.magnitudes.clear();
    
    // 计算梯度幅值
    std::vector<float> gradient_mag(width * height);
    
    // 简化的梯度计算（非SIMD）
    for (int y = 1; y < height - 1; y++) {
        for (int x = 1; x < width - 1; x++) {
            float gx, gy;
            ComputeGradientAtPoint(blurred_image, stride, x, y, gx, gy, gradient_mag[y * width + x]);
        }
    }
    
    // 提取特征点
    for (int y = 1; y < height - 1; y++) {
        unsigned char* edge_row = edge_map + y * stride;
        
        for (int x = 1; x < width - 1; x++) {
            if (edge_row[x] == 0xFF) {
                float dx, dy;
                if (!FitQuadraticSurface(gradient_mag.data(), width, x, y, dx, dy)) {
                    continue;
                }
                
                // 计算梯度
                float gx, gy, mag;
                ComputeGradientAtPoint(blurred_image, stride, x, y, gx, gy, mag);
                
                if (mag < 1e-6f) continue;
                
                // 存储到数组
                features.positions_x.push_back(x + dx);
                features.positions_y.push_back(y + dy);
                features.directions.push_back(atan2f(-gx, gy));
                features.magnitudes.push_back(mag);
            }
        }
    }
    
    return features.positions_x.empty() ? 7 : 0;
}