baseDownloadURL = 'https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip'; dataFolder = fullfile(tempdir, 'kit_velodyneslam_data_scenario1', filesep); options = weboptions('Timeout', Inf); zipFileName = dataFolder + "scenario1.zip"; % Get the full file path to the PNG files in the scenario1 folder. pointCloudFilePattern = fullfile(dataFolder, 'scenario1', 'scan*.png'); numExpectedFiles = 2513; folderExists = exist(dataFolder, 'dir'); if ~folderExists % Create a folder in a temporary directory to save the downloaded zip % file. mkdir(dataFolder); disp('Downloading scenario1.zip (153 MB) ...') websave(zipFileName, baseDownloadURL, options); % Unzip downloaded file unzip(zipFileName, dataFolder); elseif folderExists && numel(dir(pointCloudFilePattern)) < numExpectedFiles % Redownload the data if it got reduced in the temporary directory. disp('Downloading scenario1.zip (153 MB) ...') websave(zipFileName, baseDownloadURL, options); % Unzip downloaded file. unzip(zipFileName, dataFolder) end datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern); pointCloudTable = datasetTable(:, 1); insDataTable = datasetTable(:, 2:end); %% % Read the first point cloud and display it at the MATLAB(R) command prompt ptCloud = helperReadPointCloudFromFile(pointCloudTable.PointCloudFileName{1}); disp(ptCloud) %% % Display the first INS reading. The |timetable| holds |Heading|, |Pitch|, % |Roll|, |X|, |Y|, and |Z| information from the INS. disp(insDataTable(1, :)) %% % Visualize the point clouds using , % a streaming point cloud display. The vehicle traverses a path consisting % of two loops. In the first loop, the vehicle makes a series of turns and % returns to the starting point. In the second loop, the vehicle makes a % series of turns along another route and again returns to the starting % point. % Specify limits of the player xlimits = [-45 45]; % meters ylimits = [-45 45]; zlimits = [-10 20]; % Create a streaming point cloud display object lidarPlayer = pcplayer(xlimits, ylimits, zlimits); % Customize player axes labels xlabel(lidarPlayer.Axes, 'X (m)') ylabel(lidarPlayer.Axes, 'Y (m)') zlabel(lidarPlayer.Axes, 'Z (m)') title(lidarPlayer.Axes, 'Lidar Sensor Data') % Skip evey other frame since this is a long sequence skipFrames = 2; numFrames = height(pointCloudTable); for n = 1 : skipFrames : numFrames % Read a point cloud fileName = pointCloudTable.PointCloudFileName{n}; ptCloud = helperReadPointCloudFromFile(fileName); % Visualize point cloud view(lidarPlayer, ptCloud); pause(0.01) end %% Build a Map Using Odometry % First, use the approach explained in the % example to build a map. The approach consists of the % following steps: % % * *Align lidar scans:* Align successive lidar scans using a point cloud % registration technique. This example uses % for registering scans. By successively composing these % transformations, each point cloud is transformed back to the reference % frame of the first point cloud. % * *Combine aligned scans:* Generate a map by combining all the % transformed point clouds. % % This approach of incrementally building a map and estimating the % trajectory of the vehicle is called _odometry_. % % Use a object to store and manage data across multiple views. A % view set consists of a set of connected views. % % * Each view stores information associated with a single view. This % information includes the absolute pose of the view, the point cloud % sensor data captured at that view, and a unique identifier for the view. % Add views to the view set using . % * To establish a connection between views use % . A connection stores information like the relative % transformation between the connecting views, the uncertainty involved in % computing this measurement (represented as an information matrix) and the % associated view identifiers. % * Use the method to visualize the connections established by the view set. % These connections can be used to visualize the path traversed by the % vehicle. hide(lidarPlayer) % Set random seed to ensure reproducibility rng(0); % Create an empty view set vSet = pcviewset; % Create a figure for view set display hFigBefore = figure('Name', 'View Set Display'); hAxBefore = axes(hFigBefore); % Initialize point cloud processing parameters downsamplePercent = 0.1; regGridSize = 3; % Initialize transformations absTform = rigid3d; % Absolute transformation to reference frame relTform = rigid3d; % Relative transformation between successive scans viewId = 1; skipFrames = 5; numFrames = height(pointCloudTable); displayRate = 100; % Update display every 100 frames for n = 1 : skipFrames : numFrames % Read point cloud fileName = pointCloudTable.PointCloudFileName{n}; ptCloudOrig = helperReadPointCloudFromFile(fileName); % Process point cloud % - Segment and remove ground plane % - Segment and remove ego vehicle ptCloud = helperProcessPointCloud(ptCloudOrig); % Downsample the processed point cloud ptCloud = pcdownsample(ptCloud, "random", downsamplePercent); firstFrame = (n==1); if firstFrame % Add first point cloud scan as a view to the view set vSet = addView(vSet, viewId, absTform, "PointCloud", ptCloudOrig); viewId = viewId + 1; ptCloudPrev = ptCloud; continue; end % Use INS to estimate an initial transformation for registration initTform = helperComputeInitialEstimateFromINS(relTform, ... insDataTable(n-skipFrames:n, :)); % Compute rigid transformation that registers current point cloud with % previous point cloud relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize, ... "InitialTransform", initTform); % Update absolute transformation to reference frame (first point cloud) absTform = rigid3d( relTform.T * absTform.T ); % Add current point cloud scan as a view to the view set vSet = addView(vSet, viewId, absTform, "PointCloud", ptCloudOrig); % Add a connection from the previous view to the current view, representing % the relative transformation between them vSet = addConnection(vSet, viewId-1, viewId, relTform); viewId = viewId + 1; ptCloudPrev = ptCloud; initTform = relTform; if n>1 && mod(n, displayRate) == 1 plot(vSet, "Parent", hAxBefore); drawnow update end end %% % The view set object |vSet|, now holds views and connections. In the Views % table of vSet, the |AbsolutePose| variable specifies the absolute pose of % each view with respect to the first view. In the |Connections| table of % |vSet|, the |RelativePose| variable specifies relative constraints % between the connected views, the |InformationMatrix| variable specifies, % for each edge, the uncertainty associated with a connection. % Display the first few views and connections head(vSet.Views) head(vSet.Connections) %% % Now, build a point cloud map using the created view set. Align the view % absolute poses with the point clouds in the view set using |pcalign|. % Specify a grid size to control the resolution of the map. The map is % returned as a |pointCloud| object. ptClouds = vSet.Views.PointCloud; absPoses = vSet.Views.AbsolutePose; mapGridSize = 0.2; ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize); %% % Notice that the path traversed using this approach drifts over time. % While the path along the first loop back to the starting point seems % reasonable, the second loop drifts significantly from the starting point. % The accumulated drift results in the second loop terminating several % meters away from the starting point. % % A map built using odometry alone is inaccurate. Display the built point % cloud map with the traversed path. Notice that the map and traversed path % for the second loop are not consistent with the first loop. hFigAfter = figure('Name', 'View Set Display (before optimization)'); hAxAfter = axes(hFigAfter); pcshow(ptCloudMap, 'Parent', hAxAfter); % Overlay view set display hold on plot(vSetOptim, 'Parent', hAxAfter); helperMakeFigurePublishFriendly(hFigAfter);