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