S-PTAM is a Stereo SLAM system able to compute the camera trajectory in real-time. It heavily exploits the parallel nature of the SLAM problem, separating the time-constrained pose estimation from less pressing matters such as map building and refinement tasks. On the other hand, the stereo setting allows to reconstruct a metric 3D map for each frame of stereo images, improving the accuracy of the mapping process with respect to monocular SLAM and avoiding the well-known bootstrapping problem. Also, the real scale of the environment is an essential feature for robots which have to interact with their surrounding workspace.

(Click the image to redirect to S-PTAM video)

Related Publications:

[1] Taihú Pire,Thomas Fischer, Gastón Castro, Pablo De Cristóforis, Javier Civera and Julio Jacobo Berlles. S-PTAM: Stereo Parallel Tracking and Mapping Robotics and Autonomous Systems, 2017.

[2] Taihú Pire, Thomas Fischer, Javier Civera, Pablo De Cristóforis and Julio Jacobo Berlles.
Stereo Parallel Tracking and Mapping for Robot Localization
Proc. of The International Conference on Intelligent Robots and Systems (IROS), Hamburg, Germany, 2015.

License
Disclaimer
Dependencies
- ROS
- ros-utils
- g2o
- Loop Closure Dependencies
  - DLib
  - DBoW2
  - DLoopDetector
  - OpenGV
Installation
ROS Package
CMAKE flags
S-PTAM Parameters
Standalone
- Compilation
- Run

License

S-PTAM is released under GPLv3 license.

For a closed-source version of S-PTAM for commercial purposes, please contact the authors.

If you use S-PTAM in an academic work, please cite:

@article{pire2017sptam,
          title = {{S-PTAM: Stereo Parallel Tracking and Mapping}},
          author = {Pire, Taih{\'u} and Fischer, Thomas and Castro, Gast{\'o}n and De Crist{\'o}foris, Pablo and Civera, Javier and Jacobo Berlles, Julio},
          journal = {Robotics and Autonomous Systems (RAS)},
          volume = {93},
          pages = {27 -- 42},
          year = {2017},
          issn = {0921-8890},
          doi = {10.1016/j.robot.2017.03.019}
}

@inproceedings{pire2015sptam,
          title={{Stereo Parallel Tracking and Mapping for robot localization}},
          author={Pire, Taih{\'u} and Fischer, Thomas and Civera, Javier and De Crist{\'o}foris, Pablo and Jacobo berlles, Julio},
          booktitle={Proc. of the International Conference on Intelligent Robots and Systems (IROS)},
          pages = {1373--1378},
          year={2015},
          month = {September},
          doi = {10.1109/IROS.2015.7353546}
}

Disclaimer

This site and the code provided here are under active development. Even though we try to only release working high quality code, this version might still contain some issues. Please use it with caution.

Dependencies

ROS

We have tested S-PTAM in Ubuntu 16.04 with ROS Kinetic.

To install ROS (Kinetic) use the following command:

sudo apt-get install ros-kinetic-desktop

SuiteSparse

Suitespare is a dependency, so it needs to be installed

sudo apt-get install libsuitesparse-dev

ros-utils

Install our ros-utils library from the source code provided in

git clone git@github.com:lrse/ros-utils.git

g2o

Install g2o library from the source code provided in

git clone git@github.com:RainerKuemmerle/g2o.git

Tested until commit 4b9c2f5b68d14ad479457b18c5a2a0bce1541a90

git checkout 4b9c2f5b68d14ad479457b18c5a2a0bce1541a90

mkdir build && cd build
cmake ..
make 
sudo make install

Loop Closure dependencies

Only required when USE_LOOPCLOSURE flag is defined.

DBoW2 vocabularies are available through a git submodule at the bow_voc directory

git submodule update --init --recursive

DLib

Install DLib library from source code

git clone git@github.com:dorian3d/DLib.git

Tested until commit 70089a38056e8aebd5a2ebacbcb67d3751433f32

git checkout 70089a38056e8aebd5a2ebacbcb67d3751433f32

DBoW2

Install DBoW2 library from source code

git clone git@github.com:dorian3d/DBoW2.git

Tested until commit 82401cad2cfe7aa28ee6f6afb01ce3ffa0f59b44

git checkout 82401cad2cfe7aa28ee6f6afb01ce3ffa0f59b44

DLoopDetector

Install DLoopDetector library from source code

git clone git@github.com:dorian3d/DLoopDetector.git

Tested until commit 8e62f8ae84d583d9ab67796f779272b0850571ce

git checkout 8e62f8ae84d583d9ab67796f779272b0850571ce

OpenGV

Install OpenGV library from source code

git clone git@github.com:laurentkneip/opengv.git

Tested until commit 2e2d21917fd2fb75f2134e6d5be7a2536cbc7eb1

git checkout 2e2d21917fd2fb75f2134e6d5be7a2536cbc7eb1

Installation

git clone git@github.com:lrse/sptam.git

ROS Package

ROS Compilation

catkin_make --pkg sptam -DCMAKE_BUILD_TYPE=RelWithDebInfo -DSINGLE_THREAD=OFF -DSHOW_TRACKED_FRAMES=ON -DSHOW_PROFILING=ON -DPARALLELIZE=ON

To activate Loop Closing capabilities (requires DBoW2 and OpenGV dependencies).

catkin_make --pkg sptam -DCMAKE_BUILD_TYPE=RelWithDebInfo -DUSE_LOOPCLOSURE=ON -DSINGLE_THREAD=OFF -DSHOW_TRACKED_FRAMES=ON -DSHOW_PROFILING=ON -DPARALLELIZE=ON

For more information about compilation flags see CMAKE flags section.

Tutorials

We provide some examples of how to run S-PTAM with the most popular stereo datasets

KITTI dataset

Download the KITTI rosbag kitti_00.bag provided in KITTI rosbag files
Uncompress the dataset

rosbag decompress kitti_00.bag
Set use_sim_time ros variable true

rosparam set use_sim_time true
Play the dataset

rosbag play --clock kitti_00.bag

(When S-PTAM run with the flag SHOW_TRACKED_FRAMES=ON the performance is reduced notoriusly).
Run sptam using the kitti.launch

roslaunch sptam kitti.launch

EuRoc MAV dataset

Download the EuRoc rosbag Machine Hall 01 provided in EuRoc MAV Web Page
Add left and right camera_info messages in the rosbag

In S-PTAM package we provide a script euroc_add_camera_info.py to add left and right sensor_msgs/CameraInfo messages to the EuRoc MAV rosbags.

python sptam_directory/scripts/euroc_add_camera_info.py MH_01_easy.bag /mav0/cam0/sensor.yaml /mav0/cam1/sensor.yaml
Set use_sim_time ros variable true

rosparam set use_sim_time true
Play the dataset

rosbag play --clock MH_01_easy_with_camera_info.bag -s 50
Run sptam using the euroc.launch

roslaunch sptam euroc.launch

MIT Stata Center dataset

Download the MIT Stata Center rosbag 2012-01-27-07-37-01.bag provided in MIT Stata Center Web Page
Set use_sim_time ros variable true

rosparam set use_sim_time true
Play the dataset

rosbag play --clock 2012-01-27-07-37-01.bag -s 302.5 -u 87

(Here we are running the part 3 of the sequence where ground-truth was provided that is why the bag file start from a different timestamp)
Run sptam using the mit.launch

roslaunch sptam mit.launch

Indoor Level 7 S-Block dataset

Download the Level7 rosbag level07_20_05_12_trunc.bag (3747 Frame Subset) provided in Indoor Level 7 S-Block Dataset Web Page
Set use_sim_time ros variable true

rosparam set use_sim_time true
Play the dataset

rosbag play --clock level7_truncated.bag
Run sptam using the level7.launch

roslaunch sptam level7.launch

Node Information

Subscribed Topics

Camera topics should provide undistorted and stereo-rectified images. Consider using the image_proc node.

/stereo/left/image_rect (sensor_msgs/Image)
Undistorted and stereo-rectified image stream from the left camera.

/stereo/left/camera_info (sensor_msgs/CameraInfo)
Left camera metadata.

/stereo/right/image_rect (sensor_msgs/Image)
Undistorted and stereo-rectified image stream from the right camera.

/stereo/right/camera_info (sensor_msgs/CameraInfo)
Right camera metadata.

Published Topics

global_map (sensor_msgs/PointCloud2)
Global sparse mapped point cloud used for tracking.

local_map (sensor_msgs/PointCloud2)
Local map which contains points that are highly probable to be observed by the current camera.

tracked_map (sensor_msgs/PointCloud2)
Points tracked by the current camera.

keyframes (nav_msgs/Path)
All the keyframes generated along the whole trajectory.

local_keyframes (nav_msgs/Path)
Keyframes used to obtain the local map (local_map).

camera_pose (geometry_msgs::PoseWithCovarianceStamped)
Current camera pose computed after tracking.

ROS Parameters

~use_prediction (bool, default: false)
Replace decay velocity motion model by a pose prediction that will be read from the TF tree. If using this, please make sure you have also set the correct prediction_frame option.

~approximate_sync: (bool, default: false)
Whether to use approximate synchronization for stereo frames. Set to true if the left and right Cameras do not produce identical synchronized timestamps for a matching pair of frames.

~publish_transform: (bool, default: true)
Whether to publish the transformation associated to the newly computed camera pose to the TF tree.

~publish_on_fail: (bool, default: false)
Whether to publish pose and tf updates when the tracking fails.

~prediction_frame (string, default: "odom")
Reference frame for predictions in the TF tree. If use_prediction option is enabled, some source outside of the S-PTAM node is required to publish the transformation between prediction_frame and camera_frame.

~base_frame: (string, default: "base_link")
Reference frame for the robot. This is the frame whose pose will be published on the /robot/pose topic and the TF tree (if tf publication is enabled).

~camera_frame: (string, default: "camera")
Reference frame for the left camera, used to get left camera pose from tf.

~map_frame: (string, default: "map")
Name for the published map frame.

~reference_frame: (string, default: "base_link")
This is the frame that the map frame will be aligned to when initializing S-PTAM. In other word, the initial pose of the reference_frame defines the map origin.

Involved Coordinates Systems

Example of common used coordinate systems. Here, reference_frame follows the base_frame axis representation.

The camera correction computed by S-PTAM is applied between the map_frame and the base_frame (pink doted line).

The camera correction computed by S-PTAM is applied between the map_frame and the prediction_frame (pink doted line).

CMAKE flags

SINGLE_THREAD=([ON|OFF], default: OFF)
Make to S-PTAM runs tracking and mapping tasks in single thread mode, this is, tracking and mapping runs in only one thread. This options could be useful for debugging. Set it OFF to improve S-PTAM performance.

SHOW_TRACKED_FRAMES=([ON|OFF], default: OFF)
Show the tracked frames by S-PTAM. Set it OFF to improve S-PTAM performance.

SHOW_PROFILING=([ON|OFF], default: OFF)
Create a log file of S-PTAM. This log file can be used to depure S-PTAM. Set it OFF to improve S-PTAM performance.

PARALLELIZE=([ON|OFF], default: ON)
Make to S-PTAM uses parallelize code when is possible. Set it ON to improve S-PTAM performance.

USE_LOOPCLOSURE=([ON|OFF], default: OFF)
Activates loop closing capabilities of S-PTAM. Requires DBoW2 and OpenGV dependencies. Set it OFF to use S-PTAM without loop detection.

S-PTAM Parameters

~FeatureDetector/Name: (string, default: "GFTT")
Follows OpenCV convention.

~DescriptorExtractor/Name: (string, default: "BRIEF")
Follows OpenCV convention.

~DescriptorMatcher/Name: 'BruteForce-Hamming'
Follows OpenCV convention.

~DescriptorMatcher/crossCheck: false
Follows OpenCV convention.

~MatchingCellSize: (int, default: 15)
To match the map points with images features, each frame is divided in squares cells of fixed size. MatchingCellSize define the size of each cell.

~MatchingNeighborhood: (int, default: 2)
Cells' neighborhood around the point.

~MatchingDistance: (double, default: 25.0)
Descriptor distance. Use a non-fractional value when hamming distance is used. Use Fractional value when L1/L2 norm is used.

~EpipolarDistance: (double, default: 0.0)
Distance in rows from the epipolar line used to find stereo matches.

~FrustumNearPlaneDist: (double, default: 0.1)
Frustum (Field of View) near plane.

~FrustumFarPlaneDist: (double, default: 1000.0)
Frustum (Field of View) far plane.

~BundleAdjustmentActiveKeyframes: (int, default: 10)
Number of keyframes to be adjusted by the local bundle adjustment (LBA) in the local mapping.

~minimumTrackedPointsRatio: (int, default: 0.9)
Ratio of tracked points that the current frame should have with respect to the closest keyframe to be selected as a keyframe used by the keyframe selection strategy.

Standalone

In standalone directory, we include an example of code that does not use ROS framework (but it still depends of some ROS libraries). It can be useful for debugging or for those who are not familiar with ROS.

Compilation

On standalone directory do:

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release -DSHOW_TRACKED_FRAMES=ON -DUSE_LOOPCLOSURE=ON -DSHOW_PROFILING=ON

Run

Download the KITTI gray scale stereo images from KITTI dataset

./sptam-stereo ../../sptam/configurationFiles/kitti.yaml ../../sptam/configurationFiles/kitti_cam_00_to_02_13_to_21.yaml <KITTI_dataset_path>/00/image_0/ <KITTI_dataset_path>/00/image_1/ dir --timestamps <KITTI_dataset_path>/00/times.txt

Note: in order to use the Loop Closure module in standalone version, you must indicate the vocabulary location path in kitti.yaml configuration file.

lrse/sptam