Problem 8, Exercise 2.2

By use and extension of the previously created function, we obtain:


function [ projectedLine lineCov ] = projectToLaser( worldLine,poseIn, covIn)
%[projectedLine, lineCov] = PROJECTTOLASER(worldLine,poseIn,covIn)
%Project a word line to the laser scanner frame given the
%world line, the robot pose and robot pose covariance. Note that the laser
%scanner pose in the robot frame is read globally
%   worldLine: The line in world coordinates
%   poseIn: The robot pose
%   covIn: The robot pose covariance
%
%   projectedLine: The line parameters in the laser scanner frame
%   lineCov: The covariance of the line parameters

%% Constants
global lsrRelPose % The laser scanner pose in the robot frame is read globally



%% Calculation

[h(1), h(2)]=world2robot(poseIn, worldLine);
[projectedLine(1), projectedLine(2)]=world2robot(lsrRelPose, h);


lineCov = zeros(2,2)+covIn;
end

The results are shown below. We will note that the last three pictures also show the extracted lines from the data and are thus also a solution for the last problem in this exercise.

Results with 0 uncertainty:
Line track:

Circular track:

Box track:

After adding some uncertainties we notice that the extracted lines do not perfectly fit the world:

Line track:

Circle track:

Box Track: