%% Use Scan Matching to Transform Scans
% Use the matchScans function to find the relative transformation between two laser scans. Then transform the second laser scan into the coordinate frame of the first laser scan.
% This example requires an Optimization Toolbox™ license.
% Specify a laser scan as ranges and angles. Create a second laser scan that is offset from the first using transformScan. This transformation simulates a second laser scan being collected from a new coordinate frame.

refRanges = 5*ones(1,300);
refRanges(51:150) = 3*ones(1,100);
refAngles = linspace(-pi/2,pi/2,300);
offset = [0.5 0.2 0];
[currRanges,currAngles] = transformScan(refRanges,refAngles,offset);

%
% Use scan matching to find the relative pose between the two laser scans. This pose is close to the specified offset. You must have an Optimization Toolbox™ license to use the matchScans function.

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

%
% Transform the second scan to the coordinate frame of the first scan. Plot the two scans to see how they overlap.

[currRanges2,currAngles2] = transformScan(currRanges,currAngles,pose);
clf
polarplot(refAngles,refRanges,'or')
hold on
polarplot(currAngles2,currRanges2,'.b')
legend('First laser scan','Second laser scan')
hold off

% Copyright 2012 The MathWorks, Inc.