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

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

Navigation Toolbox

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

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

Создайте и визуализируйте данные

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

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

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

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

% 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;

Задайте процент inliers, чтобы рассмотреть для подстройки относительных положений.

inlierRatio = 0.1;

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

alignGridStep = 1.2;

Оценка закрытия цикла for параметров

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

loopClosureSearchRadius = 7.9;

Алгоритм закрытия цикла основан на 3-D создании подкарты и соответствии. Подкарта состоит из конкретного количества принятых сканов nScansPerSubmap. Алгоритм закрытия цикла также игнорирует конкретное количество новых сканов subMapThresh, при поиске кандидатов цикла for. Задайте порог среднеквадратической ошибки (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:

Субдискретизация облака точек
Регистрация облака точек
Подсопоставьте создание
Запрос закрытия цикла
Оптимизация графика положения

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

rng(0) % 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

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

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

Извлечения дескрипторы FPFH от каждого скана с помощью extractFPFHFeatures функция
Идентифицирует соответствия точки путем сравнения дескрипторов с помощью pcmatchfeatures функция
Оценивает относительное положение от соответствий точки с помощью estimateGeometricTransform3D функция
Подстраивает относительное положение с помощью 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

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

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

Оцените соответствия между предыдущими подкартами и текущим сканом с помощью helperEstimateLoopCandidates функция. Подкарта является соответствием, если RMSE между текущим сканом и подкартой меньше заданного значения loopClosureThreshold. Предыдущие сканы, представленные всеми подкартами соответствия, являются кандидатами закрытия цикла.
Вычислите относительное положение между текущим сканом и кандидатом закрытия цикла, использующим регистрационный алгоритм 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) функция. В общем случае положение первого узла в графике положения представляет источник. Чтобы сравнить предполагаемую траекторию с основной истиной waypoints, задайте первую основную истину waypoint как положение первого узла.

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 0.2 0.55 0.55];
pcshow(orgMap,'Parent',axMap1)
axis off
title(axMap1,'Ground Truth Map')

axMap2 = subplot(1,2,2,'Color','black','Parent',hFigMap);
axMap2.Position = [0.45,0.2,0.55,0.55];
pcshow(globalMap,'Parent',axMap2)
axis off
title(axMap2,'Estimated Map')

Ссылки

[1] “Таскалуса, AL: Сезонная Динамика Наплыва И Бесхарактерные Сообщества”. OpenTopography. Национальный Центр Бортового Лазерного Отображения, 12 января 2011. https://portal.opentopography.org/datasetMetadata? otCollectionID=OT.112011.26916.1.

Объекты

Функции

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