Exercise 10, Problem 1

Calculation of transformation from world coordinates to odometry coordinates.

function transform = findTransform(odoPose, pose)
% transform = FINDTRANSFORM(odoPose,pose)
% Find the transformation from the world coordinates to the odometry
% coordinates given a pose in the odometry coordinates (odoPose) and the
% same point in the world coordinates (pose). The output (transform) is
% simply the origo of the odometry coordinates in the world coordinates
  theta =  normalizeAngle(odoPose(3)-pose(3));
  tMatrix = [cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0;0 0 1];
  transform = -tMatrix*pose + odoPose;
  transform(3) = normalizeAngle(transform(3));
end

Where we have used the angle normalization:

function outAngle = normalizeAngle(inAngle)
    inAngle  = inAngle + 2*pi;
    outAngle = mod(inAngle,2*pi);
    outAngle = outAngle -2*pi;
end