Exponenta Banner
exponenta event banner

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

В этом примере используются:

Открыть сценарий в реальном времени

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

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

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

Создание синтетического лидарного сканирования на основе фрагментов воздушных данных, загруженных с веб-сайта OpenTopography [1]. Загрузите MAT-файл, содержащий последовательность ППМ по воздушным данным, определяющим траекторию робота. Вычислите lidar сканирования на основе данных в каждом ППМ с помощью 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

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

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

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

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

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;

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

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), чтобы хранить оценочные относительные позы между принятыми сканированиями lidar.

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 lidar с помощью алгоритма 3-D lidar 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(Панель инструментов навигации). В общем, поза первого узла в графе поз представляет начало координат. Чтобы сравнить расчетную траекторию с вехами истинности земли, укажите первый план истинности земли в качестве позы первого узла.

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 (Панель инструментов навигации) | rayIntersection (Навигационный комплект инструментов) | addRelativePose (навигационный комплект инструментов) | optimizePoseGraph (навигационный комплект инструментов) | show (навигационный комплект инструментов) | extractFPFHFeatures | pcmatchfeatures | estimateGeometricTransform3D | pcdownsample | pctransform | pcregistericp | pcalign | tform2trvec (навигационный комплект инструментов) |tform2quat(Панель инструментов навигации)

Объекты

lasFileReader | pointCloud | pcplayer | occupancyMap3D (Панель инструментов навигации) | poseGraph3D(Панель инструментов навигации) | rigid3d

Ссылки

[1] Старр, Скотт. «Tuscaloosa, AL: сезонная динамика инундации и сообщества беспозвоночных». Национальный центр воздушного лазерного картирования, 1 декабря 2011 года. OpenTopography (https://doi.org/10.5069/G9SF2T3K).

Документация по панели инструментов Lidar

Поддержка