Exercise 7, Problem 3 and 4

Make a function transform(systempose_w, pos_l) that implements the transformation.

lsr2wrld.m

function wrld =  lsr2wrld(lsr, pose)
    tm = [cos(pose(3)) -sin(pose(3)); sin(pose(3)) cos(pose(3))]; %transf matrix
    wrld = lsr;
    for i = 1:size(lsr,2)
        wrld(:,i) = tm * lsr(:,i) + [pose(1); pose(2)];
    end
end

Make a 'lines' environment with the lines: [(0,1) (4,1)] [(4, 1) (4,5)] [(0,-1) (5,-1)] [(5, -1) (5,5)]

lines=[0 1 4 1; 4 1 4 5; 0 -1 5 -1; 5 -1 5 5]';

Take three scans with the laser poses (0,0,0), (3,0,0) and (4.5,0,pi/2).


scannerPose1 = [0 0 0]; %theta is in radians
scannerPose2 = [3 0 0]; %theta is in radians
scannerPose3 = [4.5 0 pi/2]; %theta is in radians

Convert the scans to world coordinates and plot the three scans in the same plot.

laserScan = laserscan2011(scannerPose1(1), scannerPose1(2), scannerPose1(3), lines, maxDist,resol,field_of_view);
carthScan1 = polar2carth(laserScan);
worldScan1 = lsr2wrld(carthScan1,scannerPose1);

laserScan = laserscan2011(scannerPose2(1), scannerPose2(2), scannerPose2(3), lines, maxDist,resol,field_of_view);
carthScan2 = polar2carth(laserScan);
worldScan2 = lsr2wrld(carthScan2,scannerPose2);

laserScan = laserscan2011(scannerPose3(1), scannerPose3(2), scannerPose3(3), lines, maxDist,resol,field_of_view);
carthScan3 = polar2carth(laserScan);
worldScan3 = lsr2wrld(carthScan3,scannerPose3);

hold on
figure(1);
hold on
plot(worldScan1(1,:),worldScan1(2,:),'bd')
plot(worldScan2(1,:),worldScan2(2,:),'mo')
plot(worldScan3(1,:),worldScan3(2,:),'kx')
legend('Pose 1 (  0 ,0)','Pose 2 (  3 ,0)','Pose 3 (4.5,0)');