Этот пример показов, как обработать 3-D данные лидары с датчика, установленного на транспортном средстве, чтобы постепенно создавать карту и оценивать траекторию автомобиля, используя одновременную локализацию и картографию (SLAM). В сложение 3-D лидаров данных для помощи в построении карты также используется датчик инерционной навигации (INS). Карты, построенные таким образом, могут облегчить планирование пути для навигации по транспортному средству или могут использоваться для локализации.
В примере Build a Map from Lidar Data (Automated Driving Toolbox) используются данные 3-D лидара и показания БИНС, чтобы постепенно создать карту окружений, пройденных транспортным средством. Хотя этот подход создает локально последовательную карту, он подходит только для отображения небольших районов. В более длинных последовательностях дрейф накапливается в значительную ошибку. Чтобы преодолеть это ограничение, этот пример распознает ранее посещенные места и пытается исправить накопленный дрейф с помощью графиков SLAM.
Данные, используемые в этом примере, являются частью набора данных Velodyne SLAM и представляют почти 6 минут записанных данных. Загрузите данные во временную директорию.
Примечание: Эта загрузка может занять несколько минут.
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"; 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); end
Используйте helperReadDataset
функция для чтения данных из созданной папки в виде timetable
. Облака точек, захваченные лидаром, хранятся в виде файлов изображений PNG. Извлеките список имен файлов облака точек в pointCloudTable
переменная. Чтобы считать данные облака точек из файла изображения, используйте helperReadPointCloudFromFile
функция. Эта функция принимает имя файла изображения и возвращает pointCloud
объект. Показания INS считываются непосредственно из файла строения и хранятся в insDataTable
переменная.
datasetTable = helperReadDataset(dataFolder); pointCloudTable = datasetTable(:, 1); insDataTable = datasetTable(:, 2:end);
Прочтите первое облако точек и отобразите его в командной строке MATLAB ®
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: []
Отображение первого чтения INS. The timetable
содержит Heading
, Pitch
, Roll
, X
, Y
, и Z
информация от 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
Визуализация облаков точек с помощью pcplayer
, отображение облака точек потоковой передачи. Это транспортное средство проходит по пути, состоящему из двух циклов. В первом цикле транспортного средства делает ряд поворотов и возвращается к начальной точке. Во втором цикле транспортного средства делает ряд поворотов по другому маршруту и снова возвращается к начальной точке.
% 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 from Lidar Data (Automated Driving Toolbox), чтобы создать карту. Подход состоит из следующих шагов:
Выровнять сканы лидара: Выровнять последующие сканы лидара с помощью метода регистрации облака точек. Этот пример использует pcregisterndt
для регистрации сканов. Путем последовательного компиляции этих преобразований каждое облако точек преобразуется назад в опорную систему координат первого облака точек.
Объедините выровненные сканы: Сгенерируйте карту путем объединения всех преобразованных облаков точек.
Этот подход пошагового создания карты и оценки траектории транспортного средства называется одометрией.
Использование pcviewset
объект для хранения данных и управления ими в нескольких представлениях. Набор видов состоит из набора связанных видов.
Каждое представление хранит информацию, связанную с одним представлением. Эта информация включает абсолютное положение представления, данные датчика облака точек, захваченные в этом представлении, и уникальный идентификатор представления. Добавить представления в набор видов можно используя команду addView
.
Чтобы установить связь между представлениями, используйте addConnection
. Связь хранит информацию, подобную относительному преобразованию между связывающими представлениями, неопределенностью, связанной с вычислением этого измерения (представленной в виде информационной матрицы), и связанными идентификаторами представления.
Используйте plot
метод визуализации соединений, установленных набором видов. Эти соединения могут использоваться, чтобы визуализировать путь, пройденный транспортным средством.
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
Объект набора представлений vSet
, теперь содержит представления и связи. В таблице Представления vSet AbsolutePose
переменная задает абсолютное положение каждого вида относительно первого вида. В Connections
таблица vSet
, а RelativePose
переменная задает относительные ограничения между связанными видами, InformationMatrix
переменная задает для каждого ребра неопределенность, связанную с соединением.
% Display the first few views and connections
head(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}
Теперь создайте карту облака точек с помощью созданного набора представлений. Выровнять абсолютные положения вида по облакам точек в наборе видов можно используя pcalign
. Задайте размер сетки, чтобы контролировать разрешение карты. Карта возвращается как pointCloud
объект.
ptClouds = vSet.Views.PointCloud; absPoses = vSet.Views.AbsolutePose; mapGridSize = 0.2; ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);
Заметьте, что путь, пройденный с помощью этого подхода, дрейфует с течением времени. Хотя путь вдоль первого цикла назад к начальной точке кажется разумным, второй цикл значительно дрейфует от начальной точки. Накопленный дрейф результатов во втором цикле, заканчивающемся на несколько метров от начальной точки.
Карта, созданная только с использованием одометрии, неточна. Отобразите построенную карту облака точек с пройденным путем. Заметьте, что карта и пройденный путь для второго цикла не согласуются с первым циклом.
hold(hAxBefore, 'on'); pcshow(ptCloudMap); hold(hAxBefore, 'off'); close(hAxBefore.Parent)
Graph SLAM является широко используемым методом для разрешения дрейфа в одометрии. График SLAM пошагово создает график, где узлы соответствуют положениям транспортного средства, а ребра представляют измерения датчика, ограничивающие связанные положения. Такой график называется графиком положения. График положения содержит ребра, которые кодируют противоречивую информацию, из-за шума или неточностей в измерении. Узлы в построенном графику затем оптимизируются, чтобы найти набор положений транспортного средства, который оптимально объясняет измерения. Этот метод называется оптимизацией графика положения.
Чтобы создать график положения из набора видов, можно использовать createPoseGraph
функция. Эта функция создает узел для каждого вида и ребро для каждого соединения в наборе видов. Чтобы оптимизировать график положения, можно использовать optimizePoseGraph
(Navigation Toolbox) функция.
Ключевым аспектом, способствующим эффективности графика SLAM в корректировке дрейфа, является точное обнаружение петель, то есть мест, которые были ранее посещены. Это называется обнаружением замыкания цикла или распознаванием места. Добавление ребер к графику положения, соответствующему замыканиям цикла, обеспечивает противоречивое измерение для связанных положений узла, которое может быть разрешено во время оптимизации графика положения.
Замыкания цикла могут быть обнаружены с помощью дескрипторов, которые характеризуют локальное окружение, видимую датчику Лидара. Дескриптор контекста скана [1] является одним из таких дескрипторов, который можно вычислить из облака точек с помощью scanContextDescriptor
функция. Этот пример использует helperLoopClosureDetector
класс для обнаружения замыканий цикла. Этот класс использует контекстные дескрипторы скана для описания отдельных облаков точек и алгоритм поиска двухфазного дескриптора.
Дескриптор Расчета: Сканы контекстный дескриптор и звонок ключ извлекаются из каждого облака точек с помощью scanContextDescriptor
функция.
Соответствие дескриптора: В первой фазе ключ звонка используется для поиска потенциальных кандидатов цикла. Во второй фазе поиск по ближайшему соседу выполняется с использованием дескрипторов скана контекста функции среди потенциальных кандидатов цикла. The scanContextDistance
функция используется для вычисления расстояния между двумя контекстными дескрипторами скана. Смотрите helperFeatureSearcher
Класс для получения дополнительной информации.
% Set random seed to ensure reproducibility rng(0); % Create an empty view set vSet = pcviewset; % Create a loop closure detector matchThresh = 0.08; loopDetector = helperLoopClosureDetector('MatchThreshold', matchThresh); % Create a figure for view set display hFigBefore = figure('Name', 'View Set Display'); hAxBefore = axes(hFigBefore); % Initialize transformations absTform = rigid3d; % Absolute transformation to reference frame relTform = rigid3d; % 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); 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); % Detect loop closure candidates [loopFound, loopViewId] = detectLoop(loopDetector, ptCloudOrig); % A loop candidate was found if loopFound loopViewId = loopViewId(1); % Retrieve point cloud from view set ptCloudOrig = vSet.Views.PointCloud( find(vSet.Views.ViewId == loopViewId, 1) ); % 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
Создайте график положения из набора видов с помощью createPoseGraph
способ. График положения является digraph
объект с:
Узлы, содержащие абсолютное положение каждого представления
Ребра, содержащие ограничения относительного положения каждого соединения
G = createPoseGraph(vSet); disp(G)
digraph with properties: Edges: [507×3 table] Nodes: [503×2 table]
В дополнение к соединениям одометрии между последовательными видами, набор видов теперь включает соединения замыкания цикла. Например, заметьте новые соединения между обходом второго цикла и обходом первого цикла. Это соединения замыкания цикла. Они могут быть идентифицированы как ребра в графике, конечные узлы которого не являются последовательными.
% 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)
Оптимизируйте график положения с помощью optimizePoseGraph
.
optimG = optimizePoseGraph(G, 'g2o-levenberg-marquardt');
vSetOptim = updateView(vSet, optimG.Nodes);
Отобразите набор видов с оптимизированными положениями. Заметьте, что обнаруженные циклы теперь объединены, что приводит к более точной траектории.
plot(vSetOptim, 'Parent', hAxBefore)
Абсолютные положения в оптимизированном наборе представлений теперь могут использоваться, чтобы создать более точную карту. Используйте pcalign
функция для выравнивания облаков заданных точек представления с оптимизированным представлением задает абсолютные положения в одну карту облака точек. Задайте размер сетки, чтобы контролировать разрешение созданной карты облака точек.
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);
Хотя точность все еще может быть улучшена, эта карта облака точек является значительно более точной.
G. Kim and A. Kim, «Scan Context: Egocentric Spatial Descriptor for Place Recognition with 3D Point Cloud Map», 2018, IEEE/RSJ International Conference on Intelligent Robots and and SyStStems (IStems), 2018, Mros.
helperReadDataset
считывает данные из указанной папки в расписание.
function datasetTable = helperReadDataset(dataFolder) %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. % Point clouds are stored as PNG files in the scenario1 folder pointCloudFilePattern = fullfile(dataFolder, 'scenario1', 'scan*.png'); % 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
обрабатывает облако точек путем удаления точек, принадлежащих наземной плоскости и автомобилю , оборудованному датчиком.
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/findNeighborsInRadius. 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 given radius of sensor sensorLocation = [0 0 0]; radius = 3.5; egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius); if isOrganized egoFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2)); else egoFixed = false(ptCloudIn.Count, 1); end egoFixed(egoFixedIdx) = true; % 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
оценивает начальное преобразование для регистрации из показаний INS.
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 = rigid3d(T.'); end
helperMakeFigurePublishFriendly
настраивает рисунки так, чтобы скриншот, захваченный публикацией, был правильным.
function helperMakeFigurePublishFriendly(hFig) if ~isempty(hFig) && isvalid(hFig) hFig.HandleVisibility = 'callback'; end end
helperFeatureSearcher
создает объект, который может использоваться для поиска ближайших совпадений функций.
helperLoopClosureDetector
создает объект, который может использоваться для обнаружения закрытий цикла с помощью скана контекстных функций дескрипторов.