В этом примере показано, как уменьшить дрейф в расчетной траектории (местоположение и ориентация) монокулярной камеры с помощью оптимизации графика 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] Гальвес-Лопес, Д. и Дж. Д. Тардос. «Пакеты двоичных слов для быстрого распознавания места в последовательностях изображений». Транзакции IEEE по робототехнике. т. 28, № 5, 2012, стр. 1188-1197.