Создайте карту заполнения из 3-D данных о лидаре Используя SLAM

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

В этом примере вы обрабатываете облака точек инкрементно, чтобы оценить траекторию транспортного средства со смонтированным датчиком лидара. Эти оценки подвержены накапливающемуся дрейфу в зависимости от времени, который уменьшает точность созданной карты. Чтобы откорректировать дрейф, используйте обнаружение закрытия цикла и оптимизацию графика положения. Наконец, используйте оптимизированные положения, чтобы создать карту заполнения. Данные об облаке точек лидара в этом примере были собраны от сцены в среде симуляции, созданной с помощью Нереального Engine® от Epic Games®.

В этом примере вы учитесь как:

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

  2. Оцените траекторию транспортного средства с помощью регистрационного метода корреляции фазы.

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

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

Настройте сценарий в среде симуляции

Загрузите предварительно созданную Большую сцену Парковки. Выбрать пример Waypoints for Unreal Engine Simulation описывает, как в интерактивном режиме выбрать последовательность waypoints от сцены и как сгенерировать ссылочную траекторию транспортного средства. Используйте этот подход, чтобы выбрать последовательность waypoints и сгенерировать ссылочную траекторию для автомобиля, оборудованного датчиком. Добавьте дополнительные транспортные средства в сцену путем определения их положений парковки. Визуализируйте ссылочный путь и припаркованные транспортные средства на 2D виде с высоты птичьего полета на сцену.

% Load reference path
data = load('parkingLotReferenceData.mat');

% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;

% Set poses of the parked vehicles
parkedPoses = data.parkedPoses([18 21],:);

% Display the reference path and the parked vehicle locations
sceneName = 'LargeParkingLot';
hScene = figure('Name', 'Large Parking Lot', 'NumberTitle', 'off');
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),'LineWidth',2,'DisplayName','Reference Path');
scatter(parkedPoses(:,1),parkedPoses(:,2),[],'filled','DisplayName','Parked Vehicles');
xlim([-60 40])
ylim([10 60])
hScene.Position = [100,100,1000,500]; % Resize figure
legend
hold off

Откройте модель и добавьте припаркованные транспортные средства с помощью helperAddParkedVehicle функция.

modelName = 'GenerateLidarDataOfParkingLot';
open_system(modelName)
snapnow

helperAddParkedVehicles(modelName,parkedPoses);

Запишите и визуализируйте данные

Настройте простую модель с транспортным средством хэтчбека, проходящим заданный ссылочный путь при помощи Симуляции 3D Транспортное средство с блоком Ground Following. Смонтируйте лидар на центре крыши транспортного средства с помощью блока Simulation 3D Lidar. Запишите и визуализируйте данные о датчике. Используйте записанные данные, чтобы разработать алгоритм локализации.

close(hScene)

if ~ispc
    error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end

% Run simulation
simOut = sim(modelName);

close_system(modelName,0);

% Extract lidar data
ptCloudArr = helperGetPointClouds(simOut);

% Extract ground truth for the lidar data as an array of rigid3d objects
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);

Одометрия транспортного средства Используя алгоритм корреляции фазы

Чтобы создать карту с помощью собранных облаков точек, оцените относительные положения между последовательными облаками точек. От этих положений можно оценить траекторию автомобиля, оборудованного датчиком. Этот подход инкрементной оценки траектории называется одометрией.

Чтобы создать 2D карту сетки заполнения, 2D оценки положения достаточны. Следовательно, преобразуйте облака точек в 2D изображения сетки заполнения путем проектирования точек на наземную плоскость. Используйте регистрационный алгоритм корреляции фазы, чтобы вычислить 2D относительное преобразование между двумя изображениями. Путем последовательного создания этих преобразований вы преобразовываете каждое облако точек назад к системе координат первого облака точек. Этот метод также используется pcregistercorr [1].

Таким образом, это шаги, используемые, чтобы вычислить одометрию транспортного средства:

  1. Предварительно обработайте облако точек.

  2. Создайте изображение сетки заполнения облака точек путем определения заполнения на основе высоты (ось Z) значения точек.

  3. Укажите два изображения сетки заполнения, созданные из облаков точек, которые соответствуют той же сцене. Используйте imregcorr функционируйте, чтобы указать изображения сетки и оценить положение.

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

Предварительно обработайте облако точек

Шаг предварительной обработки включает следующие операции:

  1. Удалите выбросы.

  2. Сегмент и удаляет автомобиль, оборудованный датчиком.

  3. Отсеките облако точек, чтобы сконцентрироваться на сфере интересов.

ptCloud = ptCloudArr(1);

% Remove outliers in the point cloud
ptCloudDenoised = pcdenoise(ptCloud);

% Clip the point cloud. This is done to improve the processing performance
% and also to include only the areas of interest

% Set the limits to select the point cloud
selectLimitX = [-40 40];
selectLimitY = [-40 40];
selectLimitZ = [0 5];

roi = [selectLimitX selectLimitY selectLimitZ];
indices = findPointsInROI(ptCloudDenoised,roi);
ptCloudClipped = select(ptCloudDenoised,indices);

% Segment and remove ego vehicle

% Set location of the sensor and vehicle radius
sensorLocation = [0 0 0];
vehicleRadius = 3.5;

% Find the indices of the points other than the ego vehicle and create a
% point cloud with these points
egoIndices = findNeighborsInRadius(ptCloudClipped,sensorLocation,vehicleRadius);

egoFixed = false(ptCloudClipped.Count,1);
egoFixed(egoIndices) = true;
ptCloudNonEgo = select(ptCloudClipped,~egoFixed);

% Since you use the Z-values to determine the occupancy in a grid image,
% move the point cloud by sensor height so that more points are included to
% calculate occupancy

% Set the sensor height. This information can be obtained from the lidar
% point cloud
sensorHeight = groundTruthPosesLidar(1).Translation(3);

locationPts = ptCloudNonEgo.Location;
locationPts(:,3) = locationPts(:,3) + sensorHeight;
ptCloudProcessed = pointCloud(locationPts);

% Visualize and compare the point cloud before and after preprocessing.
figure('Name', 'Processed Point Clouds', 'NumberTitle','off')
pcViewAxes = pcshowpair(ptCloud, ptCloudProcessed);
title('Point cloud before and after preprocessing')
xlabel(pcViewAxes, 'X (m)')
ylabel(pcViewAxes, 'Y (m)')
zlabel(pcViewAxes, 'Z (m)')

Создайте изображение сетки заполнения

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

  1. Задайте сетку на плоскости X-Y с соответствующим разрешением

  2. Вычислите значение вероятности для каждой точки в облаке точек на основе его Z-значения. Эта вероятность определяется более низким и верхним пределом со всеми Z-значениями между пределами, масштабируемыми в области значений [0,1]. Любые точки с Z-значениями ниже нижнего предела или выше верхнего предела, сопоставлены со значениями вероятности 0 и 1 соответственно.

  3. Накопите вероятности в каждой ячейке сетки, соответствующей точкам, и вычислите среднее значение. Используйте pcbin дискретизировать сетку и получить индексы ячеек сетки для обработки.

  4. Визуализируйте изображение сетки заполнения, которое представляет вид сверху сцены.

% Set the occupancy grid size to 100 m with a resolution of 0.2 m
gridSize = 100;
gridStep = 0.2;

% The occupancy grid is created by scaling the points from 0-5 m in
% height to the probability values of [0 1]
zLimits = [0 5];

% Calculate the number of bins
spatialLimits = [-gridSize/2 gridSize/2; -gridSize/2 gridSize/2; ptCloudProcessed.ZLimits];

gridXBinSize = round(abs(spatialLimits(1,2) - spatialLimits(1,1)) / gridStep);
gridYBinSize = round(abs(spatialLimits(2,2) - spatialLimits(2,1)) / gridStep);

numBins = [gridXBinSize gridYBinSize 1];

% Find bin indices
binIndices = pcbin(ptCloudProcessed,numBins,spatialLimits,'BinOutput',false);

% Preallocate occupancy grid
occupancyGrid = zeros(gridXBinSize,gridYBinSize,'like',ptCloudProcessed.Location);
gridCount = zeros(gridXBinSize,gridYBinSize);

% Scale the Z values of the points to the probability range [0 1]
zValues = rescale(ptCloudProcessed.Location(:,3),'InputMin',zLimits(1),'InputMax',zLimits(2));

for idx = 1:numel(binIndices)
    binIdx = binIndices(idx);
    if ~isnan(binIdx)
        occupancyGrid(binIdx) = occupancyGrid(binIdx) + zValues(idx);
        gridCount(binIdx) = gridCount(binIdx) + 1;
    end
end

gridCount(gridCount == 0) = 1;

occupancyGrid = occupancyGrid ./ gridCount;

% Visualize the created occupancy grid
figure
subplot(1,2,1)
pcshow(ptCloudProcessed)
view(2)
title('Point Cloud Birds Eye View')
subplot(1,2,2)
imshow(imrotate(occupancyGrid,90))
tHandle = title('Occupancy Grid Image');
tHandle.Color = [1 1 1];

Проектирование точек на наземную плоскость работает хорошо, если наземная плоскость является плоской. Данные, собранные в этом примере, имеют относительно плоскую наземную плоскость. Если наземная плоскость не является плоской, преобразуйте наземную плоскость так, чтобы это было параллельно плоскости X-Y. Для получения дополнительной информации смотрите, что Лидар Регистра Перемещает Облако точек в пример Облака Фиксированной точки и раздел Tips pcregistercorr.

Укажите и оцените положения

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

% Pose graph for registration poses
regEstPoseGraph = poseGraph;
relTrans = groundTruthPosesLidar(1).T(4,1:2);
relYaw = -atan2(groundTruthPosesLidar(1).T(2,1),groundTruthPosesLidar(1).T(1,1));

Используйте imregcorr функционируйте, чтобы оценить относительное преобразование. Добавьте положения, полученные в график положения с помощью addRelativePose (Navigation Toolbox) метод. Визуализируйте положения, которые хранятся в графике положения, в то время как алгоритм прогрессирует.

% Get ground truth pose graph
gTPoseGraph = helperGetGTPoseGraph(groundTruthPosesLidar);

% Obtain the nodes from the pose graphs
gTNodes = gTPoseGraph.nodes();

% Plot the ground truth trajectory
figure('Name', 'Vehicle Trajectory', 'NumberTitle',"off")
compareTrajAxes = axes;
compareTrajAxes.XLim = [-10 60];
compareTrajAxes.YLim = [-20 5];
compareTrajAxes.XGrid = 'on';
compareTrajAxes.YGrid = 'on';
title('Compare Trajectories')

hold on
plot(gTNodes(:,1),gTNodes(:,2),'Color','blue','LineWidth',3,'DisplayName','Ground Truth Trajectory')

% Visualize the estimated trajectory and the location of the vehicle when
% running the algorithm
estimatedTraj = plot(gTNodes(1,1),gTNodes(1,2),'Color','green','LineWidth',3 ,'DisplayName','Estimated Trajectory');
currentLoc = scatter(gTNodes(1,1),gTNodes(1,2),50,'red','filled','DisplayName','Vehicle Location');

legend;

% Process every fourth frame. This is done to speed up the processing
% without affecting much accuracy.
skipFrames = 3;

numFrames = numel(groundTruthPosesLidar);

occGridFixed = occupancyGrid';

for frameIdx = 1+skipFrames:skipFrames:numFrames
    
    ptCloud = ptCloudArr(frameIdx);
    
    ptCloudProcessed = helperPreprocessPtCloud(ptCloud);
    
    occGridMoving = helperCreateOccupancyGrid(ptCloudProcessed,gridSize,gridStep,zLimits);
    
    % Registration
    
    % Because imregcorr reports the transformation from the origin, you must
    % provide a reference such that the sensor is at the center of each image
    Rgrid = imref2d(size(occGridMoving));
    offsetX = mean(Rgrid.XWorldLimits);
    Rgrid.XWorldLimits = Rgrid.XWorldLimits - offsetX;
    offsetY = mean(Rgrid.YWorldLimits);
    Rgrid.YWorldLimits = Rgrid.YWorldLimits - offsetY;    
    
    % Find relative pose in image coordinates
    [relPoseImage, peak] = imregcorr(occGridMoving,Rgrid,occGridFixed,Rgrid);
    
    % Convert translation to grid coordinates
    transInGrid = relPoseImage.T(3,1:2) .* gridStep;
    
    % The tranformation is a rigid transformation. Since relative pose is
    % an affine2d object, convert to rigid2d object
    rotations = relPoseImage.T(1:2,1:2);
    [u,~,v] = svd(rotations);
    relPose = rigid2d(u*v',transInGrid);
    
    % Add pose to registration estimate pose graph
    relTrans = relPose.Translation(1:2);
    relYaw = -atan2(relPose.T(2,1),relPose.T(1,1));     
    regEstPoseGraph.addRelativePose([relTrans relYaw]);  
    
    occGridFixed = occGridMoving;
    
    % Update the estimated trajectory and vehicle location
    estimatedNode = regEstPoseGraph.nodes(regEstPoseGraph.NumNodes);
    estimatedTraj.XData(end + 1) = estimatedNode(1);
    estimatedTraj.YData(end + 1) = estimatedNode(2);
    set(currentLoc,'XData',estimatedNode(1),'YData',estimatedNode(2));
    drawnow
end
hold off

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

Обнаружьте циклы и откорректируйте дрейф

График метод SLAM может использоваться, чтобы откорректировать накопленный дрейф и получить точную карту. Используйте lidarSLAM Объект (Navigation Toolbox) и его методы, которые используют оптимизацию графика положения, чтобы обнаружить циклы и правильный дрейф в траектории. Когда новый скан добавляется к lidarSLAM Объект (Navigation Toolbox), следующие шаги происходят:

  1. Оценка положения: объект использует imregcorr для регистрации.

  2. График положения сборки: объектно-ориентированная память позирует в poseGraph Объект (Navigation Toolbox).

  3. Обнаружение закрытия цикла: Циклы являются местами, которые ранее посетили. lidarSLAM Объект (Navigation Toolbox) использует регистрационные функции скана, matchScansGrid (Navigation Toolbox) и matchScans (Navigation Toolbox), чтобы обнаружить циклы. Этот процесс упоминается как обнаружение закрытия цикла. Объект выполняет обнаружение закрытия цикла путем соответствия с текущим сканом против предыдущих сканов в маленьком радиусе вокруг текущего местоположения робота. Обнаруженные циклы добавляются к графику положения.

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

  5. Создайте карту.

Установите lidarSLAM свойства emprically на основе данных.

% Set lidarSLAM properties
mapResolution = 2;
maxLidarRange = 50;
loopClosureThreshold = 2450;
loopClosureRadius = 3;

% Initialize lidarSLAM object
slamAlg = lidarSLAM(mapResolution,maxLidarRange);
slamAlg.ScanRegistrationMethod = 'PhaseCorrelation';
slamAlg.LoopClosureThreshold = loopClosureThreshold;
slamAlg.LoopClosureSearchRadius = loopClosureRadius;

Используйте addScan Функция объекта (Navigation Toolbox) lidarSLAM Объект (Navigation Toolbox) инкрементно обеспечить данные для регистрации и обнаружения закрытия цикла. Поскольку addScan (Navigation Toolbox) функция только принимает lidarScan Объект (Navigation Toolbox), необходимо преобразовать каждое изображение сетки в lidarScan Объект (Navigation Toolbox).

occGridFixed = occupancyGrid';

% Create a lidarScan object from an occupancy grid image
[rows,cols,values] = find(occGridFixed);
xLocs = -gridSize/2 + rows * gridStep;
yLocs = -gridSize/2 + cols * gridStep;

scan = lidarScan([yLocs xLocs]);

% Visualize the grid image and lidar scan
figure('Name','Occupancy image and lidar scan','NumberTitle','off')
subplot(1,2,1)
imshow(imrotate(occupancyGrid,-180))
title('Occupancy Grid Image')
subplot(1,2,2)
plot(scan)

Инкрементно добавьте сканы лидара в lidarSLAM Объект (Navigation Toolbox).

% Plot the ground truth trajectory
figure('Name','Trajectory traversal','NumberTitle','off') 
slamAlgAxes = axes;
plot(gTNodes(:,1),gTNodes(:,2),'Color','blue','LineWidth',3,'DisplayName','Ground Truth Trajectory')
hold on 
slamAlgAxes.XLim = [-10 60];
slamAlgAxes.YLim = [-20 5];
slamAlgAxes.XGrid = 'on';
slamAlgAxes.YGrid = 'on';
title('Trajectory Traversal and Loop Closure Detections')
legend

% Plot the current location of the vehicle as the algorithm progresses.
currentLoc = scatter(gTNodes(1,1),gTNodes(1,2),50,'red','filled','DisplayName','Vehicle Location');

% Also plot any location where a loop closure was detected.
loopLocation = scatter([],[],50,'black','filled','DisplayName','Loop Detected');

for frameIdx = 1:skipFrames:numFrames
    
    ptCloud = ptCloudArr(frameIdx);
    ptCloudProcessed = helperPreprocessPtCloud(ptCloud);
    occGridMoving = helperCreateOccupancyGrid(ptCloudProcessed,gridSize,gridStep,zLimits);
    
    [rows,cols,values] = find(occGridMoving);
    xLocs = -gridSize/2 + rows * gridStep;
    yLocs = -gridSize/2 + cols * gridStep;
    
    scan = lidarScan([yLocs xLocs]);  
    
    % Add lidar scan to algorithm
    [isScanAccepted,loopClosureInfo] = addScan(slamAlg,scan); 
    
    % Update the loop closure detected location
    if isScanAccepted && size(loopClosureInfo.EdgeIDs,1) > 0
        loopLocation.XData(end+1) = gTNodes(frameIdx,1); 
        loopLocation.YData(end+1) = gTNodes(frameIdx,2);
    end
    
    % Update the vehicle location
    set(currentLoc,'XData',gTNodes(frameIdx,1),'YData',gTNodes(frameIdx,2));
    drawnow
end
hold off

Визуализируйте траекторию, полученную при помощи оптимизации графика положения. Обратите внимание на то, что дрейф уменьшался. Для получения дополнительной информации о том, как вычислить ошибки для оценок положения, смотрите Локализацию Лидара с Нереальным примером Симуляции Engine.

% Obtain the poses from the pose graph
slamAlgPoses = slamAlg.PoseGraph.nodes();
estNodes = regEstPoseGraph.nodes();

% Plot the trajectories from the nodes
figure('Name','Vehicle Trajectory','NumberTitle',"off"); 
slamTrajAxes = axes;
slamTrajAxes.XLim = [-10 60];
slamTrajAxes.YLim = [-20 5];
slamTrajAxes.XGrid = 'on';
slamTrajAxes.YGrid = 'on';
title('Compare Trajectories')
legend

hold on; 

plot(gTNodes(:,1),gTNodes(:,2),'Color','blue','LineWidth',3,'DisplayName','Ground Truth Trajectory'); 
plot(estNodes(:,1),estNodes(:,2),'Color','green','LineWidth',3 ,'DisplayName','Estimated Trajectory');
plot(slamAlgPoses(:,1),slamAlgPoses(:,2),'Color','magenta','LineWidth',3 ,'DisplayName','Optimized Trajectory');

hold off;

Просмотрите и визуализируйте обнаруженные узлы закрытия цикла, добавленные к slamAlg объект.

slamAlg.PoseGraph.LoopClosureEdgeIDs
ans = 1×4

   197   199   201   203

figure('Name','Loop Closure Results','NumberTitle',"off")
pGraphAxes = slamAlg.PoseGraph.show('IDs','off');
pGraphAxes.XLim = [0 10];
pGraphAxes.YLim = [-5 3];
title('Loop Closure Edges')

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

Используйте buildMap (Navigation Toolbox) функция, чтобы создать карту сетки заполнения. Получите необходимые сканы и положения с помощью scansAndPoses Функция объекта (Navigation Toolbox) lidarSLAM Объект (Navigation Toolbox).

[scans,optimizedPoses]  = scansAndPoses(slamAlg);

map = buildMap(scans,optimizedPoses,1,50);
figure('Name','Occupancy Map','NumberTitle',"off") 
show(map);
hold on
show(slamAlg.PoseGraph,'IDs','off');
xlim([-20 100])
ylim([-50 50])
hold off;

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

sensorHeight = groundTruthPosesLidar(1).Translation(3);
ptCloudAccum = ptCloud.empty;

% Configure display
xlimits = [ -30  100];
ylimits = [-50  60];
zlimits = [-3  30];
player = pcplayer(xlimits,ylimits,zlimits);

% Define rigid transformation between the lidar sensor mounting position
% and the vehicle reference point. 
lidarToVehicleTform = helperPoseToRigidTransform(single([0 0 -1.57 0 0 0]));

% Specify vehicle dimensions
centerToFront = 1.104;
centerToRear  = 1.343;
frontOverhang = 0.828;
rearOverhang  = 0.589;
vehicleWidth  = 1.653;
vehicleHeight = 1.513;
vehicleLength = centerToFront + centerToRear + frontOverhang + rearOverhang;
hatchbackDims = vehicleDimensions(vehicleLength,vehicleWidth,vehicleHeight,...
'FrontOverhang',frontOverhang,'RearOverhang',rearOverhang);

vehicleDims   = [hatchbackDims.Length,hatchbackDims.Width,hatchbackDims.Height];
vehicleColor  = [0.85 0.325 0.098];

% Estimation trajectory handle
markerSize = 3;
hold(player.Axes,'on')
estTrajHandle   = scatter3(player.Axes,NaN,NaN,NaN,markerSize,vehicleColor,'filled');
hold(player.Axes,'off')

for frameIdx = 1:skipFrames:numFrames
    
    % Obtain the point clouds
    ptCloud = ptCloudArr(frameIdx);
    ptCloudProcessed = helperPreprocessPtCloud(ptCloud);
    
    % Obtain the pose
    poseIdx = floor((frameIdx-1)/skipFrames) + 1;
    pose = optimizedPoses(poseIdx,:);
    
    % Construct the rigid3d object required to concatenate point clouds.
    % This object holds the rigid transformation
    yaw = pose(3);
    rot = [cos(yaw) sin(yaw) 0;...
       -sin(yaw) cos(yaw) 0;...
       0 0 1];
    trans = [pose(1:2) sensorHeight];
    
    absPose = rigid3d(rot,trans);
    
    % Accumulated point cloud map
    ptCloudAccum = pccat([ptCloudAccum,pctransform(ptCloudProcessed,absPose)]);
    
    % Update accumulated point cloud map
    view(player,ptCloudAccum);
    
    % Set viewing angle to top view
    view(player.Axes,2);
    
    % Convert current absolute pose of sensor to vehicle frame 
    absVehiclePose = rigid3d( lidarToVehicleTform.T * absPose.T );
    
    % Draw vehicle at current absolute pose
    helperDrawVehicle(player.Axes,absVehiclePose,vehicleDims,'Color',vehicleColor);
    
    estTrajHandle.XData(end+1) = trans(1);
    estTrajHandle.YData(end+1) = trans(2);
    estTrajHandle.ZData(end+1) = trans(3);
    
end

hold(player.Axes,'off')

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

% Display the scene and the parked vehicle locations
figure;
sceneAxes = axes;
sceneName = 'LargeParkingLot';
[sceneImage, sceneRef] = helperGetSceneImage(sceneName);
helperShowSceneImage(imrotate(sceneImage, 180), sceneRef);
hold on
parkedPoses = [-17.255 -44.789 90; -0.7848, -44.6161, 90];
scatter(parkedPoses(:,1), parkedPoses(:,2), [], [0.8500 0.3250 0.0980], 'filled', 'DisplayName', 'Parked Vehicles');
ylim([-90 20])
sceneAxes.XTick = [];
sceneAxes.YTick = [];
sceneAxes.XLabel = [];
sceneAxes.YLabel = [];
legend
hold off;

Ссылки

[1] Димитриевский, Мартин, Дэвид Ван Хэмм, Питер Вилэерт и Уилфрид Филипс. “Устойчивое Соответствие Карт Заполнения для Одометрии в Автономных Транспортных средствах”. В Продолжениях 11-й Объединенной Конференции по Компьютерному зрению, Обработке изображений и Теории Компьютерной графики и Приложениям, 626–33. Рим, Италия: SCITEPRESS - Публикации Науки и техники, 2016.

Вспомогательные Функции

helperPoseToRigidTransform преобразует положение в твердое преобразование.

helperShowSceneImage отображает изображение сцены.

helperAddParkedVehicles добавляет припаркованные транспортные средства в сцену парковки.

helperPreprocessPtCloud предварительно обрабатывает облако точек.

helperCreateOccupancyGrid создает изображение сетки заполнения из облака точек.

helperDrawVehicle чертит транспортное средство в карте.

helperGetSceneImage получает изображение сцены и пространственную ссылку.

helperGetPointClouds извлекает массив объектов pointCloud, которые содержат данные о датчике лидара.

function ptCloudArr = helperGetPointClouds(simOut)

% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;

% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 3 : size(ptCloudData,4)
    ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n));  %#ok<AGROW>
end

end

helperGetLidarGroundTruth извлекает местоположение основной истины и ориентацию.

function gTruth = helperGetLidarGroundTruth(simOut)
numFrames = size(simOut.lidarLocation.time,1);

gTruth = repmat(rigid3d,numFrames-1,1);

for i = 2:numFrames
    gTruth(i-1).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
    % Ignore the roll and the pitch rotations since the ground is flat
    yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
    gTruth(i-1).Rotation = [cos(yaw),sin(yaw), 0; ...
        -sin(yaw),cos(yaw),0; ...
        0,         0,    1];
end
end

helperGetGTPoseGraph получает график положения из основной истины лидара.

function gTPoseGraph = helperGetGTPoseGraph(gTruthLidar)

gTPoseGraph = poseGraph;
gTPoseGraph.addRelativePose([0 0 0]);

for idx = 3:numel(gTruthLidar)
    relTform = gTruthLidar(idx).T * inv(gTruthLidar(idx-1).T);
    
    relTrans = relTform(4,1:2);
    relYaw = -atan2(relTform(2,1),relTform(1,1));     
    
    gTPoseGraph.addRelativePose([relTrans relYaw]);
    
end

end

Смотрите также

Функции

Объекты

Похожие темы

Внешние веб-сайты

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