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