corner_localization.m

摄像机标定方法张正友方法中图像坐标的提取。一种全新的方法

%************************************************************************
%   函数名：corner_localization
%   描述：  该函数对一个初始角点进行精确定位
%   输入：  
%        1 梯度图      im_dx， im_dy
%        2 初始位置    position
%   输出：角点的精确位置
%   作者：zhwang
%   日期：06.11.16
%   调试：N
%************************************************************************
function  pos = corner_localization(im_dx,im_dy,im_mag, position,nR); 

    %*******************************************************************
    %                        初始准备
    %*******************************************************************
    %返回值
    pos = position;
    
    %验证初始位置是否太靠边
    nN = 2*nR+1;
    [r,c] = size(im_dx);
    rr = position(1,1);
    cc = position(1,2);
    if rr < nR+1 | rr > r-nR | cc < nR+1 | cc > c-nR
        return;
    end
    
    %取出附近梯度
    small_dx = im_dx(rr-nR:rr+nR,cc-nR:cc+nR);
    small_dy = im_dy(rr-nR:rr+nR,cc-nR:cc+nR);
    small_mag = im_mag(rr-nR:rr+nR,cc-nR:cc+nR);
    
    %*******************************************************************
    %                        加权梯度
    %*******************************************************************
    
%     figure(1);
%     imshow(uint8(small_mag));
    
    T_avg = 1.2*mean2(small_mag);
    small_mag(find(small_mag < T_avg)) = 0;
    T_avg2 = mean(small_mag(find(small_mag > T_avg)));
    
    small_mag = small_mag./T_avg;
    index = find(small_mag == 0);
    a = 1.8;
    k = 0.9;
    beta = log((a-1)/((1-k)*a));
    small_mag = a-(a-1)*exp(beta*(1-small_mag));
    small_mag(index) = 0;
    [index_rr,index_cc] = find(small_mag > 0);
    
%     figure(2);
%     imshow(uint8(small_mag*100));

    %*******************************************************************
    %                        计算每个点形成的直线
    %*******************************************************************
    L = zeros(size(index_rr,1),5);
    for i = 1:size(index_rr,1)
        px = index_rr(i,1);
        py = index_cc(i,1);
        dy = small_dx(px,py);
        dx = small_dy(px,py);
        line = [dx,dy,-py*dy-dx*px,px,py];
        L(i,:) = line;
    end
    %*******************************************************************
    %                       像素级粗定位
    %*******************************************************************
    matrix_add = zeros(nN,nN);
    for i = 1:size(index_rr,1)
        line = L(i,1:3);
        mag_line = small_mag(L(i,4),L(i,5));
        value_norm = sqrt(line(1,1)*line(1,1) + line(1,2)*line(1,2));
 
        %投票 只对中间的5×5个点进行投票
        ttt = 5;
        for v_rr = nR+1-ttt:nR+1+ttt
            for v_cc = nR-1-ttt:nR+1+ttt
                P_3 = [v_rr,v_cc,1];
                distance = abs(dot(P_3,line))/value_norm;
                if distance < 1
                    matrix_add(v_rr,v_cc) = matrix_add(v_rr,v_cc) + (1-distance) * mag_line;
                end
            end
        end
    end
    
%     figure(3);
%     imshow(uint8(matrix_add/max(matrix_add(:))*200));
    
    max_value = max(matrix_add(:));
    [px_raw,py_raw] = find(matrix_add == max_value);
    C_raw = [px_raw(1),py_raw(1)];

    %*******************************************************************
    %                       亚素级精定位
    %*******************************************************************
    %step1: 求出支撑像素,并构造矩阵
    D_thresh = 1.0;
    A = zeros(1,3);
    count = 1;
    C_raw_3 = [C_raw(1,1),C_raw(1,2),1];
    for i = 1:size(L,1)
        line = L(i,1:3);
        P = L(i,4:5);
        
        %离角点太近的不要 3个像素
        D_T = 3;
        if norm(P-C_raw) > D_T
            continue;
        end
        
        dx = line(1,1);
        dy = line(1,2);
        distance = abs(dot(line,C_raw_3)) / sqrt(dy*dy+dx*dx);
        if distance < D_thresh
            line = line/norm(line);
            line = line*small_mag(P(1,1),P(1,2));
            A(count,:) = line;
            count = count + 1;
        end
    end
    
    %step3: 解方程Ax = 0,得到精确位置
    hh = GetLineFromPoints_Al(A);
    distance = norm(hh-C_raw_3');
    if distance > 1
        hh = C_raw_3';
    end
    pos = position + [hh(1,1),hh(2,1)] - [nR+1,nR+1];
    %*******************************************************************
    %                       End
    %*******************************************************************
    
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   GetLineFromPoints 最小二乘拟合直线（几何距离最小）
%
%   输入：点的齐次坐标矩阵（每一行表示一个点的齐次坐标）
%   输出：直线的齐次坐标（列向量）
%   调试：Y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function result = GetLineFromPoints_Gem(data);

    result = zeros(3,1);

    %求出质心
    center = sum(data)/size(data,1);

    %合成矩阵
    for i = 1:size(data)
        data(i,:) = data(i,:) - center;
    end

    %奇异值分解求a,b
    data_m = data'*data;
    [u,d,v] = svd(data_m);
    ab = v(:,2);

    %根据a,b 质心求直线方程
    c = -ab(1,1) * center(1,1) - ab(2,1) * center(1,2);

    result(1,1) = ab(1,1);
    result(2,1) = ab(2,1);
    result(3,1) = c;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   GetLineFromPoints 最小二乘拟合直线（代数距离最小）
%
%   输入： 数据 data（齐次坐标）
%          coe 选取一定比例的点进行优化
%          bFlag 优化标记 取 0不优化 1 优化
%   输出： 直线的齐次坐标（列向量）
%   调试： Y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function result = GetLineFromPoints_Al(data);

    result = zeros(3,1);
    [u,d,v] = svd(data);
    L = v(:,3);
    result = L/L(3,1);