Уменьшайте дрейф в 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 график положения
Создайте пустой график положения.
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); figure show(posegraph3D);
Оптимизируйте график положения. Узлы настроены на основе ограничений ребра, чтобы улучшить полный график положения. Чтобы видеть изменение в дрейфе, постройте предполагаемые положения и новые оптимизированные положения против наземной истины.
% Pose graph optimization optimizedPosegraph = optimizePoseGraph(posegraph3D); optimizedposes = nodes(optimizedPosegraph); % Camera trajectory plots figure estimatedposes = nodes(posegraph3D); 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.