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

Cree un mapa de ocupación a partir de imágenes de profundidad utilizando odometría visual y un gráfico de pose optimizado

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. En este ejemplo, creará un mapa de ocupación a partir de imágenes de profundidad, que se puede utilizar para planificar rutas mientras se navega en ese entorno.

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 calcularon mediante odometría visual. Los bordes de cierre del bucle se calcularon 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 un conjunto de datos que contiene imágenes de profundidad, poses de la cámara y ubicaciones ground-truth [1].

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

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. Optimice el gráfico de pose para ajustar los nodos según las restricciones de los bordes y este cierre de bucle. Almacena las poses optimizadas.

% 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);
            
optimizedPosegraph = optimizePoseGraph(pg3D);
optimizedposes = nodes(optimizedPosegraph);

figure; 
show(pg3D);

Cree un mapa de ocupación a partir de imágenes de profundidad y poses optimizadas

Cargue las imágenes de profundidad y los parámetros de la cámara del conjunto de datos [1].

load('depthimagearray.mat');    % variable depthImages
load('freburgK.mat');           % variable K

Cree un mapa de ocupación en 3D con una resolución de 50 celdas por metro. Lea las imágenes de profundidad de forma iterativa y convierta los puntos en la imagen de profundidad utilizando los parámetros de la cámara y las poses optimizadas de la cámara. Inserte los puntos como nubes de puntos en las poses optimizadas para construir el mapa. Muestra el mapa después de sumar todos los puntos. Debido a que hay muchas imágenes de profundidad, este paso puede tardar varios minutos. Considere descomentar el comando fprintf para imprimir el progreso del procesamiento de la imagen.

map3D = occupancyMap3D(50);

for k = 1:length(depthImages)
    points3D = exampleHelperExtract3DPointsFromDepthImage(depthImages{k},K);
    % fprintf('Processing Image %d\n', k);
    insertPointCloud(map3D,optimizedposes(k,:),points3D,1.5);
end

Muestra el mapa.

figure;
show(map3D);
xlim([-2 2])
ylim([-2 1])
zlim([0 4])
view([-201 47])

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.