├── .gitignore ├── CMakeLists.txt ├── README.md ├── config └── euroc │ ├── camchain.yaml │ └── euroc.yaml ├── dependencies.rosinstall ├── include └── orb_slam_2_ros │ ├── interface.hpp │ ├── interface_mono.hpp │ ├── interface_stereo.hpp │ └── types.hpp ├── launch └── run_orb_slam_2.launch ├── package.xml └── src ├── library ├── interface.cpp ├── interface_mono.cpp └── interface_stereo.cpp └── orb_slam_2_ros_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Project Files 32 | .clang_complete -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.0) 2 | project(orb_slam_2_ros) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin_simple REQUIRED) 7 | catkin_simple(ALL_DEPS_REQUIRED) 8 | 9 | find_package(Eigen REQUIRED) 10 | include_directories(${Eigen_INCLUDE_DIRS}) 11 | 12 | cs_add_library(orb_slam_2_interface 13 | src/library/interface.cpp 14 | src/library/interface_mono.cpp 15 | src/library/interface_stereo.cpp 16 | ) 17 | 18 | cs_add_executable(orb_slam_2_ros_node 19 | src/orb_slam_2_ros_node.cpp 20 | ) 21 | 22 | target_link_libraries(orb_slam_2_ros_node orb_slam_2_interface ${catkin_LIBRARIES}) 23 | 24 | cs_install() 25 | cs_export() 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # orb_slam_2_ros 2 | 3 | ## Overview 4 | 5 | This package integrates orb_slam_2 into ROS in what we believe to be a more user friendly way than what is offered by the original library [ORB_SLAM2](https://github.com/raulmur/ORB_SLAM2) 6 | 7 | **Keywords:** ROS, ORB_SLAM2, SLAM 8 | 9 | ### License 10 | 11 | The source code is released under a [GPLv3 license](https://github.com/raulmur/ORB_SLAM2/blob/master/License-gpl.txt) as is the underlying library [ORB_SLAM2](https://github.com/raulmur/ORB_SLAM2). 12 | 13 | **Author(s): Alex Millane 14 | Maintainer: Alex Millane 15 | Affiliation: Autonomous Systems Lab, ETH Zurich** 16 | 17 | The orb_slam_2_ros package has been tested under ROS Indigo on Ubuntu 14.04 and under ROS Kinetic Ubuntu 16.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 18 | 19 | 20 | ## Installation: building from Source 21 | 22 | Install [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics). 23 | 24 | Make sure you have added your SSH keys to your Github account. For more info check [connecting-to-github-with-ssh](https://help.github.com/articles/connecting-to-github-with-ssh/). 25 | 26 | Setup and configure a catkin work space. 27 | 28 | ``` 29 | mkdir -p ~/catkin_ws/src 30 | cd ~/catkin_ws 31 | catkin init 32 | catkin config --extend /opt/ros/kinetic 33 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 34 | catkin config --merge-devel 35 | ``` 36 | Get this repo 37 | 38 | ``` 39 | cd src 40 | git clone git@github.com:ethz-asl/orb_slam_2_catkin.git 41 | ``` 42 | Get the deps 43 | 44 | ``` 45 | wstool init 46 | wstool merge orb_slam_2_ros/dependencies.rosinstall 47 | wstool update -j8 48 | ``` 49 | 50 | Build everything 51 | 52 | ``` 53 | catkin build orb_slam_2_ros 54 | ``` 55 | 56 | ## Usage 57 | 58 | Before launching the node with an example dataset you need to: 59 | - Download a dataset you can use to run this package from [EUROC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) (for example the one called "Machine Hall 001") (containing rosbag to play and camera calibration); 60 | - Download and extract the [ORB_SLAM2 vocabulary](https://github.com/raulmur/ORB_SLAM2/blob/master/Vocabulary/ORBvoc.txt.tar.gz). 61 | 62 | Run the main node with 63 | 64 | roslaunch orb_slam_2_ros run_orb_slam_2.launch vocabulary_file_path:= 65 | 66 | Please not that this version of orb_slam_2_ros **DOES NOT** rectify images, that is why we are using [stereo_undistort_node](https://github.com/ethz-asl/image_undistort#stereo_undistort_node). 67 | -------------------------------------------------------------------------------- /config/euroc/camchain.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_overlaps: [1] 3 | camera_model: pinhole 4 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 5 | distortion_model: radtan 6 | intrinsics: [458.654, 457.296, 367.215, 248.375] 7 | resolution: [752, 480] 8 | rostopic: /cam0/image_raw 9 | cam1: 10 | T_cn_cnm1: 11 | - [0.9999972565, 0.0023120672, 0.0003760081, -0.1100738081] 12 | - [-0.0023171357, 0.9998980485, 0.0140898358, 0.0003991215] 13 | - [-0.0003433931, -0.0140906685, 0.9999006626, -0.0008537025] 14 | - [0.0, 0.0, 0.0, 1.0] 15 | cam_overlaps: [0] 16 | camera_model: pinhole 17 | distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 18 | distortion_model: radtan 19 | intrinsics: [457.587, 456.134, 379.999, 255.238] 20 | resolution: [752, 480] 21 | rostopic: /cam1/image_raw -------------------------------------------------------------------------------- /config/euroc/euroc.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 457.975 9 | Camera.fy: 457.975 10 | Camera.cx: 467.0 11 | Camera.cy: 235.5 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 934 #752 19 | Camera.height: 471 #480 20 | 21 | # Camera frames per second 22 | Camera.fps: 20.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 50.41289976523141 #47.90639384423901 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # Stereo Rectification done by using camchain.yaml and stereo_undistort_node form package 35 | # image_undistort (ethz-asl Github) 36 | #-------------------------------------------------------------------------------------------- 37 | 38 | #-------------------------------------------------------------------------------------------- 39 | # ORB Parameters 40 | #-------------------------------------------------------------------------------------------- 41 | 42 | # ORB Extractor: Number of features per image 43 | ORBextractor.nFeatures: 1200 44 | 45 | # ORB Extractor: Scale factor between levels in the scale pyramid 46 | ORBextractor.scaleFactor: 1.2 47 | 48 | # ORB Extractor: Number of levels in the scale pyramid 49 | ORBextractor.nLevels: 8 50 | 51 | # ORB Extractor: Fast threshold 52 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 53 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 54 | # You can lower these values if your images have low contrast 55 | ORBextractor.iniThFAST: 20 56 | ORBextractor.minThFAST: 7 57 | 58 | #-------------------------------------------------------------------------------------------- 59 | # Viewer Parameters 60 | #-------------------------------------------------------------------------------------------- 61 | Viewer.KeyFrameSize: 0.05 62 | Viewer.KeyFrameLineWidth: 1 63 | Viewer.GraphLineWidth: 0.9 64 | Viewer.PointSize:2 65 | Viewer.CameraSize: 0.08 66 | Viewer.CameraLineWidth: 3 67 | Viewer.ViewpointX: 0 68 | Viewer.ViewpointY: -0.7 69 | Viewer.ViewpointZ: -1.8 70 | Viewer.ViewpointF: 500 71 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: git@github.com:catkin/catkin_simple.git 4 | - git: 5 | local-name: orb_slam_2_catkin 6 | uri: git@github.com:ethz-asl/orb_slam_2_catkin.git 7 | - git: 8 | local-name: image_undistort 9 | uri: https://github.com/ethz-asl/image_undistort.git 10 | - git: 11 | local-name: pangolin_catkin 12 | uri: https://github.com/uzh-rpg/pangolin_catkin.git 13 | - git: 14 | local-name: glog_catkin 15 | uri: https://github.com/ethz-asl/glog_catkin.git 16 | - git: 17 | local-name: minkindr 18 | uri: https://github.com/ethz-asl/minkindr.git 19 | - git: 20 | local-name: minkindr_ros 21 | uri: https://github.com/ethz-asl/minkindr_ros.git 22 | - git: 23 | local-name: gflags_catkin 24 | uri: https://github.com/ethz-asl/gflags_catkin.git 25 | - git: 26 | local-name: eigen_catkin 27 | uri: https://github.com/ethz-asl/eigen_catkin.git 28 | - git: 29 | local-name: eigen_checks 30 | uri: https://github.com/ethz-asl/eigen_checks.git 31 | -------------------------------------------------------------------------------- /include/orb_slam_2_ros/interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ORB_SLAM_2_INTERFACE 2 | #define ORB_SLAM_2_INTERFACE 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "orb_slam_2_ros/types.hpp" 15 | 16 | namespace orb_slam_2_interface { 17 | 18 | // Default values for parameters 19 | static const bool kDefaultVerbose = true; 20 | static const std::string kDefaultFrameId = "world"; 21 | static const std::string kDefaultChildFrameId = "cam0"; 22 | 23 | // Class handling global alignment calculation and publishing 24 | class OrbSlam2Interface { 25 | public: 26 | // Constructor 27 | OrbSlam2Interface(const ros::NodeHandle& nh, 28 | const ros::NodeHandle& nh_private); 29 | 30 | protected: 31 | // Subscribes and Advertises to the appropriate ROS topics 32 | void advertiseTopics(); 33 | void getParametersFromRos(); 34 | 35 | // Callbacks 36 | void imageCallback(const sensor_msgs::ImageConstPtr& msg); 37 | 38 | // Publishing functions 39 | void publishCurrentPose(const Transformation& T, 40 | const std_msgs::Header& header); 41 | void publishCurrentPoseAsTF(const ros::TimerEvent& event); 42 | 43 | // Helper functions 44 | void convertOrbSlamPoseToKindr(const cv::Mat& T_cv, Transformation* T_kindr); 45 | 46 | // Node handles 47 | ros::NodeHandle nh_; 48 | ros::NodeHandle nh_private_; 49 | 50 | // Publishers 51 | ros::Publisher T_pub_; 52 | tf::TransformBroadcaster tf_broadcaster_; 53 | ros::Timer tf_timer_; 54 | 55 | // The orb slam system 56 | std::shared_ptr slam_system_; 57 | 58 | // The current pose 59 | Transformation T_W_C_; 60 | 61 | // Parameters 62 | bool verbose_; 63 | std::string vocabulary_file_path_; 64 | std::string settings_file_path_; 65 | 66 | // Transform frame names 67 | std::string frame_id_; 68 | std::string child_frame_id_; 69 | }; 70 | 71 | } // namespace orb_slam_2_interface 72 | 73 | #endif /* ORB_SLAM_2_INTERFACE */ 74 | -------------------------------------------------------------------------------- /include/orb_slam_2_ros/interface_mono.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ORB_SLAM_2_INTERFACE_MONO 2 | #define ORB_SLAM_2_INTERFACE_MONO 3 | 4 | #include 5 | #include 6 | 7 | #include "orb_slam_2_ros/interface.hpp" 8 | 9 | namespace orb_slam_2_interface { 10 | 11 | // Class handling global alignment calculation and publishing 12 | class OrbSlam2InterfaceMono : public OrbSlam2Interface { 13 | public: 14 | // Constructor 15 | OrbSlam2InterfaceMono(const ros::NodeHandle& nh, 16 | const ros::NodeHandle& nh_private, 17 | const bool visualization); 18 | 19 | protected: 20 | // Subscribes to the appropriate ROS topics 21 | void subscribeToTopics(); 22 | 23 | // Callbacks 24 | void imageCallback(const sensor_msgs::ImageConstPtr& msg); 25 | 26 | // Subscribers 27 | ros::Subscriber image_sub_; 28 | 29 | }; 30 | 31 | } // namespace orb_slam_2_interface 32 | 33 | #endif /* ORB_SLAM_2_INTERFACE_MONO */ 34 | -------------------------------------------------------------------------------- /include/orb_slam_2_ros/interface_stereo.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ORB_SLAM_2_INTERFACE_STEREO 2 | #define ORB_SLAM_2_INTERFACE_STEREO 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "orb_slam_2_ros/interface.hpp" 12 | 13 | namespace orb_slam_2_interface { 14 | 15 | // The synchronization policy used by the interface to sync stereo images 16 | typedef message_filters::sync_policies::ApproximateTime 18 | sync_pol; 19 | 20 | // Class handling global alignment calculation and publishing 21 | class OrbSlam2InterfaceStereo : public OrbSlam2Interface { 22 | public: 23 | // Constructor 24 | OrbSlam2InterfaceStereo(const ros::NodeHandle& nh, 25 | const ros::NodeHandle& nh_private, 26 | const bool visualization); 27 | 28 | protected: 29 | // Subscribes to the appropriate ROS topics 30 | void subscribeToTopics(); 31 | 32 | // Callbacks 33 | void stereoImageCallback(const sensor_msgs::ImageConstPtr& msg_left, 34 | const sensor_msgs::ImageConstPtr& msg_right); 35 | 36 | // Subscribers 37 | std::shared_ptr> left_sub_; 38 | std::shared_ptr> right_sub_; 39 | std::shared_ptr> sync_; 40 | }; 41 | 42 | } // namespace orb_slam_2_interface 43 | 44 | #endif /* ORB_SLAM_2_INTERFACE_STEREO */ 45 | -------------------------------------------------------------------------------- /include/orb_slam_2_ros/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ORB_SLAM_2_TYPES 2 | #define ORB_SLAM_2_TYPES 3 | 4 | #include 5 | #include 6 | 7 | namespace orb_slam_2_interface { 8 | 9 | // Convenience typedef 10 | typedef kindr::minimal::QuatTransformation Transformation; 11 | typedef kindr::minimal::RotationQuaternion Quaternion; 12 | 13 | } // namespace orb_slam_2_interface 14 | 15 | #endif /* ORB_SLAM_2_TYPES */ 16 | -------------------------------------------------------------------------------- /launch/run_orb_slam_2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orb_slam_2_ros 4 | 0.0.0 5 | 6 | Provides a ROS interface for orb slam 2. 7 | 8 | 9 | Alex Millane 10 | Alex Millane 11 | New BSD 12 | 13 | 14 | catkin 15 | catkin_simple 16 | 17 | 18 | roscpp 19 | cmake_modules 20 | sensor_msgs 21 | glog_catkin 22 | cv_bridge 23 | orb_slam_2_catkin 24 | pangolin_catkin 25 | minkindr 26 | minkindr_conversions 27 | image_undistort 28 | 29 | 30 | -------------------------------------------------------------------------------- /src/library/interface.cpp: -------------------------------------------------------------------------------- 1 | #include "orb_slam_2_ros/interface.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace orb_slam_2_interface { 10 | 11 | OrbSlam2Interface::OrbSlam2Interface(const ros::NodeHandle& nh, 12 | const ros::NodeHandle& nh_private) 13 | : nh_(nh), 14 | nh_private_(nh_private), 15 | verbose_(kDefaultVerbose), 16 | frame_id_(kDefaultFrameId), 17 | child_frame_id_(kDefaultChildFrameId) { 18 | // Getting data and params 19 | advertiseTopics(); 20 | getParametersFromRos(); 21 | } 22 | 23 | void OrbSlam2Interface::advertiseTopics() { 24 | // Advertising topics 25 | T_pub_ = nh_private_.advertise( 26 | "transform_cam", 1); 27 | // Creating a callback timer for TF publisher 28 | tf_timer_ = nh_.createTimer(ros::Duration(0.01), 29 | &OrbSlam2Interface::publishCurrentPoseAsTF, this); 30 | } 31 | 32 | void OrbSlam2Interface::getParametersFromRos() { 33 | // Getting the paths to the files required by orb slam 34 | CHECK(nh_private_.getParam("vocabulary_file_path", vocabulary_file_path_)) 35 | << "Please provide the vocabulary_file_path as a ros param."; 36 | CHECK(nh_private_.getParam("settings_file_path", settings_file_path_)) 37 | << "Please provide the settings_file_path as a ros param."; 38 | // Optional params 39 | nh_private_.getParam("verbose", verbose_); 40 | nh_private_.getParam("frame_id", frame_id_); 41 | nh_private_.getParam("child_frame_id", child_frame_id_); 42 | } 43 | 44 | void OrbSlam2Interface::publishCurrentPose(const Transformation& T, 45 | const std_msgs::Header& header) { 46 | // Creating the message 47 | geometry_msgs::TransformStamped msg; 48 | // Filling out the header 49 | msg.header = header; 50 | // Setting the child and parent frames 51 | msg.child_frame_id = child_frame_id_; 52 | // Converting from a minkindr transform to a transform message 53 | tf::transformKindrToMsg(T, &msg.transform); 54 | // Publishing the current transformation. 55 | T_pub_.publish(msg); 56 | } 57 | 58 | void OrbSlam2Interface::publishCurrentPoseAsTF(const ros::TimerEvent& event) { 59 | tf::Transform tf_transform; 60 | tf::transformKindrToTF(T_W_C_, &tf_transform); 61 | tf_broadcaster_.sendTransform(tf::StampedTransform( 62 | tf_transform, ros::Time::now(), frame_id_, child_frame_id_)); 63 | } 64 | 65 | void OrbSlam2Interface::convertOrbSlamPoseToKindr(const cv::Mat& T_cv, 66 | Transformation* T_kindr) { 67 | // Argument checks 68 | CHECK_NOTNULL(T_kindr); 69 | CHECK_EQ(T_cv.cols, 4); 70 | CHECK_EQ(T_cv.rows, 4); 71 | // Open CV mat to Eigen matrix (float) 72 | Eigen::Matrix4f T_eigen_f; 73 | cv::cv2eigen(T_cv, T_eigen_f); 74 | // Eigen matrix (float) to Eigen matrix (double) 75 | Eigen::Matrix4d T_eigen_d = T_eigen_f.cast(); 76 | // Extracting and orthonormalizing the rotation matrix 77 | Eigen::Matrix3d R_unnormalized = T_eigen_d.block<3, 3>(0, 0); 78 | Eigen::AngleAxisd aa(R_unnormalized); 79 | Eigen::Matrix3d R = aa.toRotationMatrix(); 80 | // Constructing the transformation 81 | Quaternion q_kindr(R); 82 | Eigen::Vector3d t_kindr(T_eigen_d.block<3, 1>(0, 3)); 83 | *T_kindr = Transformation(q_kindr, t_kindr); 84 | } 85 | 86 | } // namespace orb_slam_2_interface 87 | -------------------------------------------------------------------------------- /src/library/interface_mono.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "orb_slam_2_ros/interface_mono.hpp" 4 | 5 | namespace orb_slam_2_interface { 6 | 7 | OrbSlam2InterfaceMono::OrbSlam2InterfaceMono(const ros::NodeHandle& nh, 8 | const ros::NodeHandle& nh_private, 9 | const bool visualization) 10 | : OrbSlam2Interface(nh, nh_private) { 11 | // Getting data and params 12 | subscribeToTopics(); 13 | //advertiseTopics(); 14 | //getParametersFromRos(); 15 | // Creating the SlAM system 16 | slam_system_ = std::shared_ptr( 17 | new ORB_SLAM2::System(vocabulary_file_path_, settings_file_path_, 18 | ORB_SLAM2::System::MONOCULAR, visualization)); 19 | } 20 | 21 | void OrbSlam2InterfaceMono::subscribeToTopics() { 22 | // Subscribing to the required data topics 23 | image_sub_ = nh_.subscribe("camera/image_raw", 1, 24 | &OrbSlam2InterfaceMono::imageCallback, this); 25 | } 26 | 27 | void OrbSlam2InterfaceMono::imageCallback(const sensor_msgs::ImageConstPtr& msg) { 28 | // Copy the ros image message to cv::Mat. 29 | cv_bridge::CvImageConstPtr cv_ptr; 30 | try { 31 | cv_ptr = cv_bridge::toCvShare(msg); 32 | } catch (cv_bridge::Exception& e) { 33 | ROS_ERROR("cv_bridge exception: %s", e.what()); 34 | return; 35 | } 36 | // Handing the image to ORB slam for tracking 37 | cv::Mat T_C_W_opencv = 38 | slam_system_->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec()); 39 | // If tracking successfull 40 | if (!T_C_W_opencv.empty()) { 41 | // Converting to kindr transform and publishing 42 | Transformation T_C_W, T_W_C; 43 | convertOrbSlamPoseToKindr(T_C_W_opencv, &T_C_W); 44 | T_W_C = T_C_W.inverse(); 45 | publishCurrentPose(T_W_C, msg->header); 46 | // Saving the transform to the member for publishing as a TF 47 | T_W_C_ = T_W_C; 48 | } 49 | } 50 | 51 | } // namespace orb_slam_2_interface 52 | -------------------------------------------------------------------------------- /src/library/interface_stereo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "orb_slam_2_ros/interface_stereo.hpp" 4 | 5 | namespace orb_slam_2_interface { 6 | 7 | OrbSlam2InterfaceStereo::OrbSlam2InterfaceStereo(const ros::NodeHandle& nh, 8 | const ros::NodeHandle& nh_private, 9 | const bool visualization) 10 | : OrbSlam2Interface(nh, nh_private) { 11 | // Getting data and params 12 | subscribeToTopics(); 13 | //advertiseTopics(); 14 | //getParametersFromRos(); 15 | slam_system_ = std::shared_ptr( 16 | new ORB_SLAM2::System(vocabulary_file_path_, settings_file_path_, 17 | ORB_SLAM2::System::STEREO, visualization)); 18 | } 19 | 20 | void OrbSlam2InterfaceStereo::subscribeToTopics() { 21 | // Subscribing to the stereo images 22 | left_sub_ = std::shared_ptr>( 23 | new message_filters::Subscriber( 24 | nh_, "camera/left/image_raw", 1)); 25 | right_sub_ = std::shared_ptr>( 26 | new message_filters::Subscriber( 27 | nh_, "camera/right/image_raw", 1)); 28 | // Creating a synchronizer 29 | sync_ = std::shared_ptr>( 30 | new message_filters::Synchronizer(sync_pol(10), *left_sub_, 31 | *right_sub_)); 32 | // Registering the synchronized image callback 33 | sync_->registerCallback( 34 | boost::bind(&OrbSlam2InterfaceStereo::stereoImageCallback, this, _1, _2)); 35 | } 36 | 37 | void OrbSlam2InterfaceStereo::stereoImageCallback( 38 | const sensor_msgs::ImageConstPtr& msg_left, 39 | const sensor_msgs::ImageConstPtr& msg_right) { 40 | // Copy the ros image message to cv::Mat. 41 | cv_bridge::CvImageConstPtr cv_ptr_left; 42 | try { 43 | cv_ptr_left = cv_bridge::toCvShare(msg_left); 44 | } catch (cv_bridge::Exception& e) { 45 | ROS_ERROR("cv_bridge exception: %s", e.what()); 46 | return; 47 | } 48 | cv_bridge::CvImageConstPtr cv_ptr_right; 49 | try { 50 | cv_ptr_right = cv_bridge::toCvShare(msg_right); 51 | } catch (cv_bridge::Exception& e) { 52 | ROS_ERROR("cv_bridge exception: %s", e.what()); 53 | return; 54 | } 55 | // Handing the image to ORB slam for tracking 56 | cv::Mat T_C_W_opencv = 57 | slam_system_->TrackStereo(cv_ptr_left->image, cv_ptr_right->image, 58 | cv_ptr_left->header.stamp.toSec()); 59 | // If tracking successfull 60 | if (!T_C_W_opencv.empty()) { 61 | // Converting to kindr transform and publishing 62 | Transformation T_C_W, T_W_C; 63 | convertOrbSlamPoseToKindr(T_C_W_opencv, &T_C_W); 64 | T_W_C = T_C_W.inverse(); 65 | publishCurrentPose(T_W_C, msg_left->header); 66 | // Saving the transform to the member for publishing as a TF 67 | T_W_C_ = T_W_C; 68 | } 69 | 70 | } 71 | 72 | } // namespace orb_slam_2_interface 73 | -------------------------------------------------------------------------------- /src/orb_slam_2_ros_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include "orb_slam_2_ros/interface.hpp" 8 | #include "orb_slam_2_ros/interface_mono.hpp" 9 | #include "orb_slam_2_ros/interface_stereo.hpp" 10 | 11 | // A factory method for creating an interface 12 | std::unique_ptr create_interface( 13 | std::string interface_type, const ros::NodeHandle& nh, 14 | const ros::NodeHandle& nh_private, const bool visualization) { 15 | // Creating the aligner object subclass dependent on the argument 16 | std::unique_ptr interface; 17 | if (interface_type == "mono") { 18 | interface = std::unique_ptr( 19 | new orb_slam_2_interface::OrbSlam2InterfaceMono(nh, nh_private, visualization)); 20 | } else if (interface_type == "stereo") { 21 | interface = std::unique_ptr( 22 | new orb_slam_2_interface::OrbSlam2InterfaceStereo(nh, nh_private, visualization)); 23 | } else { 24 | ROS_FATAL( 25 | "interface type not recognized. Must be mono or stereo."); 26 | ros::shutdown(); 27 | exit(1); 28 | } 29 | // Returning a pointer to the frame aligner 30 | return interface; 31 | } 32 | 33 | // Standard C++ entry point 34 | int main(int argc, char** argv) { 35 | // Starting the logging 36 | google::InitGoogleLogging(argv[0]); 37 | // Announce this program to the ROS master 38 | ros::init(argc, argv, "orb_slam_2_ros_node"); 39 | // Creating the node handles 40 | ros::NodeHandle nh; 41 | ros::NodeHandle nh_private("~"); 42 | // Get the parameter describing the interface type 43 | static const std::string kDefaultInterfaceType = "mono"; 44 | std::string interface_type = kDefaultInterfaceType; 45 | nh_private.getParam("interface_type", interface_type); 46 | bool visualization = false; 47 | nh_private.getParam("visualization", visualization); 48 | // Creating the interface object to do the work 49 | std::unique_ptr interface = 50 | create_interface(interface_type, nh, nh_private, visualization); 51 | // Spinning 52 | ros::spin(); 53 | // Exit tranquilly 54 | return 0; 55 | } 56 | --------------------------------------------------------------------------------