build a map from lidar data using slam -凯发k8网页登录
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
the build a map from lidar data 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 the , 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 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
downloading scenario1.zip (153 mb) ...
use the helperreaddataset function to read data from the created folder in the form of a . 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 the pointcloudtable variable. to read the point cloud data from the image file, use the helperreadpointcloudfromfile function. this function takes an image file name and returns a object. the ins readings are read directly from a configuration file and stored in the insdatatable variable.
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. the timetable holds heading, pitch, roll, x, y, and z information 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 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 build a map from lidar data 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 = rigidtform3d; % absolute transformation to reference frame reltform = rigidtform3d; % 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 = rigidtform3d( abstform.a * reltform.a ); % 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)
viewid absolutepose pointcloud
______ ________________ ______________
1 1×1 rigidtform3d 1×1 pointcloud
2 1×1 rigidtform3d 1×1 pointcloud
3 1×1 rigidtform3d 1×1 pointcloud
4 1×1 rigidtform3d 1×1 pointcloud
5 1×1 rigidtform3d 1×1 pointcloud
6 1×1 rigidtform3d 1×1 pointcloud
7 1×1 rigidtform3d 1×1 pointcloud
8 1×1 rigidtform3d 1×1 pointcloud
viewid1 viewid2 relativepose informationmatrix
_______ _______ ________________ _________________
1 2 1×1 rigidtform3d {6×6 double}
2 3 1×1 rigidtform3d {6×6 double}
3 4 1×1 rigidtform3d {6×6 double}
4 5 1×1 rigidtform3d {6×6 double}
5 6 1×1 rigidtform3d {6×6 double}
6 7 1×1 rigidtform3d {6×6 double}
7 8 1×1 rigidtform3d {6×6 double}
8 9 1×1 rigidtform3d {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 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.
hold(haxbefore, 'on'); pcshow(ptcloudmap); hold(haxbefore, 'off'); close(haxbefore.parent)
correct drift using pose graph optimization
graph slam is 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 a pose 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 called pose graph optimization.
to create a pose graph from a view set, you can use the function. 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 the (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 called loop closure detection or place 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. the scan context descriptor [1] is one such descriptor that can be computed from a point cloud using the function. this example uses a object to manage the scan context descriptors that correspond to each view. it uses the object 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 reproducibility rng(0); % create an empty view set vset = pcviewset; % create a loop closure detector loopdetector = scancontextloopdetector; % create a figure for view set display hfigbefore = figure('name', 'view set display'); haxbefore = axes(hfigbefore); % initialize transformations abstform = rigidtform3d; % absolute transformation to reference frame reltform = rigidtform3d; % relative transformation between successive scans maxtolerablermse = 3; % maximum allowed rmse for a loop closure candidate to be accepted viewid = 1; 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); % extract the scan context descriptor from the first point cloud descriptor = scancontextdescriptor(ptcloudorig); % add the first descriptor to the loop closure detector adddescriptor(loopdetector, viewid, descriptor) 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 = rigidtform3d( abstform.a * reltform.a ); % 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); % extract the scan context descriptor from the point cloud descriptor = scancontextdescriptor(ptcloudorig); % add the descriptor to the loop closure detector adddescriptor(loopdetector, viewid, descriptor) % detect loop closure candidates loopviewid = detectloop(loopdetector); % a loop candidate was found if ~isempty(loopviewid) loopviewid = loopviewid(1); % retrieve point cloud from view set loopview = findview(vset, loopviewid); ptcloudorig = loopview.pointcloud; % process point cloud ptcloudold = helperprocesspointcloud(ptcloudorig); % downsample point cloud ptcloudold = pcdownsample(ptcloudold, "random", downsamplepercent); % use registration to estimate the relative pose [reltform, ~, rmse] = pcregisterndt(ptcloud, ptcloudold, ... reggridsize, "maxiterations", 50); acceptloopclosure = rmse <= maxtolerablermse; if acceptloopclosure % for simplicity, use a constant, small information matrix for % loop closure edges infomat = 0.01 * eye(6); % add a connection corresponding to a loop closure vset = addconnection(vset, loopviewid, viewid, reltform, infomat); end end viewid = viewid 1; ptcloudprev = ptcloud; inittform = reltform; if n>1 && mod(n, displayrate) == 1 hg = plot(vset, "parent", haxbefore); drawnow update end end
create a pose graph from the view set by using the method. the pose graph is a object 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]
in addition to the odometry connections between successive 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 connections xlim(haxbefore, [-50 50]); ylim(haxbefore, [-100 50]); % find and highlight loop closure connections loopedgeids = find(abs(diff(g.edges.endnodes, 1, 2)) > 1); highlight(hg, 'edges', loopedgeids, 'edgecolor', 'red', 'linewidth', 3)
optimize the pose graph using optimizeposegraph.
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 the function 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 display hold on plot(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
helperreaddataset reads data from specified folder into a timetable.
function datasettable = 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 order fileds = filedatastore(pointcloudfilepattern, 'readfcn', ... @helperreadpointcloudfromfile); % extract the file list from the datastore pointcloudfiles = fileds.files; imuconfigfile = fullfile(datafolder, 'scenario1', 'imu.cfg'); insdatatable = helperreadinsconfigfile(imuconfigfile); % delete the bad row from the ins config file insdatatable(1447, :) = []; % remove columns that will not be used datasettable = 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
helperprocesspointcloud processes a point cloud by removing points belonging to the ground plane and the ego vehicle.
function ptcloud = 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/findpointsincylinder. arguments ptcloudin (1,1) pointcloud method string {mustbemember(method, ["planefit","rangefloodfill"])} = "rangefloodfill" end isorganized = ~ismatrix(ptcloudin.location); if (method=="rangefloodfill" && isorganized) % segment ground using floodfill on range image groundfixedidx = segmentgroundfromlidardata(ptcloudin, ... "elevationangledelta", 11); else % segment ground as the dominant plane with reference normal % vector pointing in positive z-direction maxdistance = 0.4; maxangulardistance = 5; referencevector = [0 0 1]; [~, groundfixedidx] = pcfitplane(ptcloudin, maxdistance, ... referencevector, maxangulardistance); end if isorganized groundfixed = false(size(ptcloudin.location,1),size(ptcloudin.location,2)); else groundfixed = false(ptcloudin.count, 1); end groundfixed(groundfixedidx) = true; % segment ego vehicle as points within a cylindrical region of the sensor sensorlocation = [0 0 0]; egoradius = 3.5; egofixed = findpointsincylinder(ptcloudin,egoradius,"center",sensorlocation); % retain subset of point cloud without ground and ego vehicle if isorganized indices = ~groundfixed & ~egofixed; else indices = find(~groundfixed & ~egofixed); end ptcloud = select(ptcloudin, indices); end
helpercomputeinitialestimatefromins estimates an initial transformation for registration from ins readings.
function inittform = helpercomputeinitialestimatefromins(inittform, insdata) % if no ins readings are available, return if isempty(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.txt tnow = [-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 = rigidtform3d(t); end
helpermakefigurepublishfriendly adjusts figures so that screenshot captured by publish is correct.
function helpermakefigurepublishfriendly(hfig) if ~isempty(hfig) && isvalid(hfig) hfig.handlevisibility = 'callback'; end end
see also
functions
- | | |
objects
- | |
related topics
external websites
select a web site
choose a web site to get translated content where available and see local events and offers. based on your location, we recommend that you select: .
you can also select a web site from the following list:
americas
- (español)
- (english)
- (english)
europe
- (english)
- (english)
- (deutsch)
- (español)
- (english)
- (français)
- (english)
- (italiano)
- (english)
- (english)
- (english)
- (deutsch)
- (english)
- (english)
- switzerland
- (english)
asia pacific
- (english)
- (english)
- (english)
- 中国
- (日本語)
- (한국어)