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

SLAM visual monocular de alto rendimiento y fácil de implementar

Desde R2024a

Este ejemplo utiliza:

Abrir script en vivo

Localización y mapeo visual simultáneo (vSLAM) se refiere al proceso de calcular la posición y orientación de una cámara con respecto a su entorno mientras se mapea simultáneamente el entorno. Las aplicaciones de vSLAM incluyen realidad aumentada, robótica y conducción autónoma. En este ejemplo, el algoritmo utiliza únicamente entradas visuales de la cámara.

Los ejemplos de vSLAM MATLAB ® muestran cada uno una de estas implementaciones de vSLAM:

  • Modular y modificable─ Construye una canalización SLAM visual paso a paso mediante el uso de funciones y objetos. Para obtener más detalles y una lista de estas funciones y objetos, consulte el tema Implement Visual SLAM in MATLAB (Computer Vision Toolbox). El enfoque descrito en el tema contiene código modular y está diseñado para enseñar los detalles de una implementación de vSLAM, que se basa libremente en el popular y confiable algoritmo ORB-SLAM [1]. El código es fácilmente navegable, lo que le permite explorar el algoritmo y probar cómo sus parámetros pueden afectar el rendimiento del sistema. Esta implementación modular es la más adecuada para el aprendizaje, la experimentación y la modificación para probar sus propias ideas.

  • Rendimiento e implementación ─ Utiliza el objeto monovslam que contiene un flujo de trabajo vSLAM completo. El objeto ofrece una solución práctica con una velocidad de ejecución y generación de código enormemente mejoradas. Para generar código C/C++ multiproceso a partir de monovslam (Computer Vision Toolbox), puede utilizar MATLAB Coder ™. El código generado es portable y puedes implementarlo en hardware que no sea una PC así como también en un nodo ROS, como se muestra en el ejemplo Build and Deploy Visual SLAM Algorithm with ROS in MATLAB (Computer Vision Toolbox).

Este ejemplo muestra la implementación de alto rendimiento y desplegable para procesar datos de imágenes de una cámara monocular. Para obtener más detalles sobre la implementación modular y modificable, consulte el ejemplo Monocular Visual Simultaneous Localization and Mapping (Computer Vision Toolbox).

Descargar datos

Este ejemplo utiliza datos del benchmark TUM RGB-D [2]. El tamaño del conjunto de datos es 1,38 GB. Puede descargar el conjunto de datos a una carpeta temporal utilizando este código.

baseDownloadURL = "https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.tgz"; 
dataFolder = fullfile(tempdir,"tum_rgbd_dataset",filesep); 
options = weboptions(Timeout=Inf);
tgzFileName = dataFolder + "fr3_office.tgz";
folderExists = exist(dataFolder,"dir");

% Create a folder in a temporary directory in which to save the downloaded file
if ~folderExists  
    mkdir(dataFolder)
    disp("Downloading fr3_office.tgz (1.38 GB). This download can take a few minutes.") 
    websave(tgzFileName,baseDownloadURL,options); 
    
    % Extract contents of the downloaded file
    disp("Extracting fr3_office.tgz (1.38 GB) ...") 
    untar(tgzFileName,dataFolder); 
end

Crea un objeto imageDatastore para almacenar las imágenes RGB.

imageFolder   = dataFolder + "rgbd_dataset_freiburg3_long_office_household/rgb/";
imds          = imageDatastore(imageFolder);

Implementar SLAM visual

Crea un objeto cameraIntrinsics para almacenar los parámetros intrínsecos de la cámara. Los parámetros intrínsecos del conjunto de datos están disponibles en la página web del conjunto de datos [3]. Tenga en cuenta que las imágenes en el conjunto de datos ya no están distorsionadas, por lo tanto no es necesario especificar los coeficientes de distorsión. Si utiliza su propia cámara, puede estimar los parámetros intrínsecos utilizando la appCamera Calibrator (Computer Vision Toolbox).

focalLength = [535.4,539.2];  % in units of pixels
principalPoint = [320.1,247.6];  % in units of pixels
imageSize = [480,640];  % in units of pixels
intrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);

Crea un objeto monovslam con el objeto intrínseco de la cámara. Es posible que también necesites modificar estas propiedades según los datos de vídeo que desees utilizar.

  • MaxNumPoints ─ Esta propiedad controla la cantidad de características ORB extraídas de cada cuadro. Para una resolución de imagen de 480 por 640 píxeles, configure MaxNumPoints en 1000. Para resoluciones más altas, como 720 x 1280, configúrelo en 2000. Los valores mayores reducen la velocidad de ejecución.

  • SkipMaxFrames ─ Cuando el seguimiento de un fotograma a otro es confiable, esta propiedad establece la cantidad máxima de fotogramas que el objeto puede omitir antes de que el siguiente fotograma sea un fotograma clave. Con una velocidad de cuadros de 30 fps, configure SkipMaxFrames en 20. Para una velocidad de cuadros más lenta, elija un valor más pequeño. Aumentar SkipMaxFrames mejora la velocidad de procesamiento, pero puede provocar pérdida de seguimiento durante el movimiento rápido de la cámara.

  • TrackFeatureRange ─ Esta propiedad especifica el número mínimo y máximo de puntos de características rastreadas que debe contener un cuadro para que el objeto lo identifique como un cuadro clave. Las propiedades TrackFeatureRange y SkipMaxFrames le permiten controlar la frecuencia con la que los fotogramas del proceso de seguimiento agregan fotogramas clave.

numPoints = 1000;
numSkipFrames = 20;
trackedFeatureRange = [30 120];
vslam = monovslam(intrinsics,MaxNumPoints=numPoints,SkipMaxFrames=numSkipFrames,TrackFeatureRange=trackedFeatureRange);

Procesa cada imagen utilizando la función addFrame (Computer Vision Toolbox). Utilice el método plot (Computer Vision Toolbox) para visualizar la trayectoria estimada de la cámara y los puntos del mapa 3D.

ax = gca;
for i=1:numel(imds.Files)
    I = readimage(imds,i);
    addFrame(vslam, I);

    if hasNewKeyFrame(vslam)
        % Display 3-D map points and camera trajectory
        plot(vslam, Parent=ax);   
    end
end

Tenga en cuenta que el objeto monovslam ejecuta varias partes del algoritmo en subprocesos separados, lo que puede introducir una latencia en el procesamiento de un cuadro de imagen agregado mediante la función addFrame (Computer Vision Toolbox). 

% Plot intermediate results and wait until all images are processed
while ~isDone(vslam)
    if hasNewKeyFrame(vslam)
        plot(vslam, Parent=ax);
    end
end

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 12 objects of type line, text, patch, scatter. This object represents Camera trajectory.

Una vez procesadas todas las imágenes, puedes recopilar los puntos del mapa 3D finales y las poses de la cámara para un análisis posterior.

xyzPoints = mapPoints(vslam);
[camPoses,viewIds] = poses(vslam);

Comparar con la verdad fundamental

Puede comparar la trayectoria optimizada de la cámara con la ground-truth para evaluar la precisión de ORB-SLAM. La comparación también puede ayudar a ajustar los parámetros utilizados en el objeto monovslam (Computer Vision Toolbox). Los datos descargados contienen un archivo groundtruth.txt que almacena la ground-truth de la pose de la cámara de cada fotograma. Puede importar los datos utilizando la función helperImportGroundTruth. Una vez que importe la ground-truth, podrá visualizar la trayectoria real de la cámara y calcular el error cuadrático medio (RMSE) de las estimaciones de la trayectoria. 

% Load ground truth from the downloaded data
gTruthFileName = dataFolder+"rgbd_dataset_freiburg3_long_office_household/groundtruth.txt";
gTruth = helperImportGroundTruth(gTruthFileName,imds);

% Compute the scale between the estimated camera trajectory and 
% the actual camera trajectory
metrics = compareTrajectories(camPoses, gTruth(viewIds), AlignmentType="similarity");
disp(['Absolute RMSE for key frame location (m): ', num2str(metrics.AbsoluteRMSE(2))]);
Absolute RMSE for key frame location (m): 0.13054

% Plot the absolute translation error at each key frame
figure
ax = plot(metrics, "absolute-translation");
view(ax, [2.70 -49.20]);

Figure contains an axes object. The axes object with title Absolute Translation Error, xlabel X, ylabel Y contains 2 objects of type patch, line. These objects represent Estimated Trajectory, Ground Truth Trajectory.

Generación de código

Puede generar código C++ multiproceso eficiente a partir del objeto monovslam (Computer Vision Toolbox), que es adecuado para su implementación en una computadora host o una plataforma integrada que tenga todas las dependencias de terceros, incluidos OpenCV [4] y g2o [5]. Para fines ilustrativos, en esta sección se genera el código MEX. Para obtener más información sobre cómo implementar el código generado como un nodo ROS, consulte el ejemplo Build and Deploy Visual SLAM Algorithm with ROS in MATLAB (Computer Vision Toolbox).

Para cumplir con los requisitos de MATLAB Coder, debe reestructurar el código para aislar el algoritmo del código de visualización. El contenido algorítmico se encapsuló en la función helperMonoVisualSLAM, que toma una imagen como entrada y genera puntos del mundo 3D y poses de cámara como matrices. La función crea internamente un objeto monovslam (Computer Vision Toolbox) y lo guarda en una variable persistente llamada vslam. Tenga en cuenta que la función helperMonoVisualSLAM no muestra la nube de puntos 3D reconstruida ni las poses de la cámara. El método plot (Computer Vision Toolbox) de la clase monovslam (Computer Vision Toolbox) no fue diseñado para generar código porque la visualización normalmente no se implementa en sistemas integrados.

type("helperMonoVisualSLAM.m"); % display function contents
function [xyzPoint, camPoses] = helperMonoVisualSLAM(image)

%   Copyright 2023 The MathWorks Inc.

%#codegen

persistent vslam xyzPointsInternal camPosesInternal

if isempty(vslam)
    % Create a monovslam class to process the image data
    focalLength    = [535.4, 539.2];    % in units of pixels
    principalPoint = [320.1, 247.6];    % in units of pixels
    imageSize      = [480,     640];    % in units of pixels
    intrinsics     = cameraIntrinsics(focalLength, principalPoint, imageSize);

    vslam = monovslam(intrinsics);
end

% Process each image frame
addFrame(vslam, image);

% Get 3-D map points and camera poses
if isempty(xyzPointsInternal) || hasNewKeyFrame(vslam)
    xyzPointsInternal = mapPoints(vslam);
    camPosesInternal = poses(vslam);
end

xyzPoint = xyzPointsInternal;

% Convert camera poses to homogeneous transformation matrices
camPoses = cat(3, camPosesInternal.A);

Puede compilar la función helperMonoVisualSLAM en un archivo MEX utilizando el comando codegen. Tenga en cuenta que el archivo MEX generado tiene el mismo nombre que el archivo original MATLAB con _mex agregado, a menos que use la opción -o para especificar el nombre del ejecutable.

compileTimeInputs  = {coder.typeof(I)};

codegen helperMonoVisualSLAM -args compileTimeInputs
Code generation successful.

Procese los datos de la imagen cuadro por cuadro utilizando el archivo MEX.

% Process each image frame 
for i=1:numel(imds.Files)
    [xyzPoints, camPoses] = helperMonoVisualSLAM_mex(readimage(imds,i));
end

Implementaciones para otros sensores

Para abordar el desafío de la escala desconocida en el SLAM visual monocular, se puede utilizar una cámara estéreo o un sensor RGB-D, los cuales pueden calcular las dimensiones reales de la escena. Puede encontrar más detalles sobre la implementación de SLAM visual estéreo y RGB-D en las páginas de objetos stereovslam (Computer Vision Toolbox) y rgbdvslam (Computer Vision Toolbox), respectivamente. Alternativamente, puede integrar la cámara con un sensor IMU para recuperar la escala real utilizando el algoritmo SLAM visual-inercial. Estos ejemplos demuestran algunas de estas implementaciones:

Referencia

[1] Mur-Artal, Raúl, J. M. M. Montiel y Juan D. Tardós. “ORBE-SLAM: Un sistema SLAM monocular versátil y preciso”. IEEE Transactions on Robotics 31, n.º 5 (octubre de 2015): 1147–63. https://doi.org/10.1109/TRO.2015.2463671.

[2] Sturm, Jürgen, Nikolas Engelhard, Felix Endres, Wolfram Burgard y Daniel Cremers. "Un punto de referencia para la evaluación de sistemas SLAM RGB-D". En Actas de la Conferencia internacional IEEE/RSJ sobre robots y sistemas inteligentes, págs. 573-580, 2012.

[3] https://cvg.cit.tum.de/data/datasets/rgbd-dataset/file_formats

[4] “OpenCV: Módulos OpenCV”. https://docs.opencv.org/4.7.0/.

[5] Kümmerle, Rainer, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige y Wolfram Burgard. "G2o: “Un marco general para la optimización de gráficos”. En la Conferencia Internacional IEEE sobre Robótica y Automatización de 2011, 3607–13, 2011. https://doi.org/10.1109/ICRA.2011.5979949.

Consulte también

(Computer Vision Toolbox)

Temas