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