cp_cornerDetection.m的学习记录

function [curve,curve_start,curve_end,curve_mode,cur_num,edge_map]=extract_curve(BW,Gap_size)

%   Function to extract curves from binary edge map, if the endpoint of a
%   contour is nearly connected to another endpoint, fill the gap and continue
%   the extraction. The default gap size is 1 pixles.

[L,W]=size(BW);
BW1=zeros(L+2*Gap_size,W+2*Gap_size);
BW_edge=zeros(L,W);
BW1(Gap_size+1:Gap_size+L,Gap_size+1:Gap_size+W)=BW;
[r,c]=find(BW1==1);
cur_num=0;
% 提取边缘图中所有的连续轮廓并存为元胞数组：
while size(r,1)>0
    point=[r(1),c(1)];
    cur=point;
    BW1(point(1),point(2))=0;
    [I,J]=find(BW1(point(1)-Gap_size:point(1)+Gap_size,point(2)-Gap_size:point(2)+Gap_size)==1);
    while size(I,1)>0
        dist=(I-Gap_size-1).^2+(J-Gap_size-1).^2;
        [~,index]=min(dist);
        point=point+[I(index),J(index)]-Gap_size-1; % find continous point
        cur=[cur;point];
        BW1(point(1),point(2))=0;
        [I,J]=find(BW1(point(1)-Gap_size:point(1)+Gap_size,point(2)-Gap_size:point(2)+Gap_size)==1);
    end

    % Extract edge towards another direction
    point=[r(1),c(1)];
    BW1(point(1),point(2))=0;
    [I,J]=find(BW1(point(1)-Gap_size:point(1)+Gap_size,point(2)-Gap_size:point(2)+Gap_size)==1);
    while size(I,1)>0
        dist=(I-Gap_size-1).^2+(J-Gap_size-1).^2;
        [~,index]=min(dist);
        point=point+[I(index),J(index)]-Gap_size-1;
        cur=[point;cur];
        BW1(point(1),point(2))=0;
        [I,J]=find(BW1(point(1)-Gap_size:point(1)+Gap_size,point(2)-Gap_size:point(2)+Gap_size)==1);
    end

    if size(cur,1)>(size(BW,1)+size(BW,2))/60
        cur_num=cur_num+1;
        curve{cur_num}=cur-Gap_size;
    end
    [r,c]=find(BW1==1);

end
% 将轮廓元胞数组中的起始点存起来并判定是否为闭合轮廓
for i=1:cur_num
    curve_start(i,:)=curve{i}(1,:);
    curve_end(i,:)=curve{i}(size(curve{i},1),:);
    if (curve_start(i,1)-curve_end(i,1))^2+...
        (curve_start(i,2)-curve_end(i,2))^2 <= 4 %32
        curve_mode(i,:)='loop';
    else
        curve_mode(i,:)='line';
    end

    BW_edge(curve{i}(:,1)+(curve{i}(:,2)-1)*L)=1; % 还原边缘图像
end
edge_map = BW_edge;

其中这段代码主要就是将点BW1中等于1的点的位置（r,c）取出，赋值给point，并且将point赋值给cur数组，以便后续进行保存。

进入while循环之后，是值为1的点进行读取赋值给point，记此点为A点，之后将A点赋值为0，然后在A点的8邻域+A点寻找值为1的点，记为B点，并将B点在此邻域的位置赋值给[I,J]，之后再进入while循环计算A与B的欧式距离，选择最小的距离复赋值给dist，再计算出B点在整个平面上位置，之所以要减去-Gap_size-1，是因为在计算相对位置是加入了相对于邻域内相对于中心点的偏移量。假如point为[53,3]，在此点的8邻域内找到（2，3）此点的值为1，减去偏移量就是[0,1]，则将此点映射到全图上的位置就是[53,4]，然后继续循环寻找。

while size(r,1)>0
    point=[r(1),c(1)];
    cur=point;
    BW1(point(1),point(2))=0;
    [I,J]=find(BW1(point(1)-Gap_size:point(1)+Gap_size,point(2)-Gap_size:point(2)+Gap_size)==1);
    while size(I,1)>0
        dist=(I-Gap_size-1).^2+(J-Gap_size-1).^2;
        [~,index]=min(dist);
        point=point+[I(index),J(index)]-Gap_size-1; % find continous point
        cur=[cur;point];
        BW1(point(1),point(2))=0;
        [I,J]=find(BW1(point(1)-Gap_size:point(1)+Gap_size,point(2)-Gap_size:point(2)+Gap_size)==1);
    end

下面这段代码的作用是检查当前的矩阵 cur 是否包含足够数量的点，以被视为一个曲线。具体来说，如果当前矩阵的行数大于二值图像 BW 的行数和列数之和的 1/60（即1.67%），则判定这个矩阵表示的是一条曲线，将其保存并统计曲线数量。

具体实现上，程序首先计算出阈值 size(BW,1)+size(BW,2))/60，即满足判定条件的矩阵大小下限，然后判断当前矩阵 cur 的行数是否大于该值。若是，则将 cur 从中心位置偏移 Gap_size 个单位，并将其保存到 curve 数组中的第 cur_num + 1 个元素中，并将 cur_num 累加 1，以便保存下一个曲线。

综上所述，这段代码的最终目的是找到连续的点，将它们组合成为一条曲线，并将所有曲线存储在一个名为 curve 的结构体数组中。

    if size(cur,1)>(size(BW,1)+size(BW,2))/60
        cur_num=cur_num+1;
        curve{cur_num}=cur-Gap_size;
    end

这段代码首先遍历存储着所有曲线的结构体数组 curve，并将每条曲线的起始点和结束点提取出来存储在 curve_start 和 curve_end 数组中，同时判断曲线是否为闭合轮廓。

具体来说，curve_start(i,:) 表示第 i 条曲线的起始点坐标，curve_end(i,:) 表示该曲线的结束点坐标。如果该条曲线是一个闭合的轮廓（即该曲线的起始点和结束点相近），则将 curve_mode(i,:) 的值定义为 "loop"，否则将其定义为 "line"。

接下来的代码将 BW_edge 中表示边缘的像素点重新还原出来（设置为 1），以便后续的处理。

最后，将还原的边缘图像 BW_edge 赋值给 edge_map，作为函数的返回值。

综上所述，这段代码的主要作用是对曲线进行处理并将结果保存到对应的变量中，以得到一张表示图像边缘的二值图像。

% 将轮廓元胞数组中的起始点存起来并判定是否为闭合轮廓
for i=1:cur_num
    curve_start(i,:)=curve{i}(1,:);
    curve_end(i,:)=curve{i}(size(curve{i},1),:);
    if (curve_start(i,1)-curve_end(i,1))^2+...
        (curve_start(i,2)-curve_end(i,2))^2 <= 4 %32
        curve_mode(i,:)='loop';
    else
        curve_mode(i,:)='line';
    end

    BW_edge(curve{i}(:,1)+(curve{i}(:,2)-1)*L)=1; % 还原边缘图像
end
edge_map = BW_edge;

BW_edge(curve{i}(:,1)+(curve{i}(:,2)-1)*L)=1; % 还原边缘图像

这段代码用于将 curve 结构体数组中存储的轮廓点还原为之前边缘检测得到的二值图像 BW_edge。

具体来说，curve{i}(:,1) 表示第 i 条轮廓曲线所有点的横坐标，curve{i}(:,2) 则表示这些点的纵坐标。因为 BW_edge 是一个二维的矩阵，需要将这些轮廓点的坐标转换成一维索引才能在 BW_edge 中修改对应像素值，此处使用了类似于线性索引的方法，即将每个点的坐标转换成一维线性的索引号 (curve{i}(:,1)+(curve{i}(:,2)-1)*L)。其中，L 表示图像 I 的列数，由此可以计算出该点在 BW_edge 中的位置，并将其设置为 1，表示为图像边缘。

综上所述，是利用列扫描法从上往下一列一列进行置1

function [cout,angle]=get_corner(curve,curve_start,curve_end,curve_mode,curve_num,BW,sig,Endpoint,C,T_angle,maxlength,rflag)

corner_num=0;
cout=[];
angle=[];

GaussianDieOff = .0001; 
pw = 1:30; 
ssq = sig*sig;
width = max( find( exp(-(pw.*pw)/(2*ssq)) > GaussianDieOff) );
if isempty(width)
    width = 1;  
end
t = (-width:width); %高斯窗 width=12
gau = exp(-(t.*t)/(2*ssq)); 
gau=gau/sum(gau);
sigmaLs =  maxlength;
warning off
% beforegaus = zeros(size(BW));
% aftergaus = zeros(size(BW));
parfor i=1:curve_num
    x=curve{i}(:,1);
    y=curve{i}(:,2);
%     for k=1:length(x)
%        beforegaus(x(k),y(k)) = 1; 
%     end
    W=width;
    L=length(x);
    if L>W
        % Calculate curvature
        if curve_mode(i,:)=='loop'
            xL=[x(L-W+1:L);x;x(1:W)];
            yL=[y(L-W+1:L);y;y(1:W)];
        else
            xL=[ones(W,1)*2*x(1)-x(W+1:-1:2);x;ones(W,1)*2*x(L)-x(L-1:-1:L-W)];% 总长度2w+L
            yL=[ones(W,1)*2*y(1)-y(W+1:-1:2);y;ones(W,1)*2*y(L)-y(L-1:-1:L-W)];
        end
       
        xx=conv(xL,gau);
        xx=xx(W+1:L+3*W);
        yy=conv(yL,gau);
        yy=yy(W+1:L+3*W);
        % aftergaus(:)=1; % show curves
        % for k=width:length(x)-width+1
        %     if xx(k) < 1 || yy(k) < 1
        %         continue;
        %     end
        %    aftergaus(round(xx(k)),round(yy(k))) = 0; 
        % end
        % figure,imshow(~aftergaus);
        Xu=[xx(2)-xx(1) ; (xx(3:L+2*W)-xx(1:L+2*W-2))/2 ; xx(L+2*W)-xx(L+2*W-1)];
        Yu=[yy(2)-yy(1) ; (yy(3:L+2*W)-yy(1:L+2*W-2))/2 ; yy(L+2*W)-yy(L+2*W-1)];
        Xuu=[Xu(2)-Xu(1) ; (Xu(3:L+2*W)-Xu(1:L+2*W-2))/2 ; Xu(L+2*W)-Xu(L+2*W-1)];
        Yuu=[Yu(2)-Yu(1) ; (Yu(3:L+2*W)-Yu(1:L+2*W-2))/2 ; Yu(L+2*W)-Yu(L+2*W-1)];
        K=abs((Xu.*Yuu-Xuu.*Yu)./((Xu.*Xu+Yu.*Yu).^1.5));
        K=ceil(K*100)/100;
               
        % Find curvature local maxima as corner candidates 寻找的是波峰和波谷等极值点
        extremum=[];
        N=size(K,1);
        n=0;
        Search=1;
        % 寻找曲率极大值极小值并存入数组中，奇数点为极小值，偶数点为极大值
        for j=1:N-1
            if (K(j+1)-K(j))*Search>0
                n=n+1;
                extremum(n)=j;  % In extremum, odd points is minima and even points is maxima
                Search=-Search;
            end
        end
        if mod(size(extremum,2),2)==0
            n=n+1;
            extremum(n)=N;
        end
    
        n=size(extremum,2);
        flag=ones(size(extremum));
        lambda_LR=[];
        % Compare with adaptive local threshold to remove round corners
        for k=2:2:n
            [~,index1]=min(K(extremum(k):-1:extremum(k-1)));
            [~,index2]=min(K(extremum(k):extremum(k+1)));
            ROS=K(extremum(k)-index1+1:extremum(k)+index2-1);
            K_thre = C*mean(ROS);
            if K(extremum(k))<K_thre
                flag(k)=0;
            else
                lambda_LR=[lambda_LR; extremum(k) index1 index2];
            end
        end
        extremum=extremum(2:2:n);
        flag=flag(2:2:n);
        extremum=extremum(find(flag==1));
        
        % Check corner angle to remove false corners due to boundary noise and trivial details
        smoothed_curve=[xx,yy];
        n=size(extremum,2);
        flag=ones(size(extremum)); 
        for j=1:n
            if j==1 & j==n
                ang=curve_tangent(smoothed_curve(1:L+2*W,:),extremum(j));
            elseif j==1 
                ang=curve_tangent(smoothed_curve(1:extremum(j+1),:),extremum(j));
            elseif j==n
                ang=curve_tangent(smoothed_curve(extremum(j-1):L+2*W,:),extremum(j)-extremum(j-1)+1);
            else
                ang=curve_tangent(smoothed_curve(extremum(j-1):extremum(j+1),:),extremum(j)-extremum(j-1)+1);
            end     
            if ang>T_angle & ang<(360-T_angle)
                flag(j)=0;  
            end
        end
        extremum=extremum(find(flag~=0));
        lambda_LR=lambda_LR(find(flag~=0),:);
            
        extremum=extremum-W;
        true_corner = find(extremum>0 & extremum<=L);
        extremum=extremum(true_corner);
        lambda_LR = lambda_LR(true_corner,:);
        n=size(extremum,2);     
        for j=1:n     
            cout = [cout; curve{i}(extremum(j),:)];
            if rflag                
%**********    CAO: adaptive  parameters algorithm  *********% 
                xcor = curve{i}(extremum(j),2);
                ycor = curve{i}(extremum(j),1);
                retail = size(curve{i},1);
                lengthL = min([lambda_LR(j,2) extremum(j)]);            
                lengthR = min([lambda_LR(j,3) retail-extremum(j)+1]);
                coefL = exp(-((0:1:lengthL-1) / lengthL).^2/2);
                coefL = coefL / sum(coefL);
                coefR = exp(-((lengthR-1:-1:0) / lengthR).^2/2);
                coefR = coefR / sum(coefR);
                xL= sum(coefL' .* curve{i}(extremum(j)+1-lengthL:extremum(j),2));
                yL= sum(coefL' .* curve{i}(extremum(j)+1-lengthL:extremum(j),1));
                xR = sum(coefR' .* curve{i}(extremum(j):extremum(j)+lengthR-1,2));
                yR = sum(coefR' .* curve{i}(extremum(j):extremum(j)+lengthR-1,1));
                vL = [xL-xcor yL-ycor]; vR = [xR-xcor yR-ycor];
                vm = vL / norm(vL) + vR / norm(vR);
%*************   Assign Main Orientation  **********%
                deltax = vm(1);
                deltay = vm(2);
                if isnan(atan(deltay/deltax)) % no solution
                    orientation1 = 0;
                elseif deltay >= 0 & deltax >= 0 % quadrant I
                    orientation1 = atan(deltay / deltax);
                elseif deltay < 0 & deltax >= 0  % quadrant IV
                    orientation1 = atan(deltay / deltax) + 2*pi;
                elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                    orientation1 = atan(deltay / deltax) + pi;
                end
                angle = [angle; orientation1];
            end
        end
    end
end
% figure,subplot(121),imshow(~beforegaus);subplot(122),imshow(~aftergaus);

% Add Endpoints
if Endpoint
    for i=1:curve_num
        retail = size(curve{i},1);
        if retail>0 & curve_mode(i,:)=='line'
            % Start point compare with detected corners
            compare_corner=cout-ones(size(cout,1),1)*curve_start(i,:);
            compare_corner=compare_corner.^2;
            compare_corner=compare_corner(:,1)+compare_corner(:,2);
            if min(compare_corner)>25       % Add end points far from detected corners 
                cout = [cout; curve_start(i,:)];
                if rflag
                    xx = curve{i}(1,2);
                    yy = curve{i}(1,1);
                    coef2 = exp(-((min(retail,maxlength)-1:-1:0) / sigmaLs).^2/2);
                    coef2 = coef2 / sum(coef2);
                    xL = sum(coef2' .*curve{i}(1:min(retail,maxlength),2));
                    yL = sum(coef2' .*curve{i}(1:min(retail,maxlength),1));
                    deltax = xL - xx;
                    deltay = yL - yy;
                    if isnan(atan(deltay/deltax)) % no solution
                        orientation2 = 0;
                    elseif deltay >= 0 & deltax >= 0 % quadrant I
                        orientation2 = atan(deltay / deltax);
                    elseif deltay < 0 & deltax >= 0  % quadrant IV
                        orientation2 = atan(deltay / deltax) + 2*pi;
                    elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                        orientation2 = atan(deltay / deltax) + pi;
                    end
                    angle = [angle;orientation2];
                end
            end
            
            % End point compare with detected corners
            compare_corner=cout-ones(size(cout,1),1)*curve_end(i,:);
            compare_corner=compare_corner.^2;
            compare_corner=compare_corner(:,1)+compare_corner(:,2);
            if min(compare_corner)>25
                cout = [cout; curve_end(i,:)]; 

                if rflag
                    xx = curve{i}(end,2);
                    yy = curve{i}(end,1);
                    coef1 = exp(-((0:1:retail-max(1,retail-maxlength+1)) / sigmaLs).^2/2);
                    coef1 = coef1 / sum(coef1);
                    xL = sum(coef1' .*curve{i}(max(1,retail-maxlength+1):retail,2));
                    yL = sum(coef1' .*curve{i}(max(1,retail-maxlength+1):retail,1));
                    deltax = xL - xx;
                    deltay = yL - yy;
                    if isnan(atan(deltay/deltax)) % no solution
                        orientation3 = 0;
                    elseif deltay >= 0 & deltax >= 0 % quadrant I
                        orientation3 = atan(deltay / deltax);
                    elseif deltay < 0 & deltax >= 0  % quadrant IV
                        orientation3 = atan(deltay / deltax) + 2*pi;
                    elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                        orientation3 = atan(deltay / deltax) + pi;
                    end
                    angle = [angle;orientation3];
                end
            end
        end
    end
end

下面这段代码定义了高斯核，用于平滑曲线，具体高斯窗12*12大小，标准差为3，定义了GaussianDieOff便于舍弃太小的的值

corner_num=0;
cout=[];
angle=[];

GaussianDieOff = .0001; 
pw = 1:30; 
ssq = sig*sig;
width = max( find( exp(-(pw.*pw)/(2*ssq)) > GaussianDieOff) );
if isempty(width)
    width = 1;  
end
t = (-width:width); %高斯窗 width=12
gau = exp(-(t.*t)/(2*ssq)); 
gau=gau/sum(gau);
sigmaLs =  maxlength;
warning off

下面这段代码用于根据曲线类型（闭合或非闭合）对空间坐标进行调整。

如果曲线类型为 loop，即闭合曲线，将曲线的前 W 个点复制到末尾，并将曲线的后 W 个点复制到开头，使其形成一个环状结构。这样做是为了在计算曲率时能够将曲线首尾相接，避免出现曲线断裂的情况。

如果曲线类型为其他值，即非闭合曲线，将曲线前 W 个点关于第一个点对称，得到前补点；将曲线末尾 W 个点关于最后一个点对称，得到后补点。然后将曲线、前补点和后补点按顺序连接起来，组成一个总长度为 2W+L 的新曲线。这样做是为了在曲线两端计算曲率时可以得到平滑的曲率值。

综上所述，该段代码的主要作用是调整曲线上的坐标，以便在计算曲率时得到更加准确的结果。

        if curve_mode(i,:)=='loop'
            xL=[x(L-W+1:L);x;x(1:W)];
            yL=[y(L-W+1:L);y;y(1:W)];
        else
            xL=[ones(W,1)*2*x(1)-x(W+1:-1:2);x;ones(W,1)*2*x(L)-x(L-1:-1:L-W)];% 总长度2w+L
            yL=[ones(W,1)*2*y(1)-y(W+1:-1:2);y;ones(W,1)*2*y(L)-y(L-1:-1:L-W)];
        end

而这段代码是为了计算曲率做准备，具体使用一阶二阶导数的中心差分法进行计算，具体算法参考

数值微分|中心差分法(Central Finite Difference Approximations) - 腾讯云开发者社区-腾讯云 (tencent.com)

        % 一阶导与二阶导使用中心差分法
        Xu=[xx(2)-xx(1) ; (xx(3:L+2*W)-xx(1:L+2*W-2))/2 ; xx(L+2*W)-xx(L+2*W-1)];
        Yu=[yy(2)-yy(1) ; (yy(3:L+2*W)-yy(1:L+2*W-2))/2 ; yy(L+2*W)-yy(L+2*W-1)];
        Xuu=[Xu(2)-Xu(1) ; (Xu(3:L+2*W)-Xu(1:L+2*W-2))/2 ; Xu(L+2*W)-Xu(L+2*W-1)];
        Yuu=[Yu(2)-Yu(1) ; (Yu(3:L+2*W)-Yu(1:L+2*W-2))/2 ; Yu(L+2*W)-Yu(L+2*W-1)];
        K=abs((Xu.*Yuu-Xuu.*Yu)./((Xu.*Xu+Yu.*Yu).^1.5));
        K=ceil(K*100)/100;

下面这段代码的主要作用是在曲线上寻找拐角点，其中使用了一种比较特殊的方法，即通过寻找曲率的局部极值点来定位拐角。

具体实现上，该段代码首先利用之前计算出的曲率值 K，在其中找到所有的局部极值点。具体来说，当相邻两个曲率值的差积大于0时，说明当前位置是一个极值点，这个点的位置会被记录在 extremum 数组里面。经过上述操作后，extremum 数组里面就存储了曲线上所有的极值点。

接下来，该段代码将极值点按照奇偶性进行划分，其中奇数点被认为是曲线上的极小值点，偶数点则是极大值点。此外，该段代码还做了一个特殊处理，即当极值点数量为偶数时，在 extremum 数组末尾添加一个虚拟极值点，这个点的位置等于曲线上最后一个点的位置。

        % 寻找曲率极大值极小值并存入数组中，奇数点为极小值，偶数点为极大值
        for j=1:N-1
            if (K(j+1)-K(j))*Search>0
                n=n+1;
                extremum(n)=j;  % In extremum, odd points is minima and even points is maxima
                Search=-Search;
            end
        end
        if mod(size(extremum,2),2)==0
            n=n+1;
            extremum(n)=N;
        end
    
        n=size(extremum,2);
        flag=ones(size(extremum));
        lambda_LR=[];

这段代码的主要作用是对曲线上的极大值点进行筛选，并将保留下来的拐角点存储在 lambda_LR 数组中；同时，也会将不合格的极大值点从 extremum 数组中剔除。

具体实现上，该段代码首先对每一对相邻的极大值点都进行处理。对于当前的极大值点，它会在极大值点两侧各取一段曲率值序列，然后计算这个序列的平均值，并用这个平均值乘以一个适当的系数作为阈值。如果当前极大值点的曲率值小于阈值，则说明该位置不是拐角，应该被剔除；否则，该位置则被认为是一个拐角点，可以进一步进行角度计算、路径规划等操作。

对于通过了筛选的拐角点，该段代码会将其位置信息和序列中局部最小值点的位置 index1 和 index2 存储在 lambda_LR 数组中。其中，序列 ROS 是将局部最小值点两侧的曲率值序列截取出来的结果，其目的是为了进一步对拐角进行精细化处理（这一部分的代码在该段代码之后）。

最后，该段代码将不合格的极大值点从 extremum 数组中删除，具体实现是通过以下几个步骤完成的：

1. 通过 extremum=extremum(2:2:n); 将所有奇数位置（即极小值点）从 extremum 数组中删除，只保留偶数位置的极大值点。
2. 通过 flag=flag(2:2:n); 将所有奇数位置对应的标志位从 flag 数组中删除，只保留偶数位置对应的标志位。
3. 通过 extremum=extremum(find(flag==1)); 将非拐角点对应的位置从 extremum 数组中删除，只保留拐角点对应的位置。

综上所述，该段代码的主要作用是对曲线上的极大值点进行筛选，剔除不符合条件的点，并将经过筛选的拐角点存储在 lambda_LR 数组中，以供后续处理使用。

 举了个例子，for循环里就是找到K中极大值左右最近的两个极小值点，并将他们保存到ROS中，之后进行取舍，取舍后存入lambda_LR数组中。

对于for循环外面的三行是

1。选出偶数索引（极大值的位置）

2。选出对应的flag位置

3。对extremum数组赋值，值为符合要求的极大值 

        for k=2:2:n
            [~,index1]=min(K(extremum(k):-1:extremum(k-1)));
            [~,index2]=min(K(extremum(k):extremum(k+1)));
            ROS=K(extremum(k)-index1+1:extremum(k)+index2-1);
            K_thre = C*mean(ROS);
            if K(extremum(k))<K_thre
                flag(k)=0;
            else
                lambda_LR=[lambda_LR; extremum(k) index1 index2];
            end
        end
        extremum=extremum(2:2:n);
        flag=flag(2:2:n);
        extremum=extremum(find(flag==1));

下面这个函数主要是利用极角的概念去结算γ1，γ2，如下图可视，将一个ROS曲线分为前后两端进行计算。

smoothed_curve=[xx,yy];存储了高斯滤波后的结果，形状为[2W+L,2]

先解释curve_tangent函数：

     smoothed_curve，和center分界点输入

        

function ang=curve_tangent(cur,center)
for i=1:2 % 对同一段曲线取前后两部分计算方向，分别为 i=1 和 i=2
    if i==1 % 前半部分
        curve=cur(center:-1:1,:);
    else % 后半部分
        curve=cur(center:size(cur,1),:);
    end
    L=size(curve,1);
    
    if L>3 % 当曲线点数大于3时
        if sum(curve(1,:)~=curve(L,:))~=0 % 如果首尾两点不重合，则以第一点、中间点和最后一点作为拟合圆的三个点
            M=ceil(L/2);
            x1=curve(1,1);
            y1=curve(1,2);
            x2=curve(M,1);
            y2=curve(M,2);
            x3=curve(L,1);
            y3=curve(L,2);
        else  % 否则以第一、1/3和2/3处的点作为拟合圆的三个点
            M1=ceil(L/3);
            M2=ceil(2*L/3);
            x1=curve(1,1);
            y1=curve(1,2);
            x2=curve(M1,1);
            y2=curve(M1,2);
            x3=curve(M2,1);
            y3=curve(M2,2);
        end
        
        if abs((x1-x2)*(y1-y3)-(x1-x3)*(y1-y2))<1e-8  % straight line
            tangent_direction=angle(complex(curve(L,1)-curve(1,1),curve(L,2)-curve(1,2)));
        else
            % 否则拟合出的圆的圆心即为当前位置处的切点，取圆心和相邻两点构成的直线的方向作为当前位置处的切线方向
            % 拟合圆
            % Fit a circle 
            x0 = 1/2*(-y1*x2^2+y3*x2^2-y3*y1^2-y3*x1^2-y2*y3^2+x3^2*y1+y2*y1^2-y2*x3^2-y2^2*y1+y2*x1^2+y3^2*y1+y2^2*y3)/(-y1*x2+y1*x3+y3*x2+x1*y2-x1*y3-x3*y2);
            y0 = -1/2*(x1^2*x2-x1^2*x3+y1^2*x2-y1^2*x3+x1*x3^2-x1*x2^2-x3^2*x2-y3^2*x2+x3*y2^2+x1*y3^2-x1*y2^2+x3*x2^2)/(-y1*x2+y1*x3+y3*x2+x1*y2-x1*y3-x3*y2);
            % R = (x0-x1)^2+(y0-y1)^2;

            radius_direction=angle(complex(x0-x1,y0-y1));
            adjacent_direction=angle(complex(x2-x1,y2-y1));
            tangent_direction=sign(sin(adjacent_direction-radius_direction))*pi/2+radius_direction;
        end
    
    else % very short line% 当曲线点数小于等于3时，直接取首尾两点计算切线方向
        tangent_direction=angle(complex(curve(L,1)-curve(1,1),curve(L,2)-curve(1,2)));
    end
    direction(i)=tangent_direction*180/pi; % 将弧度转换为角度
end
ang=abs(direction(1)-direction(2));% 返回前后两个位置的切线方向之差的绝对值（即夹角）

下面这个函数就是利用上面计算出来的角度，对角度进行判断是否为真角。

 

 代码中判断是否大于目标角，如果大于则将对于的极大值点处的标志位置为0。

        % Check corner angle to remove false corners due to boundary noise and trivial details
        smoothed_curve=[xx,yy];
        n=size(extremum,2);
        flag=ones(size(extremum)); 
        for j=1:n
            if j==1 & j==n
                ang=curve_tangent(smoothed_curve(1:L+2*W,:),extremum(j));
            elseif j==1 
                ang=curve_tangent(smoothed_curve(1:extremum(j+1),:),extremum(j));
            elseif j==n
                ang=curve_tangent(smoothed_curve(extremum(j-1):L+2*W,:),extremum(j)-extremum(j-1)+1);
            else
                ang=curve_tangent(smoothed_curve(extremum(j-1):extremum(j+1),:),extremum(j)-extremum(j-1)+1);
            end     
            if ang>T_angle & ang<(360-T_angle)
                flag(j)=0;  
            end
        end

        extremum=extremum(find(flag~=0));
        lambda_LR=lambda_LR(find(flag~=0),:);
            
        extremum=extremum-W;
        true_corner = find(extremum>0 & extremum<=L);
        extremum=extremum(true_corner);
        lambda_LR = lambda_LR(true_corner,:);
        n=size(extremum,2);     
        for j=1:n     
            cout = [cout; curve{i}(extremum(j),:)];
            if rflag                
%**********    CAO: adaptive  parameters algorithm  *********% 
                xcor = curve{i}(extremum(j),2);
                ycor = curve{i}(extremum(j),1);
                retail = size(curve{i},1);
                lengthL = min([lambda_LR(j,2) extremum(j)]);            
                lengthR = min([lambda_LR(j,3) retail-extremum(j)+1]);
                coefL = exp(-((0:1:lengthL-1) / lengthL).^2/2);
                coefL = coefL / sum(coefL);
                coefR = exp(-((lengthR-1:-1:0) / lengthR).^2/2);
                coefR = coefR / sum(coefR);
                xL= sum(coefL' .* curve{i}(extremum(j)+1-lengthL:extremum(j),2));
                yL= sum(coefL' .* curve{i}(extremum(j)+1-lengthL:extremum(j),1));
                xR = sum(coefR' .* curve{i}(extremum(j):extremum(j)+lengthR-1,2));
                yR = sum(coefR' .* curve{i}(extremum(j):extremum(j)+lengthR-1,1));
                vL = [xL-xcor yL-ycor]; vR = [xR-xcor yR-ycor];
                vm = vL / norm(vL) + vR / norm(vR);
%*************   Assign Main Orientation  **********%
                deltax = vm(1);
                deltay = vm(2);
                if isnan(atan(deltay/deltax)) % no solution
                    orientation1 = 0;
                elseif deltay >= 0 & deltax >= 0 % quadrant I
                    orientation1 = atan(deltay / deltax);
                elseif deltay < 0 & deltax >= 0  % quadrant IV
                    orientation1 = atan(deltay / deltax) + 2*pi;
                elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                    orientation1 = atan(deltay / deltax) + pi;
                end
                angle = [angle; orientation1];
            end
        end
    end
end

1，将上面flag中不为1的位置找到赋值给extremum，lambda_LR 

2，消除因为高斯滤波的偏差

3，找到真正的角，赋值给extremum，lambda_LR 

4，n 表示 extremum内元素的个数

        extremum=extremum(find(flag~=0));
        lambda_LR=lambda_LR(find(flag~=0),:);
            
        extremum=extremum-W;
        true_corner = find(extremum>0 & extremum<=L);
        extremum=extremum(true_corner);
        lambda_LR = lambda_LR(true_corner,:);
        n=size(extremum,2);  

转化到了空间坐标系中，

具体都在论文中体现

        for j=1:n     
            cout = [cout; curve{i}(extremum(j),:)];
            if rflag                
%**********    CAO: adaptive  parameters algorithm  *********% 
                xcor = curve{i}(extremum(j),2);
                ycor = curve{i}(extremum(j),1);
                retail = size(curve{i},1);
                lengthL = min([lambda_LR(j,2) extremum(j)]);            
                lengthR = min([lambda_LR(j,3) retail-extremum(j)+1]);
                % gauss core
                coefL = exp(-((0:1:lengthL-1) / lengthL).^2/2);
                coefL = coefL / sum(coefL);
                coefR = exp(-((lengthR-1:-1:0) / lengthR).^2/2);
                coefR = coefR / sum(coefR);
                % PfL
                xL= sum(coefL' .* curve{i}(extremum(j)+1-lengthL:extremum(j),2));
                yL= sum(coefL' .* curve{i}(extremum(j)+1-lengthL:extremum(j),1));
                % PfR
                xR = sum(coefR' .* curve{i}(extremum(j):extremum(j)+lengthR-1,2));
                yR = sum(coefR' .* curve{i}(extremum(j):extremum(j)+lengthR-1,1));
                vL = [xL-xcor yL-ycor]; vR = [xR-xcor yR-ycor];
                vm = vL / norm(vL) + vR / norm(vR);
%*************   Assign Main Orientation  **********%
                deltax = vm(1);
                deltay = vm(2);
                if isnan(atan(deltay/deltax)) % no solution
                    orientation1 = 0;
                elseif deltay >= 0 & deltax >= 0 % quadrant I
                    orientation1 = atan(deltay / deltax);
                elseif deltay < 0 & deltax >= 0  % quadrant IV
                    orientation1 = atan(deltay / deltax) + 2*pi;
                elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                    orientation1 = atan(deltay / deltax) + pi;
                end
                angle = [angle; orientation1];
            end
        end
    end

接下来是老两种特殊的点，第一个与最后一个

主要理解的就是λL，λR是什么意思。怎么取值

% Add Endpoints
if Endpoint
    for i=1:curve_num
        retail = size(curve{i},1);
        if retail>0 & curve_mode(i,:)=='line'
            % Start point compare with detected corners
            compare_corner=cout-ones(size(cout,1),1)*curve_start(i,:);
            compare_corner=compare_corner.^2;
            compare_corner=compare_corner(:,1)+compare_corner(:,2);
            if min(compare_corner)>25       % Add end points far from detected corners 
                cout = [cout; curve_start(i,:)];
                if rflag
                    xx = curve{i}(1,2);
                    yy = curve{i}(1,1);
                    % 这里就是公式中给出的λR，λR是从start_point最临近的右端点，使用min来取值
                    coef2 = exp(-((min(retail,maxlength)-1:-1:0) / sigmaLs).^2/2);
                    coef2 = coef2 / sum(coef2);
                    xR = sum(coef2' .*curve{i}(1:min(retail,maxlength),2));
                    yR = sum(coef2' .*curve{i}(1:min(retail,maxlength),1));
                    deltax = xR - xx;
                    deltay = yR - yy;
                    if isnan(atan(deltay/deltax)) % no solution
                        orientation2 = 0;
                    elseif deltay >= 0 & deltax >= 0 % quadrant I
                        orientation2 = atan(deltay / deltax);
                    elseif deltay < 0 & deltax >= 0  % quadrant IV
                        orientation2 = atan(deltay / deltax) + 2*pi;
                    elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                        orientation2 = atan(deltay / deltax) + pi;
                    end
                    angle = [angle;orientation2];
                end
            end
            
            % End point compare with detected corners
            compare_corner=cout-ones(size(cout,1),1)*curve_end(i,:);
            compare_corner=compare_corner.^2;
            compare_corner=compare_corner(:,1)+compare_corner(:,2);
            if min(compare_corner)>25
                cout = [cout; curve_end(i,:)]; 
                if rflag
                    xx = curve{i}(end,2);
                    yy = curve{i}(end,1);
                    % 这里就是公式中给出的λL，λL是从end_point最临近的左端点，使用从0开始使用max来取值
                    coef1 = exp(-((0:1:retail-max(1,retail-maxlength+1)) / sigmaLs).^2/2);
                    coef1 = coef1 / sum(coef1);
                    xL = sum(coef1' .*curve{i}(max(1,retail-maxlength+1):retail,2));
                    yL = sum(coef1' .*curve{i}(max(1,retail-maxlength+1):retail,1));
                    deltax = xL - xx;
                    deltay = yL - yy;
                    if isnan(atan(deltay/deltax)) % no solution
                        orientation3 = 0;
                    elseif deltay >= 0 & deltax >= 0 % quadrant I
                        orientation3 = atan(deltay / deltax);
                    elseif deltay < 0 & deltax >= 0  % quadrant IV
                        orientation3 = atan(deltay / deltax) + 2*pi;
                    elseif (deltay >= 0 & deltax < 0) | (deltay < 0 & deltax < 0 ) % quadrant II & III
                        orientation3 = atan(deltay / deltax) + pi;
                    end
                    angle = [angle;orientation3];
                end
            end
        end
    end
end