ICCP算法——提取等值線+等值線最近點

在ICCP算法當中，從高度/重力地圖中提取等值線是重要步驟。1999年的文章《Vehicle localization on gravity maps》中詳細地介紹了ICCP算法每一個步驟的實現方法。

其中，提取等值線部分的算法敘述如下：

 

大致過程是搜索規定區域內每個網格點的四條邊，判斷等值線是否經過其中兩條邊。若水深/重力等值線f值在某條邊的兩個端點的值之間，則認為等值線經過該條邊。

假設等值線與一條橫邊相交，等值線與該條邊的交點橫坐標為：

縱坐標與該條邊的網格點相同。

在提取得到等值線數據后，需要尋找等值線上最近點，算法敘述如下：

尋找等值線上最近點的方法是，首先，計算INS指示點向每一條等值線線段投影的最近距離，記錄投影點坐標；比較各個最近距離，選擇其中最小的，作為最終的等值線最近點。

 

根據該論文中給出的例子，設計網格點坐標和坐標對應的水深/重力值。假設需要提取的等值線值為f=2。

 

MATLAB代碼如下：

           xx=[0,1,2,3,4]; 
          
yy=[4;3;2;1;0]; 
          
for i=1:5 
          
    xx(i,:)=[0,1,2,3,4]; 
          
end 
          
for i=1:5 
          
    yy(:,i)=[4;3;2;1;0]; 
          
end 
          
zz=[0,0,5,0,0; 
          
    0,3,5,4,4; 
          
    0,4,5,5,5; 
          
    5,5,5,5,5; 
          
    5,5,5,5,5];            %創建網格點坐標及重力值 
          
k=1; 
          
f=2;              %待提取等值線值 
          
XX=[1.8,2.2];    %待匹配點坐標 
         

           %% 提取等值線 
          
%尋找搜索區域中心點（距離XX最近的網格點） 
          
min_d=10000; 
          
for i=1:length(zz) 
          
    for j=1:length(zz) 
          
        distance=sqrt((XX(1)-xx(i,j))^2+(XX(2)-yy(i,j))^2); 
          
        if distance<min_d 
          
            min_d=distance; 
          
            center=[xx(i,j),yy(i,j)]; 
          
            center_i=i; 
          
            center_j=j; 
          
        end 
          
    end 
          
end 
          
%在3*3區域內搜索是否有等值線 
          
x=zeros(3,3); 
          
y=zeros(3,3); 
          
z=zeros(3,3); 
          
left_i=center_i-1; 
          
left_j=center_j-1;    %搜索區域左頂點標號 
          
for i=1:3 
          
    for j=1:3 
          
        x(i,j)=xx(left_i+i-1,left_j+j-1); 
          
        y(i,j)=yy(left_i+i-1,left_j+j-1); 
          
        z(i,j)=zz(left_i+i-1,left_j+j-1); 
          
    end 
          
end 
          
%是否需要擴大搜索區域 
          
flag=0; 
          
if f>max(max(z)) 
          
    flag=1; 
          
end 
         

           if f<min(min(z)) 
          
    flag=1; 
          
end 
          
%提取等值線 
          
if flag==1     %擴充搜索區域為5*5 
          
    x=zeros(5,5); 
          
    y=zeros(5,5); 
          
    z=zeros(5,5); 
          
    left_i=center_i-2; 
          
    left_j=center_j-2;    %搜索區域左頂點標號 
          
    for i=1:5 
          
        for j=1:5 
          
            x(i,j)=xx(left_i+i-1,left_j+j-1); 
          
            y(i,j)=yy(left_i+i-1,left_j+j-1); 
          
            z(i,j)=zz(left_i+i-1,left_j+j-1); 
          
        end 
          
    end 
          
    for i=1:5     %搜索網格點 
          
        for j=1:5 
          
            if z(i,j)==f 
          
                contour(k,1)=xx(i,j); 
          
                contour(k,2)=yy(i,j); 
          
                k=k+1; 
          
            end 
          
        end 
          
    end 
          
    
          
    for i=1:4 
          
        for j=1:4 
          
            if (f>z(i,j) && f<z(i,j+1)) || (f>z(i,j+1) && f<z(i,j))  %網格單元上邊 
          
                contour(k,1)=xx(i,j)+(f-z(i,j))/(z(i,j+1)-z(i,j))*(xx(i,j+1)-xx(i,j)); 
          
                contour(k,2)=yy(i,j); 
          
                k=k+1; 
          
            end 
          
            if (f>z(i+1,j) && f<z(i+1,j+1)) || (f>z(i+1,j+1) && f<z(i+1,j))   %網格單元下邊 
          
                contour(k,1)=xx(i+1,j)+(f-z(i+1,j))/(z(i+1,j+1)-z(i+1,j))*(xx(i+1,j+1)-xx(i+1,j)); 
          
                contour(k,2)=yy(i+1,j); 
          
                k=k+1; 
          
            end 
          
            if (f>z(i,j) && f<z(i+1,j)) || (f>z(i+1,j) && f<z(i,j))   %網格左邊 
          
                contour(k,1)=xx(i,j); 
          
                contour(k,2)=yy(i,j)+(f-z(i,j))/(z(i+1,j)-z(i,j))*(yy(i+1,j)-yy(i,j)); 
          
                k=k+1; 
          
            end 
          
            if (f>z(i,j+1) && f<z(i+1,j+1)) || (f>z(i+1,j+1) && f<z(i,j+1))   %網格右邊 
          
                contour(k,1)=xx(i,j+1); 
          
                contour(k,2)=yy(i,j+1)+(f-z(i,j+1))/(z(i+1,j+1)-z(i,j+1))*(yy(i+1,j+1)-yy(i,j+1)); 
          
                k=k+1; 
          
            end 
          
        end 
          
    end 
          
else                   %搜索3*3區域 
          
    for i=1:2 
          
        for j=1:2 
          
            if (f>z(i,j) && f<z(i,j+1)) || (f>z(i,j+1) && f<z(i,j))  %網格單元上邊 
          
                contour(k,1)=xx(i,j)+(f-z(i,j))/(z(i,j+1)-z(i,j))*(xx(i,j+1)-xx(i,j)); 
          
                contour(k,2)=yy(i,j); 
          
                k=k+1; 
          
            end 
          
            if (f>z(i+1,j) && f<z(i+1,j+1)) || (f>z(i+1,j+1) && f<z(i+1,j))   %網格單元下邊 
          
                contour(k,1)=xx(i+1,j)+(f-z(i+1,j))/(z(i+1,j+1)-z(i+1,j))*(xx(i+1,j+1)-xx(i+1,j)); 
          
                contour(k,2)=yy(i+1,j); 
          
                k=k+1; 
          
            end 
          
            if (f>z(i,j) && f<z(i+1,j)) || (f>z(i+1,j) && f<z(i,j))   %網格左邊 
          
                contour(k,1)=xx(i,j); 
          
                contour(k,2)=yy(i,j)+(f-z(i,j))/(z(i+1,j)-z(i,j))*(yy(i+1,j)-yy(i,j)); 
          
                k=k+1; 
          
            end 
          
            if (f>z(i,j+1) && f<z(i+1,j+1)) || (f>z(i+1,j+1) && f<z(i,j+1))   %網格右邊 
          
                contour(k,1)=xx(i,j+1); 
          
                contour(k,2)=yy(i,j+1)+(f-z(i,j+1))/(z(i+1,j+1)-z(i,j+1))*(yy(i+1,j+1)-yy(i,j+1)); 
          
                k=k+1; 
          
            end 
          
        end 
          
    end 
          
end 
         

           %% 尋找等值線上最近點 
          
n=length(contour); 
          
i=1; 
          
nn=1; 
          
%獲得等值線線段上最近點集合xi 
          
for i=1:2:n 
          
    x1=contour(i,:); 
          
    x2=contour(i+1,:); 
          
    l=sqrt((x1(1)-x2(1))^2+(x1(2)-x2(2))^2);   %等值線線段長度 
          
    alpha=(XX-x1)*(x2-x1)'/l^2; 
          
    if alpha<=0 
          
        xi(nn,:)=x1; 
          
    elseif alpha>=1 
          
        xi(nn,:)=x2; 
          
    elseif alpha>0 && alpha<1 
          
        xi(nn,:)=(1-alpha)*x1+alpha*x2; 
          
    end 
          
    nn=nn+1; 
          
end 
          
%獲得最近點yi 
          
min_d=10000; 
          
for i=1:nn-1 
          
    distance=sqrt((XX(1)-xi(i,1))^2+(XX(2)-xi(i,2))^2); 
          
    if distance<min_d 
          
        min_d=distance; 
          
        yi=xi(i,:); 
          
    end 
          
end 
         

%畫圖plot(xx(:),yy(:),'k.');
grid on;
hold on;
plot(XX(1),XX(2),'b.');
plot(yi(1),yi(2),'*b');
for i=1:2:11
    plot(contour(i:i+1,1),contour(i:i+1,2),'r-');
    hold on;
end

 

需要注意的是，儲存經過網格單元等值線線段的兩個端點，便於后續尋找等值線上最近點。在網格單元內不對水深/重力值進行插值。

得到的提取等值線（紅色）結果和最近點如圖：