raulmur · plnegre · Jan 26, 2016 · Jan 26, 2016
diff --git a/Examples/ROS/ORB_SLAM2/manifest.xml b/Examples/ROS/ORB_SLAM2/manifest.xml
diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt → Examples/ROS/orb_slam2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt → Examples/ROS/orb_slam2/CMakeLists.txt
@@ -1,13 +1,13 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+cmake_minimum_required(VERSION 2.8.3)
 
-rosbuild_init()
-
-IF(NOT ROS_BUILD_TYPE)
-  SET(ROS_BUILD_TYPE Release)
-ENDIF()
-
-MESSAGE("Build type: " ${ROS_BUILD_TYPE})
+project(orb_slam2)
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  image_transport
+  message_filters
+  cv_bridge
+  cmake_modules)
 
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
@@ -34,15 +34,18 @@ find_package(OpenCV 2.4.3 REQUIRED)
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
 
+catkin_package()
+
 include_directories(
 ${PROJECT_SOURCE_DIR}
 ${PROJECT_SOURCE_DIR}/../../../
 ${PROJECT_SOURCE_DIR}/../../../include
 ${Pangolin_INCLUDE_DIRS}
 )
 
-set(LIBS 
-${OpenCV_LIBS} 
+set(LIBS
+${catkin_LIBRARIES}
+${OpenCV_LIBS}
 ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
@@ -51,20 +54,29 @@ ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
 )
 
 # Node for monocular camera
-rosbuild_add_executable(Mono
+add_executable(mono
 src/ros_mono.cc
 )
 
-target_link_libraries(Mono
+target_link_libraries(mono
 ${LIBS}
 )
 
 # Node for RGB-D camera
-rosbuild_add_executable(RGBD
+add_executable(rgbd
 src/ros_rgbd.cc
 )
 
-target_link_libraries(RGBD
+target_link_libraries(rgbd
+${LIBS}
+)
+
+# Node for stereo camera
+add_executable(stereo
+src/ros_stereo.cc
+)
+
+target_link_libraries(stereo
 ${LIBS}
 )
 
diff --git a/Examples/ROS/orb_slam2/launch/example.launch b/Examples/ROS/orb_slam2/launch/example.launch
@@ -0,0 +1,28 @@
+<launch>
+
+  <arg name="bagfile" default="<!-- your bagfile here -->" />
+
+  <!-- Setup the camera -->
+  <arg name="camera" default="<!-- your camera basename here -->"/>
+
+  <!-- Play the bagfile -->
+  <node pkg="rosbag" type="play" name="rosbag" args="--clock $(arg bagfile)" />
+
+  <!-- ORB_SLAM2 Mono -->
+  <node pkg="orb_slam2" type="mono" name="mono" output="screen" args="$(find orb_slam2)../../../Vocabulary/ORBvoc.txt <!-- Your config file here -->">
+    <remap from="/camera/image_raw" to="$(arg camera)/image_raw"/>
+  </node>
+
+  <!-- ORB_SLAM2 RGBD -->
+  <node pkg="orb_slam2" type="rgbd" name="rgbd" output="screen" args="$(find orb_slam2)../../../Vocabulary/ORBvoc.txt <!-- Your config file here -->">
+    <remap from="/camera/rgb/image_raw" to="$(arg camera)/rgbd/image_raw"/>
+    <remap from="camera/depth_registered/image_raw" to="$(arg camera)/depth_registered/image_raw"/>
+  </node>
+
+  <!-- ORB_SLAM2 Stereo -->
+  <node pkg="orb_slam2" type="stereo" name="stereo" output="screen" args="$(find orb_slam2)../../../Vocabulary/ORBvoc.txt <!-- Your config file here -->">
+    <remap from="/camera/left/image_raw" to="$(arg camera)/left/image_raw"/>
+    <remap from="/camera/right/image_raw" to="$(arg camera)/right/image_raw"/>
+  </node>
+
+</launch>
diff --git a/Examples/ROS/orb_slam2/package.xml b/Examples/ROS/orb_slam2/package.xml
@@ -0,0 +1,30 @@
+<package>
+
+  <name>orb_slam2</name>
+  <version>0.0.1</version>
+  <description>ORB_SLAM2</description>
+  <author>Raul Mur-Artal</author>
+  <maintainer email="raulmur@unizar.es">Raul Mur-Artal</maintainer>
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>cmake_modules</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>cv_bridge</run_depend>
+
+</package>
+
+
+
diff --git a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc → Examples/ROS/orb_slam2/src/ros_mono.cc b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc → Examples/ROS/orb_slam2/src/ros_mono.cc
diff --git a/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc → Examples/ROS/orb_slam2/src/ros_rgbd.cc b/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc → Examples/ROS/orb_slam2/src/ros_rgbd.cc
diff --git a/Examples/ROS/orb_slam2/src/ros_stereo.cc b/Examples/ROS/orb_slam2/src/ros_stereo.cc
@@ -0,0 +1,105 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include<iostream>
+#include<algorithm>
+#include<fstream>
+#include<chrono>
+
+#include<ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include<opencv2/core/core.hpp>
+
+#include"../../../include/System.h"
+
+using namespace std;
+
+class ImageGrabber
+{
+public:
+    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
+
+    void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);
+
+    ORB_SLAM2::System* mpSLAM;
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "Stereo");
+    ros::start();
+
+    if(argc != 3)
+    {
+        cerr << endl << "Usage: rosrun ORB_SLAM2 Stereo path_to_vocabulary path_to_settings" << endl;
+        ros::shutdown();
+        return 1;
+    }
+
+    // Create SLAM system. It initializes all system threads and gets ready to process frames.
+    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);
+
+    ImageGrabber igb(&SLAM);
+
+    ros::NodeHandle nh;
+
+    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);
+    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/right/image_raw", 1);
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
+    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);
+    sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));
+
+    ros::spin();
+
+    // Stop all threads
+    SLAM.Shutdown();
+
+    // Save camera trajectory
+    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
+
+    ros::shutdown();
+
+    return 0;
+}
+
+void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
+{
+    // Copy the ros image message to cv::Mat.
+    cv_bridge::CvImageConstPtr cv_ptrLeft,cv_ptrRight;
+    try
+    {
+        cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
+        cv_ptrRight = cv_bridge::toCvShare(msgRight);
+    }
+    catch (cv_bridge::Exception& e)
+    {
+        ROS_ERROR("cv_bridge exception: %s", e.what());
+        return;
+    }
+
+    mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
+}
+
+
diff --git a/README.md b/README.md
@@ -1,5 +1,5 @@
 # ORB-SLAM2
-#### Current version: 1.0.0 
+#### Current version: 1.0.0
 
 ORB-SLAM2 is a real-time SLAM library for **Monocular**, **Stereo** and **RGB-D** cameras, that computes the camera trajectory and a sparse 3D reconstruction (in the stereo and RGB-D case with true scale). It is able to detect loops and relocalize the camera in real time. We provide examples to run the system in the [KITTI dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) as stereo or monocular, and in the [TUM dataset](http://vision.in.tum.de/data/datasets/rgbd-dataset) as RGB-D or monocular. We also provide a ROS node to process live monocular or RGB-D streams. **The library can be compiled without ROS**. ORB-SLAM2 provides an interface to change between *SLAM Mode* and *Localization Mode* (only camera tracking, no map building).
 
@@ -8,15 +8,15 @@ The monocular capabilities of ORB-SLAM2 compared to [ORB-SLAM Monocular](https:/
 
 ###Related Publications:
 
-[1] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015. 
+[1] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015.
 
 Link to pdf: http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO15.pdf
 
 #1. License
 
 ORB-SLAM2 is released under a [GPLv3 license](https://github.com/raulmur/ORB_SLAM2/blob/master/License-gpl.txt). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/raulmur/ORB_SLAM2/blob/master/Dependencies.md).
 
-For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors. 
+For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors.
 
 If you use ORB-SLAM2 in an academic work, please cite:
 
@@ -49,7 +49,7 @@ Required by g2o (see below). Download and install instructions can be found at:
 We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) library to perform place recognition and [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. Both modified libraries (which are both BSD) are included in the Thirdparty folder.
 
 ##2.5 ROS (optional)
-We provide some examples to process the live input of a monocular or RGB-D camera using [ROS](ros.org). Building these examples is optional. In case you want to use ROS, a version Hydro or newer is needed.
+We provide some examples to process the live input of a monocular, stereo or RGB-D camera using [ROS](ros.org). Building these examples is optional. In case you want to use ROS, a version Hydro or newer is needed.
 
 #3. Building ORB-SLAM2 library and TUM/KITTI examples
 
@@ -78,11 +78,11 @@ This will create **libORB_SLAM.so**  at *lib* folder and the executables **mono_
 ./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER
 ```
 
-##4.2 KITTI Dataset  
+##4.2 KITTI Dataset
 
-1. Download the dataset (grayscale images) from http://www.cvlibs.net/datasets/kitti/eval_odometry.php 
+1. Download the dataset (grayscale images) from http://www.cvlibs.net/datasets/kitti/eval_odometry.php
 
-2. Execute the following command. Change `KITTIX.yaml`by KITTI00-02.yaml, KITTI03.yaml or KITTI04-12.yaml for sequence 0 to 2, 3, and 4 to 12 respectively. Change `PATH_TO_DATASET_FOLDER` to the uncompressed dataset folder. Change `SEQUENCE_NUMBER` to 00, 01, 02,.., 11. 
+2. Execute the following command. Change `KITTIX.yaml`by KITTI00-02.yaml, KITTI03.yaml or KITTI04-12.yaml for sequence 0 to 2, 3, and 4 to 12 respectively. Change `PATH_TO_DATASET_FOLDER` to the uncompressed dataset folder. Change `SEQUENCE_NUMBER` to 00, 01, 02,.., 11.
 ```
 ./Examples/Monocular/mono_kitti Vocabulary/ORBvoc.txt Examples/Monocular/KITTIX.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/SEQUENCE_NUMBER
 ```
@@ -91,9 +91,9 @@ This will create **libORB_SLAM.so**  at *lib* folder and the executables **mono_
 
 ##5.1 KITTI Dataset
 
-1. Download the dataset (grayscale images) from http://www.cvlibs.net/datasets/kitti/eval_odometry.php 
+1. Download the dataset (grayscale images) from http://www.cvlibs.net/datasets/kitti/eval_odometry.php
 
-2. Execute the following command. Change `KITTIX.yaml`to KITTI00-02.yaml, KITTI03.yaml or KITTI04-12.yaml for sequence 0 to 2, 3, and 4 to 12 respectively. Change `PATH_TO_DATASET_FOLDER` to the uncompressed dataset folder. Change `SEQUENCE_NUMBER` to 00, 01, 02,.., 11. 
+2. Execute the following command. Change `KITTIX.yaml`to KITTI00-02.yaml, KITTI03.yaml or KITTI04-12.yaml for sequence 0 to 2, 3, and 4 to 12 respectively. Change `PATH_TO_DATASET_FOLDER` to the uncompressed dataset folder. Change `SEQUENCE_NUMBER` to 00, 01, 02,.., 11.
 ```
 ./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt Examples/Stereo/KITTIX.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/SEQUENCE_NUMBER
 ```
@@ -123,7 +123,7 @@ This will create **libORB_SLAM.so**  at *lib* folder and the executables **mono_
   ```
   export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS
   ```
-  
+
 2. Go to *Examples/ROS/ORB_SLAM2* folder and execute:
 
   ```
@@ -138,12 +138,18 @@ This will create **libORB_SLAM.so**  at *lib* folder and the executables **mono_
   rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
 
-4. For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM2/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
+4. For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM2/Stereo. You will need to provide the vocabulary file and a settings file. See the stereo examples above.
+
+  ```
+  rosrun ORB_SLAM2 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+  ```
+
+5. For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM2/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
 
   ```
   rosrun ORB_SLAM2 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
-  
+
 #8. Processing your own sequences
 You will need to create a settings file with the calibration of your camera. See the settings file provided for the TUM and KITTI datasets for monocular, stereo and RGB-D cameras. We use the calibration model of OpenCV. See the examples to learn how to create a program that makes use of the ORB-SLAM2 library and how to pass images to the SLAM system. Stereo input must be synchronized and rectified. RGB-D input must be synchronized.