Modified for ROS Kinetic

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,19 +77,14 @@ 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)
 
 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})
-

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)
 

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 <raulmur at unizar dot es> (University of Zaragoza)
@@ -25,6 +25,8 @@
 #include<chrono>
 
 #include<ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include <tf/transform_broadcaster.h>
 #include <cv_bridge/cv_bridge.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>
@@ -33,8 +35,10 @@
 #include<opencv2/core/core.hpp>
 
 #include"../../../include/System.h"
-
+#include <iostream>
 using namespace std;
+cv::Mat pose;
+ros::Publisher pose_pub; 
 
 class ImageGrabber
 {
@@ -114,22 +118,21 @@ int main(int argc, char **argv)
     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));
+    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/camera_pose",1);
 
     ros::spin();
 
     // Stop all threads
     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<<pose<<std::endl;
+	/* global left handed coordinate system */
+    	static cv::Mat pose_prev = cv::Mat::eye(4,4, CV_32F);
+    	static cv::Mat world_lh = cv::Mat::eye(4,4, CV_32F);
+    	// matrix to flip signs of sinus in rotation matrix, not sure why we need to do that
+    	static const cv::Mat flipSign = (cv::Mat_<float>(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<float>(0,0),   world_lh.at<float>(0,1),   world_lh.at<float>(0,2),
+                                  - world_lh.at<float>(1,0),   world_lh.at<float>(1,1),   world_lh.at<float>(1,2),
+                                    world_lh.at<float>(2,0), - world_lh.at<float>(2,1), - world_lh.at<float>(2,2));
+
+	tf::Vector3 cameraTranslation_rh( world_lh.at<float>(0,3),world_lh.at<float>(1,3), - world_lh.at<float>(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);
 }
 
 

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