В этом примере показано, как обработать 3-D данные о лидаре от датчика, смонтированного на транспортном средстве, чтобы прогрессивно создать карту и оценить траекторию транспортного средства с помощью одновременной локализации и картографии (SLAM). В дополнение к 3-D данным о лидаре инерционный датчик навигации (INS) также используется, чтобы помочь создать карту. Карты создали этот путь, может упростить планирование пути для навигации транспортного средства или может использоваться для локализации.
Сборка Карта из Данных о Лидаре (Automated Driving Toolbox) пример использует 3-D данные о лидаре и показания IMU, чтобы прогрессивно создать карту среды, пересеченной транспортным средством. В то время как этот подход создает локально сопоставимую карту, это подходит только для отображения небольших районов. По более длинным последовательностям дрейф накапливается в значительную ошибку. Чтобы преодолеть это ограничение, этот пример распознает ранее посещаемые места и пытается откорректировать для накопленного дрейфа с помощью графика подход SLAM.
Данные, используемые в этом примере, являются частью Набора данных SLAM Velodyne и представляют близко к 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. timetable
содержит Heading
Тангаж
Крен
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
Во-первых, используйте подход, объясненный в Сборке Карта из Данных о Лидаре (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
, теперь придерживается взглядов и связей. В таблице Views 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)
График SLAM является широко используемым методом для решения дрейфа в одометрии. График подход SLAM инкрементно создает график, где узлы соответствуют положениям транспортного средства и ребрам, представляет измерения датчика, ограничивающие связанные положения. Такой график называется графиком положения. График положения содержит ребра, которые кодируют противоречивую информацию, из-за шума или погрешностей в измерении. Узлы в созданном графике затем оптимизированы, чтобы найти набор положений транспортного средства, которые оптимально объясняют измерения. Этот метод называется оптимизацией графика положения.
Чтобы создать график положения из набора представления, можно использовать createPoseGraph
функция. Эта функция создает узел для каждого представления и ребро для каждой связи в наборе представления. Чтобы оптимизировать график положения, можно использовать optimizePoseGraph
(Navigation Toolbox) функция.
Ключевой аспект, способствующий эффективности графика, SLAM в исправлении дрейфа является точным обнаружением циклов, то есть, места, которые ранее посетили. Это называется обнаружением закрытия цикла или распознаванием места. Добавление ребер к графику положения, соответствующему закрытиям цикла, обеспечивает противоречащее измерение для связанных положений узла, которые могут быть разрешены во время оптимизации графика положения.
Закрытия цикла могут быть обнаружены с помощью дескрипторов, которые характеризуют окружение, видимое к датчику Лидара. Дескриптор Контекста Скана [1] является одним таким дескриптором, который может быть вычислен из облака точек с помощью scanContextDescriptor
функция. Этот пример использует helperLoopClosureDetector
класс, чтобы обнаружить закрытия цикла. Этот класс использует дескрипторы контекста скана для описания отдельных облаков точек и двухфазного алгоритма поиска дескриптора.
Расчет дескриптора: дескриптор контекста скана и кольцевой ключ извлечены из каждого облака точек с помощью scanContextDescriptor
функция.
Дескриптор, Соответствующий: В первой фазе кольцевой ключ используется, чтобы найти потенциальных кандидатов цикла. Во второй фазе самый близкий соседний поиск выполняется с помощью дескрипторов функции контекста скана среди потенциальных кандидатов цикла. 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);
В то время как точность может все еще быть улучшена, эта карта облака точек значительно более точна.
Г. Ким и А. Ким, "Контекст Скана: Эгоцентрический Пространственный Дескриптор для Распознавания Места В рамках 3D Карты Облака точек", 2018 Международных конференций IEEE/RSJ по вопросам Интеллектуальных Роботов и Систем (IROS), Мадрида, 2018, стр 4802-4809.
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
создает объект, который может использоваться, чтобы обнаружить закрытия цикла с помощью дескрипторов функции контекста скана.