matchScans function to compute the pose difference between a series of laser scans. Compose the relative poses by using a defined
composePoses function to get a transformation to the initial frame. Then, transform all laser scans into the initial frame using these composed poses.
Specify the original laser scan and offsets to generate a series of shifted laser scans. Iterate through the scans and transform the original scan based on each offset. Plot the laser scans to see the shifted data.
ranges = zeros(300,4); angles = zeros(300,4); ranges(:,1) = 5*ones(300,1); ranges(11:30,1) = 4*ones(1,20); ranges(101:200,1) = 3*ones(1,100); angles(:,1) = linspace(-pi/2,pi/2,300); offset(1,:) = [0.1 0.1 0]; offset(2,:) = [0.4 0.1 0.1]; offset(3,:) = [-0.2 0 -0.1]; for i = 2:4 [ranges(:,i),angles(:,i)] = transformScan(ranges(:,i-1),angles(:,i-1),offset(i-1,:)); end [x,y] = pol2cart(angles,ranges); plot(x,y) axis equal
Perform scan matching on each laser scan set to get the relative pose between each scan. The outputs from the
matchScans function are close to the specified offsets. The initial scan is in the initial frame, so the pose difference is
[0 0 0].
relPoses(1,:) = [0 0 0]; for i = 2:4 relPoses(i,:) = matchScans(ranges(:,i),angles(:,i),ranges(:,i-1),angles(:,i-1),... 'SolverAlgorithm','fminunc','CellSize',1); end
composePoses function in a loop to get the absolute transformation for each laser scan. This function is defined at the end of the example. Transform each scan to get them all in the initial frame.
transRanges = zeros(300,4); transAngles = zeros(300,4); transRanges(:,1) = ranges(:,1); transAngles(:,1) = angles(:,1); composedPoses(1,:) = [0 0 0]; for i = 2:4 composedPoses(i,:) = composePoses(relPoses(i,:),composedPoses(i-1,:)); [transRanges(:,i),transAngles(:,i)] = transformScan(ranges(:,i),angles(:,i),composedPoses(i,:)); end
Plot the transformed ranges and angles. They overlap well, based on the calculated transformations from
[x,y] = pol2cart(transAngles,transRanges); plot(x,y) axis equal
composePoses function. This function takes in the transformation of the initial frame to the base frame and the relative transformation from the initial frame to a second frame. For a series of laser scans, the
relative input is the relative pose between the last two frames, and the
base input is the composed pose over all previous scans.
You can also define this function in a separate script and save to the current folder.
function composedPose = composePoses(relative,base) %Convert both poses (3-by-1 vector) to transformations (4-by-4 matrix) and multiply %together using pose2tform function. tform = pose2tform(base)*pose2tform(relative); % Extract translational vector and Euler angles as ZYX. trvec = tform2trvec(tform); eul = tform2eul(tform); % Concatenate the elements of the transform as [x y theta]. composedPose = [trvec(1:2) eul(1)]; % Function to convert pose to transform. function tform = pose2tform(pose) x = pose(1); y = pose(2); th = wrapTo2Pi(pose(3)); tform = trvec2tform([x y 0])*eul2tform([th 0 0]); end end