Bathymetric Graph SLAM¶
Baseline SLAM framework for underwater vehicles. The algorithm gets a set of bathymetric submaps as input and corrects the global map constructed while refining the vehicle trajectory through a map-to-map registration followed by a pose graph optimization.
Dependencies (Ubuntu 16.04)¶
Building¶
Clone this repository and create a build folder under the root, then execute
cd build
cmake ..
make -j4
Available apps¶
There are two applications available under the bin folder.
If you compile the apps with the macro “INTERACTIVE” = 1 here, you’ll have to hit “q” for every step to be executed. This will allow you to visualize the before/after of the GICP registration per submap and the global graph optimization (recommended while finding your way around/debugging).
The process outputs .png images with the maps of bathymetry and consistency error.
The current script optimizes the graph with Ceres, but the app outputs a “graph.g2o” file which you can solve with G2O if preferred.
SLAM with simulated data¶
In order to test the framework with data from the SMARC simulator, use the toy dataset map_small under sim_data.
You can visualize both the ground truth map and vehicle trajectory in the visualizer. To start the optimization process, hit “q”.
./test_slam_simulation --folder /path/to/folder/
The simulation outputs a measure of the error contained in the map, as well as the height maps and error plots as .png files.
To increase the complexity of the sim dataset, increase the Gaussian noise to the vehicle’s position estimate.
In order to adapt the performance of the algorithm to the dataset, adjust the weights of the edges of the pose-graph accordingly and tune the GICP and the Ceres solver parameters.
The algorithm is by default tuned for the toy example map_small.
You can find a bigger and more challenging dataset from the simulator here.
SLAM with real data¶
To run the SLAM solution with real data from a bathymetric survey, currently the input is in the form of a cereal file containing all the necessary information from your data files. You can find a real survey carried out with an ROV here. Download it, adjust the framework values, and test it.
./test_slam_real --file /path/to/datasets/your_data.cereal
Generating your own data from the SMARC simulator¶
You can generate bigger and more complex bathymetric surveys from user-defined environments within the SMARC simulator. To install it, follow the instructions here. To create and save the surveys with the default AUV and environment, run
roslaunch smarc_bringup auv_scenarios.launch
roslaunch smarc_bringup auv_system.launch
rosrun rviz rviz
Through the pop-up window, navigate the AUV over the area to map. You can press UP, DOWN,LEFT, RIGHT to steer, w to start the thruster, and s to stop it.
When you have finished with the survey, move the .pdc files with the submaps generated from ~/.ros/ to a new folder and run the framework as explained above.