Уменьшайте дрейф в 3-D визуальной траектории одометрии Используя графики положения

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

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

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

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

Создайте 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);

figure
show(pg3D);

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

% Pose graph optimization
optimizedPosegraph = optimizePoseGraph(pg3D);
optimizedposes = nodes(optimizedPosegraph);
% Camera trajectory plots
figure
estimatedposes = nodes(pg3D);
plot3(estimatedposes(:,1),estimatedposes(:,2),estimatedposes(:,3),'r');
hold on
plot3(groundtruthlocations(:,1),groundtruthlocations(:,2),groundtruthlocations(:,3),'g');
plot3(optimizedposes(:,1),optimizedposes(:,2),optimizedposes(:,3),'b');
hold off
legend('Estimated pose graph','Ground truth pose graph', 'Optimized pose graph');
view(-20.8,-56.4);

Ссылки

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

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