В этом примере показано, как уменьшать дрейф в предполагаемой траектории (местоположение и ориентация) монокулярной камеры с помощью 3-D оптимизации графика положения. Визуальная одометрия оценивает текущее глобальное положение камеры (текущая система координат). Из-за плохого соответствия или ошибок в 3-D триангуляции точки, траектории робота часто имеют тенденцию дрейфовать от основной истины. Обнаружение закрытия цикла и оптимизация графика положения уменьшают этот дрейф и правильный для ошибок.
Загрузите предполагаемые положения камеры и ребра закрытия цикла. Предполагаемые положения камеры вычисляются с помощью визуальной одометрии. Ребра закрытия цикла вычисляются путем нахождения предыдущей системы координат, которая видела текущую сцену и оценку относительного положения между текущей системой координат и кандидатом закрытия цикла. Системы координат камеры производятся от [1].
% Estimated poses load('estimatedpose.mat'); % Loopclosure edge load('loopedge.mat'); % Groundtruth camera locations load('groundtruthlocations.mat');
Создайте пустой график положения.
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.
