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

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