%% Match Laser Scans
% This example uses the 'fminunc' solver algorithm to perform scan matching. This solver algorithm requires an Optimization Toolbox™ license.
% Specify a reference laser scan as ranges and angles.

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

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

[currRanges,currAngles] = transformScan(refRanges,refAngles,[0.5 0.2 0]);

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

pose = matchScans(currRanges,currAngles,refRanges,refAngles,'SolverAlgorithm','fminunc');

%
% Improve the estimate by giving an initial pose estimate.

pose = matchScans(currRanges,currAngles,refRanges,refAngles,...
                  'SolverAlgorithm','fminunc','InitialPose',[-0.4 -0.1 0]);

%
% 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.

[currRanges2,currAngles2] = transformScan(currRanges,currAngles,pose);

[x1, y1] = pol2cart(refAngles,refRanges);
[x2, y2] = pol2cart(currAngles,currRanges);
[x3, y3] = pol2cart(currAngles2,currRanges2);

subplot(1,2,1)
plot(x1,y1,'o',x2,y2,'*r')
title('Original Scans')
subplot(1,2,2)
plot(x1,y1,'o',x3,y3,'*r')
title('Aligned Scans')

% Copyright 2019 The MathWorks, Inc.