From e47348318fdb2fc62016d8182c8260fca29cd724 Mon Sep 17 00:00:00 2001 From: Ayush Date: Tue, 29 Nov 2016 18:21:30 +0530 Subject: [PATCH] Modified for ROS Kinetic --- CMakeLists.txt | 13 +---- Examples/ROS/ORB_SLAM2/CMakeLists.txt | 2 +- Examples/ROS/ORB_SLAM2/src/ros_stereo.cc | 64 ++++++++++++++++++++---- README.md | 8 ++- 4 files changed, 64 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fadce0f29c..556932de3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 2.4.3 REQUIRED) +find_package(OpenCV 3 REQUIRED) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) @@ -77,7 +77,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) add_executable(rgbd_tum Examples/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum ${PROJECT_NAME}) +target_link_libraries(rgbd_tum ${PROJECT_NAME} ${OpenCV_LIBS} opencv_core opencv_videoio opencv_highgui opencv_imgcodecs ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) @@ -85,11 +85,6 @@ add_executable(stereo_kitti Examples/Stereo/stereo_kitti.cc) target_link_libraries(stereo_kitti ${PROJECT_NAME}) -add_executable(stereo_euroc -Examples/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc ${PROJECT_NAME}) - - set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) add_executable(mono_tum @@ -100,7 +95,3 @@ add_executable(mono_kitti Examples/Monocular/mono_kitti.cc) target_link_libraries(mono_kitti ${PROJECT_NAME}) -add_executable(mono_euroc -Examples/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc ${PROJECT_NAME}) - diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt index 3c032fe8ab..61cd495665 100644 --- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt +++ b/Examples/ROS/ORB_SLAM2/CMakeLists.txt @@ -30,7 +30,7 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) -find_package(OpenCV 2.4.3 REQUIRED) +find_package(OpenCV 3 REQUIRED) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) diff --git a/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc b/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc index 9a0f9acc35..525760180f 100644 --- a/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc +++ b/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc @@ -1,4 +1,4 @@ -/** +/* * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) @@ -25,6 +25,8 @@ #include #include +#include "tf/transform_datatypes.h" +#include #include #include #include @@ -33,8 +35,10 @@ #include #include"../../../include/System.h" - +#include using namespace std; +cv::Mat pose; +ros::Publisher pose_pub; class ImageGrabber { @@ -114,6 +118,7 @@ int main(int argc, char **argv) typedef message_filters::sync_policies::ApproximateTime sync_pol; message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2)); + pose_pub = nh.advertise("/camera_pose",1); ros::spin(); @@ -121,15 +126,13 @@ int main(int argc, char **argv) SLAM.Shutdown(); // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); - SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); - SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); + //SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); + //SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); + //SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.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. @@ -160,13 +163,54 @@ void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const se cv::Mat imLeft, imRight; cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR); cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR); - mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()); + pose = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()); } else { - mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); + pose = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); } - + if (pose.empty()) + return; + //std::cout<(4,4) << 1,-1,-1, 1, + -1, 1,-1, 1, + -1,-1, 1, 1, + 1, 1, 1, 1); + cv::Mat translation = (pose * pose_prev.inv()).mul(flipSign); + world_lh = world_lh * translation; + pose_prev = pose.clone(); + /* transform into global right handed coordinate system, publish in ROS*/ + tf::Matrix3x3 cameraRotation_rh( - world_lh.at(0,0), world_lh.at(0,1), world_lh.at(0,2), + - world_lh.at(1,0), world_lh.at(1,1), world_lh.at(1,2), + world_lh.at(2,0), - world_lh.at(2,1), - world_lh.at(2,2)); + + tf::Vector3 cameraTranslation_rh( world_lh.at(0,3),world_lh.at(1,3), - world_lh.at(2,3) ); + + //rotate 270deg about x and 270deg about x to get ENU: x forward, y left, z up + const tf::Matrix3x3 rotation270degXZ( 0, 1, 0, + 0, 0, 1, + 1, 0, 0); + static tf::TransformBroadcaster br; + tf::Matrix3x3 globalRotation_rh = cameraRotation_rh * rotation270degXZ; + tf::Vector3 globalTranslation_rh = cameraTranslation_rh * rotation270degXZ; + tf::Transform transform = tf::Transform(globalRotation_rh, globalTranslation_rh); + br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_link", "camera_pose")); + geometry_msgs::PoseStamped _pose; + _pose.pose.position.x = transform.getOrigin().x(); + _pose.pose.position.y = transform.getOrigin().y(); + _pose.pose.position.z = transform.getOrigin().z(); + _pose.pose.orientation.x = transform.getRotation().x(); + _pose.pose.orientation.y = transform.getRotation().y(); + _pose.pose.orientation.z = transform.getRotation().z(); + _pose.pose.orientation.w = transform.getRotation().w(); + + _pose.header.stamp = ros::Time::now(); + _pose.header.frame_id = "camera_link"; + pose_pub.publish(_pose); } diff --git a/README.md b/README.md index 4710dad7b5..7f24989591 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,11 @@ # ORB-SLAM2 -**Authors:** [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)) +Modified version of ORB-SLAM2 + +**Changelog** Compatible with ROS Kinetic (OpenCV 3, Ubuntu 16.04) + Stereo node pose and tf publisher + + +**Authors:** [Ayush Gaud] [Raul Mur-Artal (Original Author)](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)) **Current version:** 1.0.0