forstner角点检测

clc;
clear;
I=imread（‘1.jpg‘）;    
[mn]=size（I）;   %宽m，长n
% 利用差分算子（像素（cr）在上下左右四个方向的灰度差分绝对值）提取初选点。
g=double（I）;
T=30;               %阈值
Initpointcoord=[];   %初选点坐标
for i=2:m-1
    for j=2:n-1
        dg1=abs（g（ij）-g（i-1j））;   %上侧
        dg2=abs（g（ij）-g（i+1j））;   %下侧
        dg3=abs（g（ij）-g（ij-1））;   %左侧
        dg4=abs（g（ij）-g（ij+1））;   %右侧
        dg=[dg1 dg2 dg3 dg4];
        temp=sort（dg）;
        if temp（3）>T
            Initpointcoord=[Initpointcoord;i j];
        end
    end
end
%  以初选点（cr）为中心的w*w窗口中，计算协方差矩阵N与误差椭圆的圆度q确定备选点。
w=3;                          %窗口宽的值
Initlen=length（Initpointcoord）;   %初选点个数
k=floor（w/2）;
Tq=0.51;                       %阈值Tq
candidatepoint=[];             %候选点
for no=1:Initlen
    Initpoint=Initpointcoord（no:）;
    c=Initpoint（1）;
    r=Initpoint（2）;
    h=c-k;
    l=c+k-1;
    p=r-k;
    w=r+k-1;
    gu2=0;gv2=0;guv=0;
    for e=h:l
        for f=p:w
            gu2=gu2+（g（e+1f+1）-g（ef））^2;
            gv2=gv2+（g（ef+1）-g（e+1f））^2;
            guv=guv+（g（e+1f+1）-g（ef））*（g（ef+1）-g（e+1f））;
        end
    end
    DetN=gu2*gv2-guv^2;
    trN=gu2+gv2;
    q=4*DetN/（trN*trN）;
    if q>Tq
        candidatepoint=[candidatepoint;c r];
    end
end
clear h p q j c r
w2=10;
cadipotlen=length（candidatepoint）;
k2=floor（5/w2）;
featurepoint=[];
h=floor（m/w2）;   %窗口行数
d=floor（n/w2）;   %窗口列数
for p=1:h
    for q=1:d
        tp=（p-1）*w2;
        tq=（q