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

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%获取描述墙体形状的点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [closePoint] = getClosePoint(lineNum,linePara,px,py)
    closePoint = [];
    
    for i = 1:lineNum
        %非垂线
        if(linePara(i,1))
            x1 = px(linePara(i,4));
            x2 = px(linePara(i,5));
            y1 = py(linePara(i,4));
            y2 = py(linePara(i,5));
            k = linePara(i,2);
            b = linePara(i,3);
            x1 = (x1 + k*y1 - k*b)/(k*k + 1);
            y1 = k*x1 + b;
            x2 = (x2 + k*y2 - k*b)/(k*k + 1);
            y2 = k*x2 + b;
        %垂线
        else
            x1 = linePara(i,2);
            x2 = x1;
            y1 = py(linePara(i,4));
            y2 = py(linePara(i,5));
        end
        
        %添加到数组
        closePoint = [closePoint;x1,y1,x2,y2];
    end
%     closePoint
end