How to add kalman filter to the Lidar odometry and mapping slam present in the mathworks document?
This is the code to perform LOAM for the self driving car moving around the parking lot, now how to add kalman filter in this code given below:
% Load reference path % 60 vehicles are parked
data = load("parkingLotReferenceData.mat");
% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;
% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;
% Display the reference trajectory and the parked vehicle locations
sceneName = "LargeParkingLot";
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2,DisplayName="Reference Path");
scatter(parkedPoses(:,1),parkedPoses(:,2),[],"filled",DisplayName="Parked Vehicles");
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500]; % Resize figure
title("Large Parking Lot")
legend
modelName = ‘GenerateLidarDataOfParkingLot’;
open_system(modelName)
snapnow
helperAddParkedVehicles(modelName,parkedPoses)
close(hScene)
if ~ispc
error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end
% Run simulation
simOut = sim(modelName);
close_system(modelName,0)
ptCloudArr = helperGetPointClouds(simOut);
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);
ptCloud = ptCloudArr(1);
nextPtCloud = ptCloudArr(2);
gridStep = 0.5;
tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);
disp(tform)
egoRadius = 3.5;
cylinderRadius = 50;
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
selectedIdx = findPointsInCylinder(nextPtCloud,[egoRadius cylinderRadius]);
nextPtCloud = select(nextPtCloud,selectedIdx,OutputSize="full");
figure
hold on
title("Cylindrical Neighborhood")
pcshow(ptCloud)
view(2)
maxPlanarSurfacePoints = 8;
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
figure
hold on
title("LOAM Points")
show(points,MarkerSize=12)
[~,rmseValue] = pcregisterloam(points,nextPoints);
disp([‘RMSE: ‘ num2str(rmseValue)])
points = downsampleLessPlanar(points,gridStep);
figure
hold on
title(‘LOAM Points After Downsampling the Less Planar Surface Points’)
show(points,’MarkerSize’,12)
absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetOdometry = pcviewset;
viewId = 1;
vSetOdometry = addView(vSetOdometry,viewId,absPose);
% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];
numSkip = 5;
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);
% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);
% Update the absolute pose and store it in the view set
absPose = rigidtform3d(absPose.A * relPose.A);
vSetOdometry = addView(vSetOdometry,viewId,absPose);
% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8);
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Odometry")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end
absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetMapping = pcviewset;
ptCloud = ptCloudArr(1);
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
points = detectLOAMFeatures(ptCloud,’MaxPlanarSurfacePoints’,maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
viewId = 1;
vSetMapping = addView(vSetMapping,viewId,absPose);
voxelSize = 0.1;
loamMap = pcmaploam(voxelSize);
addPoints(loamMap,points,absPose);
% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPtCloud = ptCloud;
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);
% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,MatchingMethod="one-to-one",InitialTransform=relPose);
% Find the refined absolute pose that aligns the points to the map
absPose = findPose(loamMap,points,relPose);
% Store the refined absolute pose in the view set
vSetMapping = addView(vSetMapping,viewId,absPose);
% Add the new points to the map
addPoints(loamMap,points,absPose);
% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8)
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Mapping")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end
% Get the ground truth trajectory
groundTruthTrajectory = vertcat(groundTruthPosesLidar.Translation);
% Get the estimated trajectories
odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);
mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);
% Plot the trajectories
figure
plot3(groundTruthTrajectory(:,1),groundTruthTrajectory(:,2),groundTruthTrajectory(:,3),LineWidth=2,DisplayName="Ground Truth")
hold on
plot3(odometryPositions(:,1),odometryPositions(:,2),odometryPositions(:,3),LineWidth=2,DisplayName="Odometry")
plot3(mappingPositions(:,1),mappingPositions(:,2),mappingPositions(:,3),LineWidth=2,DisplayName="Odometry and Mapping")
view(2)
legend
title("Compare Trajectories")
% Select the poses to compare
selectedGroundTruth = groundTruthTrajectory(1:numSkip:end,:);
% Compute the RMSE
rmseOdometry = rmse(selectedGroundTruth,odometryPositions,"all");
rmseWithMapping = rmse(selectedGroundTruth,mappingPositions,"all");
disp([‘RMSE of the trajectory estimated with Odometry: ‘ num2str(rmseOdometry)])
disp([‘RMSE of the trajectory estimated with Odometry and Mapping: ‘ num2str(rmseWithMapping)])
function ptCloudArr = helperGetPointClouds(simOut)
% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;
% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 1:size(ptCloudData,4)
ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n)); %#ok<AGROW>
end
end
function gTruth = helperGetLidarGroundTruth(simOut)
numFrames = size(simOut.lidarLocation.time,1);
gTruth = repmat(rigidtform3d,numFrames-1,1);
for i = 1:numFrames
gTruth(i).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
% Ignore the roll and the pitch rotations since the ground is flat
yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
gTruth(i).R = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
end
endThis is the code to perform LOAM for the self driving car moving around the parking lot, now how to add kalman filter in this code given below:
% Load reference path % 60 vehicles are parked
data = load("parkingLotReferenceData.mat");
% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;
% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;
% Display the reference trajectory and the parked vehicle locations
sceneName = "LargeParkingLot";
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2,DisplayName="Reference Path");
scatter(parkedPoses(:,1),parkedPoses(:,2),[],"filled",DisplayName="Parked Vehicles");
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500]; % Resize figure
title("Large Parking Lot")
legend
modelName = ‘GenerateLidarDataOfParkingLot’;
open_system(modelName)
snapnow
helperAddParkedVehicles(modelName,parkedPoses)
close(hScene)
if ~ispc
error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end
% Run simulation
simOut = sim(modelName);
close_system(modelName,0)
ptCloudArr = helperGetPointClouds(simOut);
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);
ptCloud = ptCloudArr(1);
nextPtCloud = ptCloudArr(2);
gridStep = 0.5;
tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);
disp(tform)
egoRadius = 3.5;
cylinderRadius = 50;
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
selectedIdx = findPointsInCylinder(nextPtCloud,[egoRadius cylinderRadius]);
nextPtCloud = select(nextPtCloud,selectedIdx,OutputSize="full");
figure
hold on
title("Cylindrical Neighborhood")
pcshow(ptCloud)
view(2)
maxPlanarSurfacePoints = 8;
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
figure
hold on
title("LOAM Points")
show(points,MarkerSize=12)
[~,rmseValue] = pcregisterloam(points,nextPoints);
disp([‘RMSE: ‘ num2str(rmseValue)])
points = downsampleLessPlanar(points,gridStep);
figure
hold on
title(‘LOAM Points After Downsampling the Less Planar Surface Points’)
show(points,’MarkerSize’,12)
absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetOdometry = pcviewset;
viewId = 1;
vSetOdometry = addView(vSetOdometry,viewId,absPose);
% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];
numSkip = 5;
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);
% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);
% Update the absolute pose and store it in the view set
absPose = rigidtform3d(absPose.A * relPose.A);
vSetOdometry = addView(vSetOdometry,viewId,absPose);
% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8);
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Odometry")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end
absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetMapping = pcviewset;
ptCloud = ptCloudArr(1);
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
points = detectLOAMFeatures(ptCloud,’MaxPlanarSurfacePoints’,maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
viewId = 1;
vSetMapping = addView(vSetMapping,viewId,absPose);
voxelSize = 0.1;
loamMap = pcmaploam(voxelSize);
addPoints(loamMap,points,absPose);
% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPtCloud = ptCloud;
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);
% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,MatchingMethod="one-to-one",InitialTransform=relPose);
% Find the refined absolute pose that aligns the points to the map
absPose = findPose(loamMap,points,relPose);
% Store the refined absolute pose in the view set
vSetMapping = addView(vSetMapping,viewId,absPose);
% Add the new points to the map
addPoints(loamMap,points,absPose);
% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8)
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Mapping")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end
% Get the ground truth trajectory
groundTruthTrajectory = vertcat(groundTruthPosesLidar.Translation);
% Get the estimated trajectories
odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);
mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);
% Plot the trajectories
figure
plot3(groundTruthTrajectory(:,1),groundTruthTrajectory(:,2),groundTruthTrajectory(:,3),LineWidth=2,DisplayName="Ground Truth")
hold on
plot3(odometryPositions(:,1),odometryPositions(:,2),odometryPositions(:,3),LineWidth=2,DisplayName="Odometry")
plot3(mappingPositions(:,1),mappingPositions(:,2),mappingPositions(:,3),LineWidth=2,DisplayName="Odometry and Mapping")
view(2)
legend
title("Compare Trajectories")
% Select the poses to compare
selectedGroundTruth = groundTruthTrajectory(1:numSkip:end,:);
% Compute the RMSE
rmseOdometry = rmse(selectedGroundTruth,odometryPositions,"all");
rmseWithMapping = rmse(selectedGroundTruth,mappingPositions,"all");
disp([‘RMSE of the trajectory estimated with Odometry: ‘ num2str(rmseOdometry)])
disp([‘RMSE of the trajectory estimated with Odometry and Mapping: ‘ num2str(rmseWithMapping)])
function ptCloudArr = helperGetPointClouds(simOut)
% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;
% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 1:size(ptCloudData,4)
ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n)); %#ok<AGROW>
end
end
function gTruth = helperGetLidarGroundTruth(simOut)
numFrames = size(simOut.lidarLocation.time,1);
gTruth = repmat(rigidtform3d,numFrames-1,1);
for i = 1:numFrames
gTruth(i).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
% Ignore the roll and the pitch rotations since the ground is flat
yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
gTruth(i).R = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
end
end This is the code to perform LOAM for the self driving car moving around the parking lot, now how to add kalman filter in this code given below:
% Load reference path % 60 vehicles are parked
data = load("parkingLotReferenceData.mat");
% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;
% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;
% Display the reference trajectory and the parked vehicle locations
sceneName = "LargeParkingLot";
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2,DisplayName="Reference Path");
scatter(parkedPoses(:,1),parkedPoses(:,2),[],"filled",DisplayName="Parked Vehicles");
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500]; % Resize figure
title("Large Parking Lot")
legend
modelName = ‘GenerateLidarDataOfParkingLot’;
open_system(modelName)
snapnow
helperAddParkedVehicles(modelName,parkedPoses)
close(hScene)
if ~ispc
error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end
% Run simulation
simOut = sim(modelName);
close_system(modelName,0)
ptCloudArr = helperGetPointClouds(simOut);
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);
ptCloud = ptCloudArr(1);
nextPtCloud = ptCloudArr(2);
gridStep = 0.5;
tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);
disp(tform)
egoRadius = 3.5;
cylinderRadius = 50;
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
selectedIdx = findPointsInCylinder(nextPtCloud,[egoRadius cylinderRadius]);
nextPtCloud = select(nextPtCloud,selectedIdx,OutputSize="full");
figure
hold on
title("Cylindrical Neighborhood")
pcshow(ptCloud)
view(2)
maxPlanarSurfacePoints = 8;
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
figure
hold on
title("LOAM Points")
show(points,MarkerSize=12)
[~,rmseValue] = pcregisterloam(points,nextPoints);
disp([‘RMSE: ‘ num2str(rmseValue)])
points = downsampleLessPlanar(points,gridStep);
figure
hold on
title(‘LOAM Points After Downsampling the Less Planar Surface Points’)
show(points,’MarkerSize’,12)
absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetOdometry = pcviewset;
viewId = 1;
vSetOdometry = addView(vSetOdometry,viewId,absPose);
% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];
numSkip = 5;
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);
% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);
% Update the absolute pose and store it in the view set
absPose = rigidtform3d(absPose.A * relPose.A);
vSetOdometry = addView(vSetOdometry,viewId,absPose);
% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8);
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Odometry")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end
absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetMapping = pcviewset;
ptCloud = ptCloudArr(1);
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
points = detectLOAMFeatures(ptCloud,’MaxPlanarSurfacePoints’,maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
viewId = 1;
vSetMapping = addView(vSetMapping,viewId,absPose);
voxelSize = 0.1;
loamMap = pcmaploam(voxelSize);
addPoints(loamMap,points,absPose);
% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPtCloud = ptCloud;
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);
% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);
% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,MatchingMethod="one-to-one",InitialTransform=relPose);
% Find the refined absolute pose that aligns the points to the map
absPose = findPose(loamMap,points,relPose);
% Store the refined absolute pose in the view set
vSetMapping = addView(vSetMapping,viewId,absPose);
% Add the new points to the map
addPoints(loamMap,points,absPose);
% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8)
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Mapping")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end
% Get the ground truth trajectory
groundTruthTrajectory = vertcat(groundTruthPosesLidar.Translation);
% Get the estimated trajectories
odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);
mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);
% Plot the trajectories
figure
plot3(groundTruthTrajectory(:,1),groundTruthTrajectory(:,2),groundTruthTrajectory(:,3),LineWidth=2,DisplayName="Ground Truth")
hold on
plot3(odometryPositions(:,1),odometryPositions(:,2),odometryPositions(:,3),LineWidth=2,DisplayName="Odometry")
plot3(mappingPositions(:,1),mappingPositions(:,2),mappingPositions(:,3),LineWidth=2,DisplayName="Odometry and Mapping")
view(2)
legend
title("Compare Trajectories")
% Select the poses to compare
selectedGroundTruth = groundTruthTrajectory(1:numSkip:end,:);
% Compute the RMSE
rmseOdometry = rmse(selectedGroundTruth,odometryPositions,"all");
rmseWithMapping = rmse(selectedGroundTruth,mappingPositions,"all");
disp([‘RMSE of the trajectory estimated with Odometry: ‘ num2str(rmseOdometry)])
disp([‘RMSE of the trajectory estimated with Odometry and Mapping: ‘ num2str(rmseWithMapping)])
function ptCloudArr = helperGetPointClouds(simOut)
% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;
% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 1:size(ptCloudData,4)
ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n)); %#ok<AGROW>
end
end
function gTruth = helperGetLidarGroundTruth(simOut)
numFrames = size(simOut.lidarLocation.time,1);
gTruth = repmat(rigidtform3d,numFrames-1,1);
for i = 1:numFrames
gTruth(i).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
% Ignore the roll and the pitch rotations since the ground is flat
yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
gTruth(i).R = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
end
end slam, loam, autonomous vehicle, kalman filter MATLAB Answers — New Questions