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

需要注意的是，储存经过网格单元等值线线段的两个端点，便于后续寻找等值线上最近点。在网格单元内不对水深/重力值进行插值。

得到的提取等值线（红色）结果和最近点如图：