Воздушный лидар SLAM с использованием дескрипторов FPFH

Этот пример использует:

Открыть Live Script

Этот пример демонстрирует, как реализовать алгоритм одновременной локализации и картографии (SLAM) для воздушного отображения с использованием 3-D функций. Цель этого примера состоит в том, чтобы оценить траекторию робота и создать карту облака точек его окружения.

Алгоритм SLAM в этом примере оценивает траекторию путем нахождения грубого выравнивания между уменьшенными принятыми сканами, используя дескрипторы гистограммы функций быстрой точки (FPFH), извлеченные в каждой точке, затем применяет итерационный алгоритм ближайшей точки (ICP), чтобы точно настроить выравнивание. 3-D оптимизации графика положения из Navigation Toolbox™ уменьшает дрейф в предполагаемой траектории.

Создание и визуализация данных

Создайте синтетические сканы лидара из закрашенной фигуры с воздушными данными, загруженного с сайта OpenTopography [1]. Загрузите MAT-файл, содержащий последовательность путевых точек по воздушным данным, которая определяет траекторию робота. Вычислите сканы лидара из данных в каждой путевой точке с помощью helperCreateDataset функция. Функция выводит сканы лидара, вычисленные в каждой путевой точке как массив pointCloud объекты, оригинальная карта, покрытая роботом и основной истиной путевыми точками.

datafile = 'aerialMap.tar.gz';
wayPointsfile = 'gTruthWayPoints.mat';

% Generate a lidar scan at each waypoint using the helper function
[pClouds,orgMap,gTruthWayPts] = helperCreateDataset(datafile,wayPointsfile);

Визуализируйте основную истину путевые точки на исходной карте, покрытой роботом.

% Create a figure window to visualize the ground truth map and waypoints 
hFigGT = figure;
axGT = axes('Parent',hFigGT,'Color','black');

% Visualize the ground truth waypoints
pcshow(gTruthWayPts,'red','MarkerSize',150,'Parent',axGT)
hold on

% Visualize the original map covered by the robot
pcshow(orgMap,'MarkerSize',10,'Parent',axGT)
hold off

% Customize the axis labels
xlabel(axGT,'X (m)')
ylabel(axGT,'Y (m)')
zlabel(axGT,'Z (m)')
title(axGT,'Ground Truth Map And Robot Trajectory')

Визуализируйте извлеченные сканы лидара с помощью pcplayer объект.

% Specify limits for the player
xlimits = [-90 90];
ylimits = [-90 90];
zlimits = [-625 -587];

% Create a pcplayer object to visualize the lidar scans
lidarPlayer = pcplayer(xlimits,ylimits,zlimits);

% Customize the pcplayer axis labels
xlabel(lidarPlayer.Axes,'X (m)')
ylabel(lidarPlayer.Axes,'Y (m)')
zlabel(lidarPlayer.Axes,'Z (m)')
title(lidarPlayer.Axes,'Lidar Scans')

% Loop over and visualize the data
for l = 1:length(pClouds)
    
    % Extract the lidar scan
    ptCloud = pClouds(l);
    
    % Update the lidar display
    view(lidarPlayer,ptCloud)
    pause(0.05)
end

Настройка настраиваемых параметров

Задайте параметры для оценки траектории и замыкания цикла. Настройте эти параметры для вашего конкретного робота и окружения.

Параметры для регистрации облака точек

Укажите количество сканов лидара, которые будут пропущены между каждым сканом, принятым для регистрации. Поскольку последующие сканы имеют высокое перекрытие, пропуск нескольких систем координат может улучшить скорость алгоритма, не ставя под угрозу точность.

skipFrames = 3;

Сканы лидара с понижающей дискретизацией могут улучшить скорость алгоритма. Прямоугольный сеточный фильтр понижает значение облака точек путем усреднения всех точек в каждой камере до одной точки. Задайте размер отдельных камер коробчатого сеточного фильтра в метрах.

gridStep = 1.5; % in meters

Дескрипторы FPFH извлекаются для каждой допустимой точки в облаке точек с понижающей дискретизацией. Укажите размер окрестности для метода поиска k-ближайших соседей (KNN), используемого для вычисления дескрипторов.

neighbors = 60;

Установите порог и отношение для соответствия дескрипторов FPFH, используемых для идентификации соответствия точек.

matchThreshold = 0.1;
matchRatio = 0.97;

Установите максимальное расстояние и количество маршрутов для относительной оценки положения между последовательными сканами.

maxDistance = 1;
maxNumTrails = 3000;

Укажите процент инлиеров, которые нужно учитывать для подстройки относительных положений.

inlierRatio = 0.1;

Задайте размер каждой камеры коробчатого сеточного фильтра, используемого для создания карт путем выравнивания сканов лидара.

alignGridStep = 1.2;

Параметры для оценки замыкания цикла

Задайте радиус вокруг текущего местоположения робота, чтобы найти кандидатов замыкания цикла.

loopClosureSearchRadius = 7.9;

Алгоритм замыкания цикла основан на создании и 3-D подкарты. Подкарта состоит из заданного количества принятых сканов nScansPerSubmap. Алгоритм закрытия цикла также игнорирует указанное количество самых последних сканов subMapThresh, во время поиска кандидатов циклов. Задайте корневую среднюю квадратичную невязку (RMSE) loopClosureThreshold, для принятия подкарты в качестве совпадающей. Более низкий счет может указывать на лучшее соответствие, но счета варьируются на основе данных о датчике и предварительной обработки.

nScansPerSubmap = 3;
subMapThresh = 15;
loopClosureThreshold = 0.6;

Задайте максимально допустимую среднюю квадратичную невязку корня (RMSE) для оценки относительного положения между кандидатами цикла rmseThreshold. Выбор более низкого значения может привести к более точным ребрам замыкания цикла, что оказывает высокое влияние на оптимизацию графика положения, но счета варьируются на основе данных о датчике и предварительной обработки.

rmseThreshold = 0.6;

Инициализация переменных

Создайте график положения, используя poseGraph3D Объект (Navigation Toolbox) для хранения предполагаемых относительных положений между принятыми сканами лидара.

pGraph = poseGraph3D;

% Default serialized upper-right triangle of a 6-by-6 Information Matrix
infoMat = [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1];

Предварительно выделите и инициализируйте переменные, необходимые для расчетов.

% Allocate memory to store submaps
subMaps = cell(floor(length(pClouds)/(skipFrames*nScansPerSubmap)),1);

% Allocate memory to store each submap pose
subMapPoses = zeros(round(length(pClouds)/(skipFrames*nScansPerSubmap)),3);

% Initialize variables to store accepted scans and their transforms for
% submap creation
pcAccepted = pointCloud.empty(0);
tformAccepted = rigid3d.empty(0);

% Initialize variable to store relative poses from the feature-based approach
% without pose graph optimization
fpfhTform = rigid3d.empty(0);

% Counter to track the number of scans added
count = 1;

Создайте переменные, чтобы визуализировать обработанные сканы лидара и предполагаемую траекторию.

% Set to 1 to visualize processed lidar scans during build process
viewPC = 0;

% Create a pcplayer object to view the lidar scans while
% processing them sequentially, if viewPC is enabled
if viewPC == 1
    pplayer = pcplayer(xlimits,ylimits,zlimits);
    
    % Customize player axis labels
    xlabel(pplayer.Axes,'X (m)')
    ylabel(pplayer.Axes,'Y (m)')
    zlabel(pplayer.Axes,'Z (m)')
    title(pplayer.Axes,'Processed Scans')
end

% Create a figure window to visualize the estimated trajectory
hFigTrajUpdate = figure;
axTrajUpdate = axes('Parent',hFigTrajUpdate,'Color','black');
title(axTrajUpdate,'Sensor Pose Trajectory')

Оценка и уточнение траектории

Траектория робота - это набор его положений, состоящий из его расположения и ориентации в трехмерном пространстве. Оцените положение робота из образца сканирования 3-D лидара с помощью 3-D алгоритма SLAM. Используйте эти процессы для выполнения 3-D оценки SLAM:

  1. Понижающая дискретизация облака точек

  2. Регистрация облака точек

  3. Создание подкарты

  4. Запрос замыкания цикла

  5. Оптимизация графика положения

Итерационно обработайте сканы лидара, чтобы оценить траекторию робота.

rng('default') % Set random seed to guarantee consistent results in pcmatchfeatures
for FrameIdx = 1:skipFrames:length(pClouds)

Понижающая дискретизация облака точек

Понижающая дискретизация облака точек может улучшить скорость алгоритма регистрации облака точек. Понижающая дискретизация должна быть настроена на ваши конкретные потребности.

    % Downsample the current scan
    curScan = pcdownsample(pClouds(FrameIdx),'gridAverage',gridStep);
    if viewPC == 1
        
        % Visualize down sampled point cloud
        view(pplayer,curScan)
    end

Регистрация облака точек

Регистрация облака точек оценивает относительное положение между текущим сканом и предыдущим сканом. Пример использует следующие шаги для регистрации:

  1. Извлекает дескрипторы FPFH из каждого скана с помощью extractFPFHFeatures функция

  2. Идентифицирует соответствия точек путем сравнения дескрипторов с помощью pcmatchfeatures функция

  3. Оценивает относительное положение из соответствия точек с помощью estimateGeometricTransform3D функция

  4. Точная настройка относительного положения с помощью pcregistericp функция

    % Extract FPFH features
    curFeature = extractFPFHFeatures(curScan,'NumNeighbors',neighbors);
    
    if FrameIdx == 1
        
        % Update the acceptance scan and its tform
        pcAccepted(count,1) = curScan;
        tformAccepted(count,1) = rigid3d;
        
        % Update the initial pose to the first waypoint of ground truth for
        % comparison
        fpfhTform(count,1) = rigid3d(eye(3),gTruthWayPts(1,:));
    else
        
        % Identify correspondences by matching current scan to previous scan 
        indexPairs = pcmatchfeatures(curFeature,prevFeature,curScan,prevScan, ...
            'MatchThreshold',matchThreshold,'RejectRatio',matchRatio);
        matchedPrevPts = select(prevScan,indexPairs(:,2));
        matchedCurPts = select(curScan,indexPairs(:,1));
        
        % Estimate relative pose between current scan and previous scan 
        % using correspondences
        tform1 = estimateGeometricTransform3D(matchedCurPts.Location, ...
            matchedPrevPts.Location,'rigid','MaxDistance',maxDistance, ...
            'MaxNumTrials',maxNumTrails);

        % Perform ICP registration to fine-tune relative pose
        tform = pcregistericp(curScan,prevScan,'InitialTransform',tform1, ...
            'InlierRatio',inlierRatio);

Преобразуйте твердое преобразование в xyz-положение и кватернионную ориентацию, чтобы добавить его к графику положения. 

        relPose = [tform2trvec(tform.T') tform2quat(tform.T')];
        
        % Add relative pose to pose graph
        addRelativePose(pGraph,relPose);

Сохраните принятые сканы и их положения для создания подкарты.

        % Update counter and store accepted scans and their poses
        count = count + 1;
        pcAccepted(count,1) = curScan;
        accumPose = pGraph.nodes(height(pGraph.nodes));
        tformAccepted(count,1) = rigid3d((trvec2tform(accumPose(1:3)) * ...
            quat2tform(accumPose(4:7)))');
        
        % Update estimated poses
        fpfhTform(count) = rigid3d(tform.T * fpfhTform(count-1).T);
    end

Создание подкарты

Создайте подкарты путем выравнивания последовательных, принятых сканов друг с другом в группах заданного размера nScansPerSubmap, с использованием pcalign функция. Использование подкартов может привести к более быстрым запросам на закрытие цикла.

    currSubMapId = floor(count/nScansPerSubmap);
    if rem(count,nScansPerSubmap) == 0
        
        % Align an array of lidar scans to create a submap
        subMaps{currSubMapId} = pcalign(...
            pcAccepted((count - nScansPerSubmap + 1):count,1), ...
            tformAccepted((count - nScansPerSubmap + 1):count,1), ...
            alignGridStep);
        
        % Assign center scan pose as pose of submap
        subMapPoses(currSubMapId,:) = tformAccepted(count - ...
            floor(nScansPerSubmap/2),1).Translation;
    end

Запрос на закрытие цикла

Используйте запрос замыкания цикла, чтобы идентифицировать ранее посещенные места. Запрос на закрытие цикла состоит из нахождения подобия между текущим сканом и предыдущими подкартами. Если вы находите аналогичный скан, то текущий скан является кандидатом на закрытие цикла. Оценка кандидата на закрытие цикла состоит из следующих шагов:

  1. Оценка соответствует между предыдущими подкартами и текущим сканом с помощью helperEstimateLoopCandidates функция. Подчиненная карта соответствует, если RMSE между текущим сканом и подкартой меньше заданного значения loopClosureThreshold. Предыдущие сканы, представленные всеми соответствующими подкартами, являются кандидатами на закрытие цикла.

  2. Вычислите относительное положение между текущим сканом и кандидатом на закрытие цикла с помощью алгоритма регистрации ICP. Кандидат на закрытие цикла с самым низким RMSE является наилучшим вероятным соответствием для текущего скана.

Кандидат на закрытие цикла принимается как допустимое закрытие цикла только, когда RMSE ниже заданного порога.

    if currSubMapId > subMapThresh
        mostRecentScanCenter = pGraph.nodes(height(pGraph.nodes));
        
        % Estimate possible loop closure candidates by matching current
        % scan with submaps
        [loopSubmapIds,~] = helperEstimateLoopCandidates(subMaps,curScan, ...
            subMapPoses,mostRecentScanCenter,currSubMapId,subMapThresh, ...
            loopClosureSearchRadius,loopClosureThreshold);
        
        if ~isempty(loopSubmapIds)
            rmseMin = inf;
            
            % Estimate the best match for the current scan from the matching submap ids
            for k = 1:length(loopSubmapIds)
                
                % Check every scan within the submap
                for kf = 1:nScansPerSubmap
                    probableLoopCandidate = ...
                        loopSubmapIds(k)*nScansPerSubmap - kf + 1;
                    [pose_Tform,~,rmse] = pcregistericp(curScan, ...
                        pcAccepted(probableLoopCandidate));
                    
                    % Update the best loop closure candidate
                    if rmse < rmseMin
                        rmseMin = rmse;
                        matchingNode = probableLoopCandidate;
                        Pose = [tform2trvec(pose_Tform.T') ...
                            tform2quat(pose_Tform.T')];
                    end
                end
            end
            
            % Check if loop closure candidate is valid
            if rmseMin < rmseThreshold
                
                % Add relative pose of loop closure candidate to pose graph
                addRelativePose(pGraph,Pose,infoMat,matchingNode, ...
                    pGraph.NumNodes);
            end
        end
    end
    
    % Update previous point cloud and feature
    prevScan = curScan;
    prevFeature = curFeature;

    % Visualize the estimated trajectory from the accepted scan.
    show(pGraph,'IDs','off','Parent',axTrajUpdate);
    drawnow
end

Оптимизация графика положения

Уменьшите дрейф в предполагаемой траектории при помощи optimizePoseGraph (Navigation Toolbox) функция. В целом положение первого узла в графике положения представляет источник. Чтобы сравнить предполагаемую траекторию с основной истиной путевыми точками, задайте первую основную истину путевую точку в качестве положения первого узла.

pGraph = optimizePoseGraph(pGraph,'FirstNodePose',[gTruthWayPts(1,:) 1 0 0 0]);

Визуализация сравнений траекторий

Визуализируйте предполагаемую траекторию, используя функции без оптимизации графика положения, функции с оптимизацией графика положения и достоверные данные.

% Get estimated trajectory from pose graph
pGraphTraj = pGraph.nodes;

% Get estimated trajectory from feature-based registration without pose
% graph optimization
fpfhEstimatedTraj = zeros(count,3);
for i = 1:count
    fpfhEstimatedTraj(i,:) = fpfhTform(i).Translation;
end

% Create a figure window to visualize the ground truth and estimated
% trajectories
hFigTraj = figure;
axTraj = axes('Parent',hFigTraj,'Color','black');
plot3(fpfhEstimatedTraj(:,1),fpfhEstimatedTraj(:,2),fpfhEstimatedTraj(:,3), ...
    'r*','Parent',axTraj)
hold on
plot3(pGraphTraj(:,1),pGraphTraj(:,2),pGraphTraj(:,3),'b.','Parent',axTraj)
plot3(gTruthWayPts(:,1),gTruthWayPts(:,2),gTruthWayPts(:,3),'go','Parent',axTraj)
hold off
axis equal
view(axTraj,2)
xlabel(axTraj,'X (m)')
ylabel(axTraj,'Y (m)')
zlabel(axTraj,'Z (m)')
title(axTraj,'Trajectory Comparison')
legend(axTraj,'Estimated trajectory without pose graph optimization', ...
    'Estimated trajectory with pose graph optimization', ...
    'Ground Truth Trajectory','Location','bestoutside')

Построение и визуализация сгенерированной карты

Преобразуйте и объедините сканы лидара, используя предполагаемые положения, чтобы создать накопленную карту облака точек.

% Get the estimated trajectory from poses
estimatedTraj = pGraphTraj(:,1:3);

% Convert relative poses to rigid transformations
estimatedTforms = rigid3d.empty(0);
for idx=1:pGraph.NumNodes
    pose = pGraph.nodes(idx);
    rigidPose = rigid3d((trvec2tform(pose(1:3)) * quat2tform(pose(4:7)))');
    estimatedTforms(idx,1) = rigidPose;
end

% Create global map from processed point clouds and their relative poses
globalMap = pcalign(pcAccepted,estimatedTforms,alignGridStep);

% Create a figure window to visualize the estimated map and trajectory
hFigTrajMap = figure;
axTrajMap = axes('Parent',hFigTrajMap,'Color','black');
pcshow(estimatedTraj,'red','MarkerSize',150,'Parent',axTrajMap)
hold on
pcshow(globalMap,'MarkerSize',10,'Parent',axTrajMap)
hold off

% Customize axis labels
xlabel(axTrajMap,'X (m)')
ylabel(axTrajMap,'Y (m)')
zlabel(axTrajMap,'Z (m)')
title(axTrajMap,'Estimated Robot Trajectory And Generated Map')

Визуализируйте основную истину карту и предполагаемую карту.

% Create a figure window to display both the ground truth map and estimated map
hFigMap = figure('Position',[0 0 700 400]);
axMap1 = subplot(1,2,1,'Color','black','Parent',hFigMap);
axMap1.Position = [0.08 0.2 0.4 0.55];
pcshow(orgMap,'Parent',axMap1)
xlabel(axMap1,'X (m)')
ylabel(axMap1,'Y (m)')
zlabel(axMap1,'Z (m)')
title(axMap1,'Ground Truth Map')

axMap2 = subplot(1,2,2,'Color','black','Parent',hFigMap);
axMap2.Position = [0.56 0.2 0.4 0.55];
pcshow(globalMap,'Parent',axMap2)
xlabel(axMap2,'X (m)')
ylabel(axMap2,'Y (m)')
zlabel(axMap2,'Z (m)')
title(axMap2,'Estimated Map')

См. также

Функции

readPointCloud | insertPointCloud (Navigation Toolbox) | rayIntersection (Navigation Toolbox) | addRelativePose (Navigation Toolbox) | optimizePoseGraph (Navigation Toolbox) | show (Navigation Toolbox) | extractFPFHFeatures | pcmatchfeatures | estimateGeometricTransform3D | pcdownsample | pctransform | pcregistericp | pcalign | tform2trvec (Navigation Toolbox) | tform2quat (Navigation Toolbox)

Объекты

lasFileReader | pointCloud | pcplayer | occupancyMap3D (Navigation Toolbox) | poseGraph3D (Navigation Toolbox) | rigid3d

Ссылки

[1] Старр, Скотт. Tuscaloosa, AL: Seasonal Inundation Dynamics and Fertebrate Communities (неопр.) (недоступная ссылка). Национальный центр воздушного лазерного отображения, 1 декабря 2011 года. OpenTopography (https://doi.org/10.5069/G9SF2T3K).

Документация по Lidar Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте