search

srv / stereo_slam

Other
129 stars 64 forks source link

readme

Stereo SLAM

stereo_slam is a ROS node to execute Simultaneous Localization And Mapping (SLAM) using only one stereo camera. The algorithm was designed and tested for underwater robotics. This node is based on the G2O library for graph optimization and uses the power of libhaloc to find loop closures between graph nodes. It uses a keyframe to multi-keyframe loop closing mechanism, based on keypoint clustering, to improve the SLAM corrections on feature-poor environments.

You can see it in action here:

Stereo-SLAM video

Specially designed for underwater scenarios:

Video: ORB-SLAM vs Stereo SLAM (underwater)

More videos...

Video: Stereo SLAM and 3D reconstruction and Video: Stereo SLAM at UIB outdoor pond

Related paper

ICRA'16, nominated for the best student paper award

CITATION:

@INPROCEEDINGS{7487416,
  author={P. L. Negre and F. Bonin-Font and G. Oliver},
  booktitle={2016 IEEE International Conference on Robotics and Automation (ICRA)},
  title={Cluster-based loop closing detection for underwater slam in feature-poor regions},
  year={2016},
  pages={2589-2595},
  keywords={SLAM (robots);autonomous underwater vehicles;feature extraction;image matching;image registration;mobile robots;object detection;path planning;robot vision;AUV navigation;Balearic Islands;autonomous underwater vehicle;cluster-based loop closing detection;feature matching;feature-poor underwater environment;marine environments;sandbanks;seagrass;simultaneous localization and mapping;underwater SLAM;vision-based localization systems;visual features;visual information;visual keypoint clustering;visual registration;Cameras;Feature extraction;Image edge detection;Pipelines;Rocks;Simultaneous localization and mapping;Visualization},
  doi={10.1109/ICRA.2016.7487416},
  month={May}
}

Installation (Ubuntu + ROS)

1) Install the dependencies

sudo apt-get install ros-<your ros distro>-libg2o

sudo apt-get install libceres-dev

You also need to setup a stereo visual odometer (e.g. viso2 or fovis).

3) Download the code, save it to your ROS workspace enviroment and compile.

roscd
git clone https://github.com/srv/stereo_slam.git
cd stereo_slam
rosmake

Parameters

Subscribed topics

Published Topics

Run the node

You can run the node using the following launch file (please, for a better performance scale your images if more than 960px width).

<launch>
  <arg name = "camera"        default = "/stereo"/>
  <arg name = "robot_name"  default = "robot_0"/>

  <!-- Run the stereo image proc -->
  <node ns = "$(arg camera)" pkg = "stereo_image_proc" type = "stereo_image_proc" name = "stereo_image_proc" />

  <node pkg = "viso2_ros" type = "stereo_odometer" name = "stereo_odometer">
    <remap from = "stereo" to = "$(arg camera)"/>
    <remap from = "image" to = "image_rect"/>
  </node>

  <node pkg="stereo_slam" type="localization" name="stereo_slam" output="screen" if = "$(eval enable_decimate_x1 == true)">
    <remap from = "odom"                        to = "stereo_odometer/odometry"/>
    <remap from = "left_camera_info"            to = "/$(arg camera)/left/camera_info"/>
    <remap from = "right_camera_info"           to = "/$(arg camera)/right/camera_info"/>
    <remap from = "left_image_rect_color"       to = "/$(arg camera)/left/image_rect_color"/>
    <remap from = "right_image_rect_color"      to = "/$(arg camera)/right/image_rect_color"/> 
    <param name = "refine"                      value = "false"/>
    <param name = "distance_between_keyframes"  value = "0.5"/>
    <param name = "feature_detector_selection"  value = "ORB"/>
    <param name = "lc_min_inliers"              value = "30"/>
    <param name = "lc_epipolar_thresh"          value = "1.0"/>
    <param name = "map_frame_name"              value = "/$(arg robot_name)/map"/>
    <param name = "lc_neighbors"                value = "5"/>
    <param name = "lc_discard_window"           value = "20"/>
    <param name = "ransac_iterations"           value = "150"/>
  </node>
</launch>

Saved data

The node stores some data into the stereo_slam directory during the execution:

Online graph viewer

The node provides a python script to visualize the results of the stereo_slam during the execution: scripts/graph_viewer.py:

usage:  graph_viewer.py [-h]
    ground_truth_file visual_odometry_file
    graph_vertices_file graph_edges_file

or (automatically detect the graph files):

roscd stereo_slam
./scripts/graph_viewer.py

The odometry file can be recorded directly from ros using:

rostopic echo -p /your_odometry_topic/odometry > odometry.txt

The ground truth file must have the same format than the odometry.

Post processing

The node provides a python script to evaluate the results of the stereo_slam once the execution finishes: scripts/slam_evaluation.py:

usage: slam_evaluation.py [-h]
    ground_truth_file visual_odometry_file
    graph_vertices_file graph_edges_file

This script perform a set of operations in order to evaluate the performance of the stereo_slam algorithm:

1) Align all the curves (ground truth, visual_odometry and graph vertices) to the same origin.

2) Compute the average translation error.

3) Plot the error vs trajectory distance.