Build a Map from Lidar Data Using SLAM

This example uses:

Open Script

This example shows how to process 3-D lidar data from a sensor mounted on a vehicle to progressively build a map and estimate the trajectory of a vehicle using simultaneous localization and mapping (SLAM). In addition to 3-D lidar data, an inertial navigation sensor (INS) is also used to help build the map. Maps built this way can facilitate path planning for vehicle navigation or can be used for localization.

Overview

TheBuild a Map from Lidar Data(Automated Driving Toolbox)example uses 3-D lidar data and IMU readings to progressively build a map of the environment traversed by a vehicle. While this approach builds a locally consistent map, it is suitable only for mapping small areas. Over longer sequences, the drift accumulates into a significant error. To overcome this limitation, this example recognizes previously visited places and tries to correct for the accumulated drift using the graph SLAM approach.

Load and Explore Recorded Data

The data used in this example is part of theVelodyne SLAM Dataset, and represents close to 6 minutes of recorded data. Download the data to a temporary directory.

Note:This download can take a few minutes.

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 fileunzip(zipFileName, dataFolder);elseiffolderExists && 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

Downloading scenario1.zip (153 MB) ...

Use thehelperReadDatasetfunction to read data from the created folder in the form of atimetable. The point clouds captured by the lidar are stored in the form of PNG image files. Extract the list of point cloud file names in thepointCloudTablevariable. To read the point cloud data from the image file, use thehelperReadPointCloudFromFilefunction. This function takes an image file name and returns apointCloudobject. The INS readings are read directly from a configuration file and stored in theinsDataTablevariable.

datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern); pointCloudTable = datasetTable(:, 1); insDataTable = datasetTable(:, 2:end);

Read the first point cloud and display it at the MATLAB® command prompt

ptCloud = helperReadPointCloudFromFile(pointCloudTable.PointCloudFileName{1}); disp(ptCloud)

pointCloud with properties: Location: [64×870×3 single] Count: 55680 XLimits: [-78.4980 77.7062] YLimits: [-76.8795 74.7502] ZLimits: [-2.4839 2.6836] Color: [] Normal: [] Intensity: []

Display the first INS reading. ThetimetableholdsHeading,Pitch,Roll,X,Y, andZinformation from the INS.

disp(insDataTable(1, :))

Timestamps Heading Pitch Roll X Y Z ____________________ _______ ________ _________ _______ _______ ______ 13-Jun-2010 06:27:31 1.9154 0.007438 -0.019888 -2889.1 -2183.9 116.47

Visualize the point clouds usingpcplayer, 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 playerxlimits = [-45 45];％metersylimits = [-45 45]; zlimits = [-10 20];％Create a streaming point cloud display objectlidarPlayer = pcplayer(xlimits, ylimits, zlimits);％Customize player axes labelsxlabel(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 sequenceskipFrames = 2; numFrames = height(pointCloudTable);forn = 1 : skipFrames : numFrames％Read a point cloudfileName = pointCloudTable.PointCloudFileName{n}; ptCloud = helperReadPointCloudFromFile(fileName);％Visualize point cloudview(lidarPlayer, ptCloud); pause(0.01)end

Build a Map Using Odometry

First, use the approach explained in theBuild a Map from Lidar Data(Automated Driving Toolbox)example to build a map. The approach consists of the following steps:

Align lidar scans:一致连续激光雷达扫描使用a point cloud registration technique. This example usespcregisterndtfor 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测程法.

Use apcviewsetobject 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 usingaddView.
To establish a connection between views useaddConnection. 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 theplotmethod 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 reproducibilityrng(0);％Create an empty view setvSet = pcviewset;％Create a figure for view set displayhFigBefore = figure('Name','View Set Display'); hAxBefore = axes(hFigBefore);％Initialize point cloud processing parametersdownsamplePercent = 0.1; regGridSize = 3;％Initialize transformationsabsTform = rigid3d;％Absolute transformation to reference framerelTform = rigid3d;％Relative transformation between successive scansviewId = 1; skipFrames = 5; numFrames = height(pointCloudTable); displayRate = 100;％Update display every 100 framesforn = 1 : skipFrames : numFrames％Read point cloudfileName = pointCloudTable.PointCloudFileName{n}; ptCloudOrig = helperReadPointCloudFromFile(fileName);％Process point cloud％- Segment and remove ground plane％- Segment and remove ego vehicleptCloud = helperProcessPointCloud(ptCloudOrig);％Downsample the processed point cloudptCloud = pcdownsample(ptCloud,"random", downsamplePercent); firstFrame = (n==1);iffirstFrame％Add first point cloud scan as a view to the view setvSet = addView(vSet, viewId, absTform,"PointCloud", ptCloudOrig); viewId = viewId + 1; ptCloudPrev = ptCloud;continue;end％Use INS to estimate an initial transformation for registrationinitTform = helperComputeInitialEstimateFromINS(relTform,...insDataTable(n-skipFrames:n, :));％Compute rigid transformation that registers current point cloud with％previous point cloudrelTform = 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 setvSet = addView(vSet, viewId, absTform,"PointCloud", ptCloudOrig);％Add a connection from the previous view to the current view, representing％the relative transformation between themvSet = addConnection(vSet, viewId-1, viewId, relTform); viewId = viewId + 1; ptCloudPrev = ptCloud; initTform = relTform;ifn>1 && mod(n, displayRate) == 1 plot(vSet,"Parent", hAxBefore); drawnowupdateendend

The view set objectvSet, now holds views and connections. In the Views table of vSet, theAbsolutePosevariable specifies the absolute pose of each view with respect to the first view. In theConnectionstable ofvSet,RelativePosevariable specifies relative constraints between the connected views, theInformationMatrixvariable specifies, for each edge, the uncertainty associated with a connection.

％Display the first few views and connectionshead(vSet.Views) head(vSet.Connections)

ans = 8×3 table ViewId AbsolutePose PointCloud ______ ____________ ______________ 1 1×1 rigid3d 1×1 pointCloud 2 1×1 rigid3d 1×1 pointCloud 3 1×1 rigid3d 1×1 pointCloud 4 1×1 rigid3d 1×1 pointCloud 5 1×1 rigid3d 1×1 pointCloud 6 1×1 rigid3d 1×1 pointCloud 7 1×1 rigid3d 1×1 pointCloud 8 1×1 rigid3d 1×1 pointCloud ans = 8×4 table ViewId1 ViewId2 RelativePose InformationMatrix _______ _______ ____________ _________________ 1 2 1×1 rigid3d {6×6 double} 2 3 1×1 rigid3d {6×6 double} 3 4 1×1 rigid3d {6×6 double} 4 5 1×1 rigid3d {6×6 double} 5 6 1×1 rigid3d {6×6 double} 6 7 1×1 rigid3d {6×6 double} 7 8 1×1 rigid3d {6×6 double} 8 9 1×1 rigid3d {6×6 double}

Now, build a point cloud map using the created view set. Align the view absolute poses with the point clouds in the view set usingpcalign. Specify a grid size to control the resolution of the map. The map is returned as apointCloudobject.

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.

hold(hAxBefore,'on'); pcshow(ptCloudMap); hold(hAxBefore,'off'); close(hAxBefore.Parent)

Correct Drift Using Pose Graph Optimization

Graph SLAMis a widely used technique for resolving the drift in odometry. The graph SLAM approach incrementally creates a graph, where nodes correspond to vehicle poses and edges represent sensor measurements constraining the connected poses. Such a graph is called apose graph. The pose graph contains edges that encode contradictory information, due to noise or inaccuracies in measurement. The nodes in the constructed graph are then optimized to find the set of vehicle poses that optimally explain the measurements. This technique is calledpose graph optimization.

To create a pose graph from a view set, you can use thecreatePoseGraphfunction. This function creates a node for each view, and an edge for each connection in the view set. To optimize the pose graph, you can use theoptimizePoseGraph(Navigation Toolbox)function.

A key aspect contributing to the effectiveness of graph SLAM in correcting drift is the accurate detection of loops, that is, places that have been previously visited. This is calledloop closure detectionorplace recognition. Adding edges to the pose graph corresponding to loop closures provides a contradictory measurement for the connected node poses, which can be resolved during pose graph optimization.

Loop closures can be detected using descriptors that characterize the local environment visible to the Lidar sensor. TheScan Contextdescriptor [1] is one such descriptor that can be computed from a point cloud using thescanContextDescriptorfunction. This example uses ascanContextLoopDetectorobject to manage the scan context descriptors that correspond to each view. It uses thedetectLoopobject function to detect loop closures with a two phase descriptor search algorithm. In the first phase, it computes the ring key subdescriptors to find potential loop candidates. In the second phase, it classifies views as loop closures by thresholding the scan context distance.

％Set random seed to ensure reproducibilityrng(0);％Create an empty view setvSet = pcviewset;％Create a loop closure detectorloopDetector = scanContextLoopDetector;％Create a figure for view set displayhFigBefore = figure('Name','View Set Display'); hAxBefore = axes(hFigBefore);％Initialize transformationsabsTform = rigid3d;％Absolute transformation to reference framerelTform = rigid3d;％Relative transformation between successive scansmaxTolerableRMSE = 3;％Maximum allowed RMSE for a loop closure candidate to be acceptedviewId = 1;forn = 1 : skipFrames : numFrames％Read point cloudfileName = pointCloudTable.PointCloudFileName{n}; ptCloudOrig = helperReadPointCloudFromFile(fileName);％Process point cloud％- Segment and remove ground plane％- Segment and remove ego vehicleptCloud = helperProcessPointCloud(ptCloudOrig);％Downsample the processed point cloudptCloud = pcdownsample(ptCloud,"random", downsamplePercent); firstFrame = (n==1);iffirstFrame％Add first point cloud scan as a view to the view setvSet = addView(vSet, viewId, absTform,"PointCloud", ptCloudOrig);％Extract the scan context descriptor from the first point clouddescriptor = scanContextDescriptor(ptCloudOrig);％Add the first descriptor to the loop closure detectoraddDescriptor(loopDetector, viewId, descriptor) viewId = viewId + 1; ptCloudPrev = ptCloud;continue;end％Use INS to estimate an initial transformation for registrationinitTform = helperComputeInitialEstimateFromINS(relTform,...insDataTable(n-skipFrames:n, :));％Compute rigid transformation that registers current point cloud with％previous point cloudrelTform = 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 setvSet = addView(vSet, viewId, absTform,"PointCloud", ptCloudOrig);％Add a connection from the previous view to the current view representing％the relative transformation between themvSet = addConnection(vSet, viewId-1, viewId, relTform);％Extract the scan context descriptor from the point clouddescriptor = scanContextDescriptor(ptCloudOrig);％Add the descriptor to the loop closure detectoraddDescriptor(loopDetector, viewId, descriptor)％Detect loop closure candidatesloopViewId = detectLoop(loopDetector);％A loop candidate was foundif~isempty(loopViewId) loopViewId = loopViewId(1);％Retrieve point cloud from view setloopView = findView(vSet, loopViewId); ptCloudOrig = loopView.PointCloud;％Process point cloudptCloudOld = helperProcessPointCloud(ptCloudOrig);％Downsample point cloudptCloudOld = pcdownsample(ptCloudOld,"random", downsamplePercent);％Use registration to estimate the relative pose[relTform, ~, rmse] = pcregisterndt(ptCloud, ptCloudOld,...regGridSize,"MaxIterations", 50); acceptLoopClosure = rmse <= maxTolerableRMSE;ifacceptLoopClosure％For simplicity, use a constant, small information matrix for％loop closure edgesinfoMat = 0.01 * eye(6);％Add a connection corresponding to a loop closurevSet = addConnection(vSet, loopViewId, viewId, relTform, infoMat);endendviewId = viewId + 1; ptCloudPrev = ptCloud; initTform = relTform;ifn>1 && mod(n, displayRate) == 1 hG = plot(vSet,"Parent", hAxBefore); drawnowupdateendend

Create a pose graph from the view set by using thecreatePoseGraphmethod. The pose graph is adigraphobject with:

Nodes containing the absolute pose of each view
Edges containing the relative pose constraints of each connection

G = createPoseGraph(vSet); disp(G)

digraph with properties: Edges: [539×3 table] Nodes: [503×2 table]

除了测程法苏之间的连接ccessive views, the view set now includes loop closure connections. For example, notice the new connections between the second loop traversal and the first loop traversal. These are loop closure connections. These can be identified as edges in the graph whose end nodes are not consecutive.

％Update axes limits to focus on loop closure connectionsxlim(hAxBefore, [-50 50]); ylim(hAxBefore, [-100 50]);％Find and highlight loop closure connectionsloopEdgeIds = find(abs(diff(G.Edges.EndNodes, 1, 2)) > 1); highlight(hG,'Edges', loopEdgeIds,'EdgeColor','red','LineWidth', 3)

Optimize the pose graph usingoptimizePoseGraph.

optimG = optimizePoseGraph(G,'g2o-levenberg-marquardt'); vSetOptim = updateView(vSet, optimG.Nodes);

Display the view set with optimized poses. Notice that the detected loops are now merged, resulting in a more accurate trajectory.

plot(vSetOptim,'Parent', hAxBefore)

The absolute poses in the optimized view set can now be used to build a more accurate map. Use thepcalignfunction to align the view set point clouds with the optimized view set absolute poses into a single point cloud map. Specify a grid size to control the resolution of the created point cloud map.

mapGridSize = 0.2; ptClouds = vSetOptim.Views.PointCloud; absPoses = vSetOptim.Views.AbsolutePose; ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize); hFigAfter = figure('Name','View Set Display (after optimization)'); hAxAfter = axes(hFigAfter); pcshow(ptCloudMap,'Parent', hAxAfter);％Overlay view set displayholdonplot(vSetOptim,'Parent', hAxAfter); helperMakeFigurePublishFriendly(hFigAfter);

While accuracy can still be improved, this point cloud map is significantly more accurate.

References

G. Kim and A. Kim, "Scan Context: Egocentric Spatial Descriptor for Place Recognition Within 3D Point Cloud Map,"2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, pp. 4802-4809.

Supporting Functions and Classes

helperReadDatasetreads data from specified folder into a timetable.

functiondatasetTable = helperReadDataset(dataFolder, pointCloudFilePattern)％helperReadDataset Read Velodyne SLAM Dataset data into a timetable％datasetTable = helperReadDataset(dataFolder) reads data from the％folder specified in dataFolder into a timetable. The function％expects data from the Velodyne SLAM Dataset.％％See also fileDatastore, helperReadINSConfigFile.％Create a file datastore to read in files in the right orderfileDS = fileDatastore(pointCloudFilePattern,'ReadFcn',...@helperReadPointCloudFromFile);％Extract the file list from the datastorepointCloudFiles = fileDS.Files; imuConfigFile = fullfile(dataFolder,'scenario1','imu.cfg'); insDataTable = helperReadINSConfigFile(imuConfigFile);％Delete the bad row from the INS config fileinsDataTable(1447, :) = [];％Remove columns that will not be useddatasetTable = removevars(insDataTable,...{'Num_Satellites','Latitude','Longitude','Altitude','Omega_Heading',...'Omega_Pitch','Omega_Roll','V_X','V_Y','V_ZDown'}); datasetTable = addvars(datasetTable, pointCloudFiles,'Before', 1,...'NewVariableNames',"PointCloudFileName");end

helperProcessPointCloudprocesses a point cloud by removing points belonging to the ground plane and the ego vehicle.

functionptCloud = helperProcessPointCloud(ptCloudIn, method)％helperProcessPointCloud Process pointCloud to remove ground and ego vehicle％ptCloud = helperProcessPointCloud(ptCloudIn, method) processes％ptCloudIn by removing the ground plane and the ego vehicle.％method can be "planefit" or "rangefloodfill".％％See also pcfitplane, pointCloud/findNeighborsInRadius.argumentsptCloudIn(1,1) pointCloudmethodstring {mustBeMember(method, ["planefit","rangefloodfill"])}="rangefloodfill"endisOrganized = ~ismatrix(ptCloudIn.Location);if(method=="rangefloodfill"&& isOrganized)％Segment ground using floodfill on range imagegroundFixedIdx = segmentGroundFromLidarData(ptCloudIn,..."ElevationAngleDelta", 11);else％Segment ground as the dominant plane with reference normal％vector pointing in positive z-directionmaxDistance = 0.4;maxAngularDistance = 5;referenceVector = [0 0 1]; [~, groundFixedIdx] = pcfitplane(ptCloudIn, maxDistance,...referenceVector, maxAngularDistance);endifisOrganized groundFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));elsegroundFixed = false(ptCloudIn.Count, 1);endgroundFixed(groundFixedIdx) = true;％Segment ego vehicle as points within a given radius of sensorsensorLocation = [0 0 0]; radius = 3.5; egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius);ifisOrganized egoFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));elseegoFixed = false(ptCloudIn.Count, 1);endegoFixed(egoFixedIdx) = true;％Retain subset of point cloud without ground and ego vehicleifisOrganized indices = ~groundFixed & ~egoFixed;elseindices = find(~groundFixed & ~egoFixed);endptCloud = select(ptCloudIn, indices);end

helperComputeInitialEstimateFromINS估计一个初始registrati转换on from INS readings.

functioninitTform = helperComputeInitialEstimateFromINS(initTform, insData)％If no INS readings are available, returnifisempty(insData)return;end％The INS readings are provided with X pointing to the front, Y to the left％and Z up. Translation below accounts for transformation into the lidar％frame.insToLidarOffset = [0 -0.79 -1.73];％See DATAFORMAT.txtTnow = [-insData.Y(end), insData.X(end), insData.Z(end)].' + insToLidarOffset'; Tbef = [-insData.Y(1) , insData.X(1) , insData.Z(1)].' + insToLidarOffset';％Since the vehicle is expected to move along the ground, changes in roll％and pitch are minimal. Ignore changes in roll and pitch, use heading only.Rnow = rotmat(quaternion([insData.Heading(end) 0 0],'euler','ZYX','point'),'point'); Rbef = rotmat(quaternion([insData.Heading(1) 0 0],'euler','ZYX','point'),'point'); T = [Rbef Tbef;0 0 0 1] \ [Rnow Tnow;0 0 0 1]; initTform = rigid3d(T.');end

helperMakeFigurePublishFriendlyadjusts figures so that screenshot captured by publish is correct.

functionhelperMakeFigurePublishFriendly(hFig)if~isempty(hFig) && isvalid(hFig) hFig.HandleVisibility ='callback';endend

External Websites

Velodyne SLAM Dataset

Build a Map from Lidar Data Using SLAM

Overview

Load and Explore Recorded Data

Build a Map Using Odometry

Correct Drift Using Pose Graph Optimization

References

Supporting Functions and Classes

See Also

Functions

Objects

Related Topics

External Websites