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

Загрузите предполагаемые положения камеры и ребра закрытия цикла. Предполагаемые положения камеры были вычислены с помощью визуальной одометрии. Ребра закрытия цикла были вычислены путем нахождения предыдущей системы координат, которая видела текущую сцену и оценку относительного положения между текущей системой координат и кандидатом закрытия цикла. Системы координат камеры производятся от набора данных, который содержит изображения глубины, положения камеры и местоположения основной истины [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] Гальвес-Лопес, D. и Дж. Д. Тардос. "Мешки Двоичных слов для Быстрого Распознавания Места в Последовательностях изображений". Транзакции IEEE на Робототехнике. Издание 28, № 5, 2012, стр 1188-1197.