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

Construya e implemente el algoritmo SLAM visual con ROS en MATLAB

Este ejemplo utiliza:

Abrir script en vivo

En este ejemplo, se implementa un algoritmo de localización y mapeo simultáneo visual (SLAM) para estimar las poses de la cámara para el conjunto de datos TUM RGB-D Benchmark [1]. Luego, genera código C++ para el algoritmo SLAM visual y lo implementa como un nodo ROS en un dispositivo remoto usando MATLAB ®.

Este ejemplo requiere MATLAB Coder ™.

Requisitos previos para el dispositivo remoto

El dispositivo remoto en el que desea implementar el código debe tener las siguientes dependencias instaladas:

  • OpenCV 4.5.0 — Para obtener más información sobre cómo descargar el código fuente de OpenCV y compilarlo en su dispositivo remoto, consulte Instalación de OpenCV en Linux.

  • Biblioteca Ceres-solver 2.0.0: descargue el código fuente y compílelo como una biblioteca compartida en su dispositivo remoto.

  • Biblioteca Eigen3 — Instale la biblioteca eigen3 usando el comando $ sudo apt install libeigen3-dev.

Conectarse al dispositivo remoto

Para este ejemplo, descargue una máquina virtual (VM) siguiendo las instrucciones de Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox) y luego siga estos pasos.

  • Inicie la máquina virtual de Ubuntu®.

  • En el escritorio de Ubuntu, haga clic en el ícono ROS Noetic Core para iniciar el núcleo ROS en la máquina virtual.

  • Especifique la dirección IP y el número de puerto del maestro ROS a MATLAB para que pueda comunicarse con la máquina virtual. Para este ejemplo, el maestro ROS está en la dirección 192.168.192.135 en el puerto 11311.

  • Inicie la red ROS utilizando rosinit.

masterIP = '192.168.192.135';
rosinit(masterIP,11311)
Initializing global node /matlab_global_node_33565 with NodeURI http://192.168.192.1:60627/ and MasterURI http://192.168.192.135:11311.

Configurar datos en un dispositivo remoto

Este ejemplo utiliza el conjunto de datos TUM RGB-D Benchmark [1]. Descargue el conjunto de datos como un archivo ROS bag en el dispositivo remoto.

$ wget https://cvg.cit.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag

Si tiene problemas al reproducir el archivo bag, descomprímalo primero.

$ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag

Implementar el algoritmo SLAM visual

Este ejemplo utiliza el objeto monovslam (Computer Vision Toolbox) para implementar SLAM visual. Para cada nuevo cuadro agregado mediante su función de objeto addFrame, el objeto monovslam extrae y rastrea características para estimar las poses de la cámara, identificar cuadros clave y calcular los puntos del mapa 3D en el marco mundial. El objeto monovslam también busca cierres de bucle utilizando el algoritmo de bolsa de características y optimiza las poses de la cámara utilizando la optimización del gráfico de poses, una vez que se detecta un cierre de bucle. Para obtener más información sobre la canalización SLAM visual, consulte Monocular Visual Simultaneous Localization and Mapping (Computer Vision Toolbox).

La función helperROSVisualSLAM implementa el algoritmo SLAM visual utilizando estos pasos:

  1. Crea un objeto monovslam usando características intrínsecas de la cámara conocidas.

  2. Cree un suscriptor de ROS para escuchar los fotogramas del conjunto de datos TUM RGB-D publicados por el archivo de bolsa de ROS.

  3. Crea un publicador para publicar puntos de mapa y poses de cámara en MATLAB.

  4. Para cada cuadro publicado por el archivo bag, agréguelo al objeto monovslam usando su función de objeto addFrame. Luego, el objeto monovslam determina si es un fotograma clave.

  5. Para cada fotograma clave, obtenga los puntos del mapa en el marco mundial y las poses de la cámara del objeto monovlsam.

  6. Publica los puntos del mapa y las poses de la cámara en MATLAB para su visualización.

type("helperROSVisualSLAM.m")
function helperROSVisualSLAM()
% The helperROSVisualSLAM function implements the visual SLAM pipeline for
% deployment as a ROS node.
% Copyright 2023 The MathWorks, Inc.
    
    % Create a visual SLAM object
    intrinsics = cameraIntrinsics([535.4,539.2], [320.1,247.6], [480, 640]);
    vslam = monovslam(intrinsics);

    % Create a subscriber to read camera images
    cameraSub = rossubscriber('/camera/rgb/image_color', 'sensor_msgs/Image', @(varargin)vslamNodeCallback(varargin{:}),DataFormat="struct");

    % Create a publisher to publish map points and camera poses to MATLAB
    cameraPub = rospublisher('/visualizePoints','std_msgs/Float64MultiArray','DataFormat','struct');

    while 1
        if hasNewKeyFrame(vslam)
            msg = rosmessage('std_msgs/Float64MultiArray', 'DataFormat', 'struct');
            % Get map points and camera poses
            worldPoints = mapPoints(vslam);
            [camPoses] = poses(vslam);
            
            % Pack camera poses for publishing
            poseSize = numel(camPoses);
            transAndRots = zeros(poseSize*4,3);
            for i = 0:poseSize-1
                transAndRots(i*4+1,:) = camPoses(i+1).Translation;
                transAndRots(i*4+2:i*4+4,:) = camPoses(i+1).R;
            end
            
            % Concatenate poses and points into one struct
            allData = vertcat(transAndRots, worldPoints);
            allDataSize = size(allData,1);
            flattenPoints = reshape(allData,[allDataSize*3 1]);
            msg.Data = flattenPoints;
            msg.Layout.Dim = rosmessage('std_msgs/MultiArrayDimension', 'DataFormat', 'struct');
            msg.Layout.Dim(end).Size = uint32(poseSize);

            % Send data to MATLAB
            send(cameraPub, msg);
        end
    end

    function vslamNodeCallback(~, data)
        img = rosReadImage(data);
        addFrame(vslam, img);
    end
end

Generar e implementar un nodo SLAM visual

Utilice MATLAB Coder ™ para generar un nodo ROS para el algoritmo SLAM visual definido por la función helperROSVisualSLAM. Luego puede implementar este nodo en la máquina virtual remota. Cree un objeto de configuración MATLAB Coder que utilice hardware "Robot Operating System (ROS)". Antes de la implementación remota, configure estos parámetros de configuración para la máquina virtual Linux. Tenga en cuenta que, si realiza la implementación en una máquina remota diferente, debe cambiar estos parámetros a los adecuados para su dispositivo.

Nota: De forma predeterminada, la acción de compilación "Build and Load" implementa el nodo en el dispositivo, pero no lo ejecuta automáticamente. Si desea que el nodo se ejecute inmediatamente después de la generación del código, utilice la acción de compilación "Build and Run" en su lugar.

cfg = coder.config('exe');
cfg.Hardware = coder.hardware('Robot Operating System (ROS)');
cfg.Hardware.BuildAction = 'Build and load';
cfg.Hardware.CatkinWorkspace = '~/catkin_ws';
cfg.Hardware.RemoteDeviceAddress = '192.168.192.135';
cfg.Hardware.RemoteDeviceUsername = 'user';
cfg.Hardware.RemoteDevicePassword = 'password';
cfg.Hardware.DeployTo = 'Remote Device';
codegen helperROSVisualSLAM -args {} -config cfg -std:c++11

Configurar la visualización

Utilice el objeto helperVisualSLAMViewer para crear un visor que visualice los puntos del mapa junto con la trayectoria de la cámara y la pose actual de la cámara.

viewer = helperVisualSLAMViewer(zeros(0,3),rigidtform3d(eye(4)));

Cree un suscriptor ROS para visualizar los puntos del mapa y las poses de la cámara publicadas por el nodo SLAM visual implementado. Asigna la función helperVisualizePointsAndPoses como una callback que se activará cada vez que el suscriptor reciba un mensaje del nodo implementado.

visualizeSub = rossubscriber('/visualizePoints', 'std_msgs/Float64MultiArray', @(varargin)helperVisualizePointsAndPoses(varargin{:}, viewer),'DataFormat','struct');

Ejecutar el nodo SLAM visual implementado en un dispositivo remoto

En el escritorio de Ubuntu de la máquina virtual, haga clic en el ícono ROS Noetic Terminal. Fuente del espacio de trabajo de catkin.

$ source ~/catkin_ws/devel/setup.bash

Para ayudar al nodo implementado a acceder a las dependencias de la biblioteca, agregue la ruta /usr/local/lib a la variable de entorno, LD_LIBRARY_PATH.

$ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib

Navegue al directorio de origen del nodo helperrosvisualslam implementado en el dispositivo remoto. Debe ejecutar el nodo desde este directorio porque el archivos de bolsa de características utilizado por el nodo implementado está presente en este directorio.

$ cd ~/catkin_ws/src/helperrosvisualslam/src/

Ejecute el nodo SLAM visual implementado en el dispositivo remoto.

$ rosrun helperrosvisualslam helperROSVisualSLAM

Comience a reproducir el archivo de bolsa ROS en una Terminal ROS Noetic separada.

$ rosbag play rgbd_dataset_freiburg3_long_office_household.bag

Desconectar

Desconectarse de la red ROS después de que los nodos hayan terminado la ejecución.

rosshutdown

Referencias

[1] 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 las Actas de la Conferencia Internacional IEEE/RSJ sobre Robots y Sistemas Inteligentes, págs. 573-580, 2012

Funciones auxiliares

La función helperVisualizePointsAndPoses descomprime el mensaje std_msgs/Float64MultiArray recibido desde el nodo implementado en puntos de mapa y poses de cámara. Luego, llama a la función del objeto updatePlot del objeto helperVisualSLAMViewer para actualizar la visualización con los nuevos datos.

function helperVisualizePointsAndPoses(~, msg,viewer)
    % Unpack multi-array message based on the packing layout of translation
    % and rotation values
    offset = msg.Layout.Dim.Size * 4;
    nSize = numel(msg.Data) / 3;
    allData = reshape(msg.Data, [nSize 3]);
    % Extract camera poses and map points
    camPosesR = allData(1:offset, :);
    xyzPoints = allData(offset+1:end, :);
    % Convert camera poses to an array of rigidtform values
    camPoses = [];
    for i=0:msg.Layout.Dim.Size-1
        if i == 0
            camPoses = rigidtform3d(camPosesR(i*4+2:i*4+4,:),camPosesR(i*4+1,:));
        else
            camPoses(end+1) = rigidtform3d(camPosesR(i*4+2:i*4+4,:),camPosesR(i*4+1,:));
        end
    end
    
    % Update the viewer plot
    viewer.updatePlot(xyzPoints, camPoses);
end

Consulte también

(Computer Vision Toolbox)

Temas