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

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%将小坐标里的兴趣点转换到大坐标里的兴趣点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [mapInterestPoint] = changeCoordinate(interestPoint,robx,roby,robphi)

mapInterestPoint = [];

if(~isempty(interestPoint))
    x = interestPoint(:,1);
    y = interestPoint(:,2);

    th = pi/2 - robphi;

    N = length(x);

    mapInterestPoint = zeros(N,2);

    for i = 1:N
        mapInterestPoint(i,1) = x(i)*cos(th) + y(i)*sin(th) + robx;
        mapInterestPoint(i,2) = -x(i)*sin(th) + y(i)*cos(th) + roby;
    end
end

end