Документация

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

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

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

Создайте 3-D график положения

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

posegraph3D = robotics.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(posegraph3D,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(posegraph3D,relativePose,informationmatrix,...
                loopcandidateframeid,currentframeid);
            
optimizedPosegraph = optimizePoseGraph(posegraph3D);
optimizedposes = nodes(optimizedPosegraph);

figure; 
show(posegraph3D);

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

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

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

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

occupancymap = robotics.OccupancyMap3D(50);

for k = 1:length(depthImages)
    points3D = exampleHelperExtract3DPointsFromDepthImage(depthImages{k},K);
    % fprintf('Processing Image %d\n', k);
    insertPointCloud(occupancymap,optimizedposes(k,:),points3D,1.5);
end
figure;
show(occupancymap);
view(-2.4,-90);

Ссылки

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