www.gusucode.com > MATLAB编程毕业设计 EKF SLAM仿真全部源代码 > extractInterestPoint.m

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%检测兴趣点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function interestPoint = extractInterestPoint(closePoint,lineNum,linePara,px,py)
N = length(closePoint(:,1));

%兴趣点集合
interestPoint = [];

%首尾点检测
ax = closePoint(1,1);
ay = closePoint(1,2);
bx = closePoint(N,3);
by = closePoint(N,4);
%两点到原点的分别距离平方
aDist = ax*ax+ay*ay;
bDist = bx*bx+by*by;

if(ay>30 && aDist<=102400)
    %检测首点之前的一个扫描点
    tempIndex = linePara(1,4);
    ra = checkExtentionLineRange(ax,ay,closePoint(1,3),closePoint(1,4),5);
    if(ra<330)
        if(tempIndex ~= 1)
            tempx = px(tempIndex-1);
            tempy = py(tempIndex-1);
            dis = tempx*tempx + tempy*tempy;
            ang = (tempx*ax + tempy*ay)^2/(tempx*tempx + tempy*tempy)/(ax*ax + ay*ay);
            if(dis>6400 && ang<0.98063)
                %首点和之前的一个点不共线，是兴趣点
                 %将首点加入兴趣点
                 interestPoint = [interestPoint;ax,ay];
            end
        else
            %将首点加入兴趣点
            interestPoint = [interestPoint;ax,ay];
        end
    end
end

if(by>30 && bDist<=102400)
    %检测尾点后的一个扫描点
    tempIndex = linePara(lineNum,5);
    rb = checkExtentionLineRange(bx,by,closePoint(N,1),closePoint(N,2),5);
    if(rb<330)
        if(tempIndex ~= length(px))
            tempx = px(tempIndex+1);
            tempy = py(tempIndex+1);
            dis = tempx*tempx + tempy*tempy;
            ang = (tempx*bx + tempy*by)^2/(tempx*tempx + tempy*tempy)/(bx*bx + by*by);
            if(dis>6400 && ang<0.98063)
                 %尾点和之后的一个点不共线，是兴趣点
                 %将尾点加入兴趣点
                 interestPoint = [interestPoint;bx,by];
            end
        else
            %将尾点加入兴趣点
            interestPoint = [interestPoint;bx,by];
        end
    end
end


%检测双点
for i = 1:N-1
    ax = closePoint(i,3);
    ay = closePoint(i,4);
    bx = closePoint(i+1,1);
    by = closePoint(i+1,2);
    
    %两点之间的距离平方
    abDist = (ax-bx)*(ax-bx)+(ay-by)*(ay-by);
    %两点到原点的分别距离
    aDist = ax*ax+ay*ay;
    bDist = bx*bx+by*by;
    %两向量的夹角的cos的平方
    abAngle = (ax*bx+ay*by)^2/(ax*ax+ay*ay)/(bx*bx+by*by);
    
    %【双点】AB距离大于80cm，夹角小于5°
    if(abDist>6400 && abAngle>0.98063)
        if(aDist<bDist)
            %A点较近，是兴趣点
            %%%%%%
            interestPoint = [interestPoint;ax,ay];
        else
            %B点较近，是兴趣点
            %%%%%%
            interestPoint = [interestPoint;bx,by];
        end
    %【单点】AB距离大于80cm，夹角大于5°
    elseif(abDist>6400 && abAngle<0.98063)
        %检测A点
        ra = checkExtentionLineRange(ax,ay,closePoint(i,1),closePoint(i,2),5);
        if(ra < 330)
            %在合理范围内，A点是兴趣点
            %%%%%%
            interestPoint = [interestPoint;ax,ay];
        end
        %检测B点
        rb = checkExtentionLineRange(bx,by,closePoint(i+1,3),closePoint(i+1,4),5);
        if(rb < 330)
            %在合理范围内，B点是兴趣点
            %%%%%%
            interestPoint = [interestPoint;bx,by];
        end
    %【拐点】AB距离小于80cm，可能是拐点
    elseif(abDist<6400)
        v1x =  closePoint(i,1) - closePoint(i,3);
        v1y =  closePoint(i,2) - closePoint(i,4);
        v2x =  closePoint(i+1,1) - closePoint(i+1,3);
        v2y =  closePoint(i+1,2) - closePoint(i+1,4);
        %计算两向量之间夹角的平方，夹角在75~105度之间，认为有拐点出现
        vAngle = (v1x*v2x+v1y*v2y)^2/(v1x^2+v1y^2)/(v2x^2+v2y^2);
        if(vAngle<0.066987)
            [crossx,crossy] = getCrossPoint(closePoint(i,:),closePoint(i+1,:));
            interestPoint = [interestPoint;crossx,crossy];
        end
    end
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%求直线延长线的距离
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function r = checkExtentionLineRange(mx,my,nx,ny,angle)
aa = nx*nx + ny*ny;
bb = (mx-nx)^2 + (my-ny)^2;
cc = mx*mx + my*my;
b = sqrt(bb);
c = sqrt(cc);
cos_belta = (bb + cc -aa)/2/b/c;
belta = acos(cos_belta);
theta = angle/180*pi;

if(belta-theta>0)
    r = c*sin(belta)/sin(belta-theta);
else
    r = 5000;
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%求两条直线的交点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,y] = getCrossPoint(a,b)
ax = a(1);
ay = a(2);
bx = a(3);
by = a(4);
cx = b(1);
cy = b(2);
dx = b(3);
dy = b(4);

M = by - ay;
N = bx - ax;
P = dy - cy;
Q = dx - cx;

x = (M*Q*ax - P*N*cx + N*Q*(cy - ay))/(M*Q - P*N);
y = (N*P*ay - M*Q*cy + M*P*(cx - ax))/(N*P - M*Q);

end