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

Загрузите расчетные положения камеры и края замыкания петли. Расчетные позы камеры рассчитывали с использованием визуальной одометрии. Края замыкания контура вычислялись путем нахождения предыдущего кадра, который видел текущую сцену, и оценки относительной позы между текущим кадром и кандидатом замыкания контура. Кадры камеры отбираются из набора данных, который содержит изображения глубины, позы камеры и местоположения истинности земли [1].

load('estimatedpose.mat');          % Estimated poses
load('loopedge.mat');               % Loopclosure edge
load('groundtruthlocations.mat');   % Ground truth camera locations 

График построения 3-D позы

Создайте пустой график позы.

pg3D = poseGraph3D;

Добавьте узлы к графу позы с рёбрами, определяющими относительную позу и информационную матрицу для графа позы. Преобразование расчетных поз, заданных как преобразования, в относительные позы [x y theta qw qx qy qz] вектор. Для информационной матрицы для каждой позы используется единичная матрица.

len = size(estimatedPose,2);
informationmatrix = [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1]; 
% Insert relative poses between all successive frames 
for k = 2:len
    % Relative pose between current and previous frame
    relativePose = estimatedPose{k-1}/estimatedPose{k};
    % Relative orientation represented in quaternions
    relativeQuat = tform2quat(relativePose);
    % Relative pose as [x y theta qw qx qy qz] 
    relativePose = [tform2trvec(relativePose),relativeQuat];
    % Add pose to pose graph
    addRelativePose(pg3D,relativePose,informationmatrix);
end

Добавьте кромку замыкания контура. Добавьте этот край между двумя существующими узлами от текущего кадра к предыдущему кадру. Оптимизируйте график позы для корректировки узлов на основе ограничений кромок и этого замыкания контура. Храните оптимизированные позы.

% Convert pose from transformation to pose vector.
relativeQuat = tform2quat(loopedge);
relativePose = [tform2trvec(loopedge),relativeQuat];
% Loop candidate
loopcandidateframeid = 1;
% Current frame
currentframeid = 100;

addRelativePose(pg3D,relativePose,informationmatrix,...
                loopcandidateframeid,currentframeid);
            
optimizedPosegraph = optimizePoseGraph(pg3D);
optimizedposes = nodes(optimizedPosegraph);

figure; 
show(pg3D);

Создание карты занятости по изображениям глубины и оптимизированным позициям

Загрузите изображения глубины и параметры камеры из набора данных [1].

load('depthimagearray.mat');    % variable depthImages
load('freburgK.mat');           % variable K

Создайте карту заполнения 3-D с разрешением 50 ячеек на метр. Считывайте изображения глубины итеративно и преобразуйте точки изображения глубины, используя параметры камеры и оптимизированные позы камеры. Вставьте точки как облака точек в оптимизированные позы для построения карты. Отображение карты после добавления всех точек. Поскольку имеется много изображений глубины, этот шаг может занять несколько минут. Подумайте о том, чтобы не комментировать fprintf для печати хода обработки изображения.

map3D = occupancyMap3D(50);

for k = 1:length(depthImages)
    points3D = exampleHelperExtract3DPointsFromDepthImage(depthImages{k},K);
    % fprintf('Processing Image %d\n', k);
    insertPointCloud(map3D,optimizedposes(k,:),points3D,1.5);
end

Показать карту.

figure;
show(map3D);
xlim([-2 2])
ylim([-2 1])
zlim([0 4])
view([-201 47])

Ссылки

[1] Гальвес-Лопес, Д. и Дж. Д. Тардос. «Пакеты двоичных слов для быстрого распознавания места в последовательностях изображений». Транзакции IEEE по робототехнике. т. 28, № 5, 2012, стр. 1188-1197.