Stereo Visual SLAM for UAV Navigation in 3D Simulation

Visual SLAM is the process of calculating the position and orientation of a camera with respect to its surroundings while simultaneously mapping the environment. Developing a visual SLAM algorithm and evaluating its performance in varying conditions is a challenging task. One of the biggest challenges is generating the ground truth of the camera sensor, especially in outdoor environments. The use of simulation enables testing under a variety of scenarios and camera configurations while providing precise ground truth.

This example demonstrates the use of Unreal Engine® simulation to develop a visual SLAM algorithm for a UAV equipped with a stereo camera in a city block scenario.

Set Up Simulation Environment

First, set up a scenario in the simulation environment that can be used to test the visual SLAM algorithm. Use a scene depicting a typical city block with a UAV as the vehicle under test.

Next, select a trajectory for the UAV to follow in the scene. You can follow the Select Waypoints for Unreal Engine Simulation (Automated Driving Toolbox) example to interactively select a sequence of waypoints and then use the helperSelectSceneWaypoints function to generate a reference trajectory for the UAV. This example uses a recorded reference trajectory as shown below:

% Load reference path
data        = load("uavStereoSLAMData.mat");

pos         = data.pos;          % Position
orientEuler = data.orientEuler;  % Orientation

The UAVVisualSLAMIn3DSimulation Simulink® model is configured with the US City Block scene using the Simulation 3D Scene Configuration (UAV Toolbox) block. The model places a UAV on the scene using the Simulation 3D UAV Vehicle (UAV Toolbox) block. A stereo camera consisting of two Simulation 3D Camera (UAV Toolbox) blocks is attached to the UAV. In the dialog box of the Simulation 3D Camera (UAV Toolbox) block, use the Mounting tab to adjust the placement of the camera. Use the Parameters tab to configure properties of the camera to simulate different cameras. To estimate the intrinsics of the stereo camera that you want to simulate, use the Using the Stereo Camera Calibrator App app.

% Stereo camera parameters
focalLength    = [1109, 1109]; % In pixels
principalPoint = [640, 360];   % In pixels [x, y]
imageSize      = [720, 1280];  % In pixels [mrows, ncols]
baseline       = 0.5;          % In meters

% Open the model
modelName = 'UAVVisualSLAMIn3DSimulation';
open_system(modelName);

Warning: Unrecognized function or variable 'CloneDetectionUI.internal.CloneDetectionPerspective.register'.

Run Stereo Visual SLAM Simulation

The Helper Stereo Visual SLAM System block implements the stereo visual SLAM pipeline using the stereovslam class. Simulate the model and visualize the results.

% Set the random seed to get consistent results
rng(0);

% Run simulation
sim(modelName);

Close the model.

close_system(modelName);

References

[1] Mur-Artal, Raul, and Juan D. Tardós. "ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras." IEEE Transactions on Robotics 33, no. 5 (2017): 1255-1262.