Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Reduzca la deriva en la trayectoria de odometría visual 3D mediante gráficos de pose

Este ejemplo muestra cómo reducir la desviación en la trayectoria estimada (ubicación y orientación) de una cámara monocular mediante la optimización del gráfico de pose 3D. La odometría visual estima la pose global actual de la cámara (cuadro actual). Debido a una mala coincidencia o errores en la triangulación de puntos tridimensionales, las trayectorias de los robots a menudo tienden a desviarse de ground-truth. La detección de cierre de bucle y la optimización del gráfico de pose reducen esta desviación y corrigen los errores.

Cargar poses estimadas para la optimización del gráfico de poses

Cargue las poses estimadas de la cámara y los bordes de cierre del bucle. Las poses estimadas de la cámara se calculan mediante odometría visual. Los bordes de cierre del bucle se calculan encontrando el fotograma anterior que vio la escena actual y estimando la pose relativa entre el fotograma actual y el candidato a cierre del bucle. Los fotogramas de la cámara se toman como muestra de [1].

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

Construya un gráfico de pose en 3D

Crea un gráfico de pose vacío.

pg3D = poseGraph3D;

Agregue nodos al gráfico de pose, con bordes que definan la pose relativa y la matriz de información para el gráfico de pose. Convierta las poses estimadas, dadas como transformaciones, en poses relativas como un vector [x y theta qw qx qy qz] . Se utiliza una matriz de identidad para la matriz de información de cada pose.

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

Añade un borde de cierre de bucle. Agregue este borde entre dos nodos existentes del marco actual a un marco anterior.

% 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);

Optimiza el gráfico de pose. Los nodos se ajustan según las restricciones de los bordes para mejorar el gráfico de pose general. Para ver el cambio en la deriva, trace las poses estimadas y las nuevas poses optimizadas con respecto a ground-truth.

% 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);

Referencias

[1] Gálvez-López, D. y J. D. Tardós. "Bolsas de palabras binarias para el reconocimiento rápido de lugares en secuencias de imágenes". Transacciones IEEE sobre Robótica. vol. 28, núm. 5, 2012, págs. 1188-1197.