%% Match Lidar Scans
% Create a reference lidar scan using lidarScan. Specify ranges and angles as vectors.

refRanges = 5*ones(1,300);
refAngles = linspace(-pi/2,pi/2,300); 
refScan = lidarScan(refRanges,refAngles);

%
% Using the transformScan function, generate a second lidar scan at an x,y offset of (0.5,0.2).

currScan = transformScan(refScan,[0.5 0.2 0]);

%
% Match the reference scan and the second scan to estimate the pose difference between them.

pose = matchScans(currScan,refScan);

%
% Use the transformScan function to align the scans by transforming the second scan into the frame of the first scan using the relative pose difference. Plot both the original scans and the aligned scans.

currScan2 = transformScan(currScan,pose);

subplot(2,1,1);
hold on
plot(currScan)
plot(refScan)
title('Original Scans')
hold off

subplot(2,1,2);
hold on
plot(currScan2)
plot(refScan)
title('Aligned Scans')
xlim([0 5])
hold off

% Copyright 2012 The MathWorks, Inc.