Fix bugs to pass compilation

.gitignore

@@ -4,6 +4,13 @@ Examples/Monocular/mono_kitti
 Examples/Monocular/mono_tum
 Examples/RGB-D/rgbd_tum
 Examples/ROS/ORB_SLAM2/CMakeLists.txt.user
+Examples/ROS/ORB_SLAM2/Mono
+Examples/ROS/ORB_SLAM2/MonoAR
+Examples/ROS/ORB_SLAM2/Monopub
+Examples/ROS/ORB_SLAM2/Monosub
+Examples/ROS/ORB_SLAM2/RGBD
+Examples/ROS/ORB_SLAM2/RGBDpub
+Examples/ROS/ORB_SLAM2/Stereo
 Examples/ROS/ORB_SLAM2/build/
 Examples/Stereo/stereo_euroc
 Examples/Stereo/stereo_kitti

CMakeLists.txt

@@ -37,12 +37,14 @@ endif()
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
+find_package(roscpp REQUIRED)
 
 include_directories(
 ${PROJECT_SOURCE_DIR}
 ${PROJECT_SOURCE_DIR}/include
 ${EIGEN3_INCLUDE_DIR}
 ${Pangolin_INCLUDE_DIRS}
+${roscpp_INCLUDE_DIRS}
 )
 
 set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)

Examples/Monocular/mono_euroc.cc

@@ -27,6 +27,7 @@
 #include<opencv2/core/core.hpp>
 
 #include<System.h>
+#include <unistd.h>
 
 using namespace std;
 

Examples/Monocular/mono_tum.cc

@@ -27,6 +27,7 @@
 #include<opencv2/core/core.hpp>
 
 #include<System.h>
+#include <unistd.h>
 
 using namespace std;
 

Examples/RGB-D/kinect.yaml

@@ -0,0 +1,70 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters. Adjust them!
+#--------------------------------------------------------------------------------------------
+
+# Camera calibration and distortion parameters (OpenCV) 
+Camera.fx: 519.236508
+Camera.fy: 518.041127
+Camera.cx: 326.303867
+Camera.cy: 253.288728
+
+Camera.k1: 0.006908
+Camera.k2: -0.036737
+Camera.p1: 0.003964
+Camera.p2: 0.003891
+Camera.k3: 0.000000
+
+Camera.width: 640
+Camera.height: 480
+
+# Camera frames per second 
+Camera.fps: 30.0
+
+# IR projector baseline times fx (aprox.)
+Camera.bf: 40.0
+
+# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+Camera.RGB: 1
+
+# Close/Far threshold. Baseline times.
+ThDepth: 40.0
+
+# Deptmap values factor 
+DepthMapFactor: 5000.0
+
+#--------------------------------------------------------------------------------------------
+# ORB Parameters
+#--------------------------------------------------------------------------------------------
+
+# ORB Extractor: Number of features per image
+ORBextractor.nFeatures: 1000
+
+# ORB Extractor: Scale factor between levels in the scale pyramid 	
+ORBextractor.scaleFactor: 1.2
+
+# ORB Extractor: Number of levels in the scale pyramid	
+ORBextractor.nLevels: 8
+
+# ORB Extractor: Fast threshold
+# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+# You can lower these values if your images have low contrast			
+ORBextractor.iniThFAST: 20
+ORBextractor.minThFAST: 7
+
+#--------------------------------------------------------------------------------------------
+# Viewer Parameters
+#--------------------------------------------------------------------------------------------
+Viewer.KeyFrameSize: 0.05
+Viewer.KeyFrameLineWidth: 1
+Viewer.GraphLineWidth: 0.9
+Viewer.PointSize:2
+Viewer.CameraSize: 0.08
+Viewer.CameraLineWidth: 3
+Viewer.ViewpointX: 0
+Viewer.ViewpointY: -0.7
+Viewer.ViewpointZ: -1.8
+Viewer.ViewpointF: 500
+

Examples/RGB-D/rgbd_tum.cc

@@ -27,6 +27,7 @@
 #include<opencv2/core/core.hpp>
 
 #include<System.h>
+#include <unistd.h>
 
 using namespace std;
 

Examples/ROS/ORB_SLAM2/CMakeLists.txt

@@ -41,6 +41,7 @@ endif()
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
 find_package(PCL 1.7 REQUIRED)
+find_package(Boost REQUIRED)
 
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
@@ -60,6 +61,7 @@ ${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
 ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
 ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
+${Boost_SYSTEM_LIBRARY}
 )
 
 # Node for monocular camera
@@ -116,3 +118,10 @@ target_link_libraries(RGBD
 ${LIBS}
 )
 
+rosbuild_add_executable(RGBDpub
+src/ros_rgbd_pub.cc
+)
+
+target_link_libraries(RGBDpub
+${LIBS}
+)

Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc

@@ -25,6 +25,7 @@
 #include <mutex>
 #include <thread>
 #include <cstdlib>
+#include <unistd.h>
 
 using namespace std;
 

Examples/ROS/ORB_SLAM2/src/ros_mono_pub.cc

@@ -60,20 +60,21 @@ void LoadImages(const string &strSequence, vector<string> &vstrImageFilenames,
 	vector<double> &vTimestamps);
 inline bool isInteger(const std::string & s);
 void publish(ORB_SLAM2::System &SLAM, ros::Publisher &pub_pts_and_pose,
-	ros::Publisher &pub_all_kf_and_pts, int frame_id);
+			 ros::Publisher &pub_all_kf_and_pts, ros::Publisher &pub_cur_camera_pose, int frame_id);
 
 class ImageGrabber{
 public:
 	ImageGrabber(ORB_SLAM2::System &_SLAM, ros::Publisher &_pub_pts_and_pose,
-		ros::Publisher &_pub_all_kf_and_pts) :
+		ros::Publisher &_pub_all_kf_and_pts, ros::Publisher &_pub_cur_camera_pose) :
 		SLAM(_SLAM), pub_pts_and_pose(_pub_pts_and_pose),
-		pub_all_kf_and_pts(_pub_all_kf_and_pts), frame_id(0){}
+		pub_all_kf_and_pts(_pub_all_kf_and_pts), pub_cur_camera_pose(_pub_cur_camera_pose), frame_id(0){}
 
 	void GrabImage(const sensor_msgs::ImageConstPtr& msg);
 
 	ORB_SLAM2::System &SLAM;
 	ros::Publisher &pub_pts_and_pose;
 	ros::Publisher &pub_all_kf_and_pts;
+	ros::Publisher &pub_cur_camera_pose;
 	int frame_id;
 };
 bool parseParams(int argc, char **argv);
@@ -94,8 +95,9 @@ int main(int argc, char **argv){
 	//ros::Publisher pub_cloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("cloud_in", 1000);
 	ros::Publisher pub_pts_and_pose = nodeHandler.advertise<geometry_msgs::PoseArray>("pts_and_pose", 1000);
 	ros::Publisher pub_all_kf_and_pts = nodeHandler.advertise<geometry_msgs::PoseArray>("all_kf_and_pts", 1000);
+	ros::Publisher pub_cur_camera_pose = nodeHandler.advertise<geometry_msgs::Pose>("/cur_camera_pose", 1000);
 	if (read_from_topic) {
-		ImageGrabber igb(SLAM, pub_pts_and_pose, pub_all_kf_and_pts);
+		ImageGrabber igb(SLAM, pub_pts_and_pose, pub_all_kf_and_pts, pub_cur_camera_pose);
 		ros::Subscriber sub = nodeHandler.subscribe(image_topic, 1, &ImageGrabber::GrabImage, &igb);
 		ros::spin();
 	}
@@ -131,7 +133,7 @@ int main(int argc, char **argv){
 			// Pass the image to the SLAM system
 			cv::Mat curr_pose = SLAM.TrackMonocular(im, tframe);
 
-			publish(SLAM, pub_pts_and_pose, pub_all_kf_and_pts, frame_id);
+			publish(SLAM, pub_pts_and_pose, pub_all_kf_and_pts, pub_cur_camera_pose, frame_id);
 
 			//cv::imshow("Press escape to exit", im);
 			//if (cv::waitKey(1) == 27) {
@@ -145,11 +147,13 @@ int main(int argc, char **argv){
 	//ros::spin();
 
 	mkdir("results", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
-	SLAM.getMap()->Save("results//map_pts_out.obj");
-	SLAM.getMap()->SaveWithTimestamps("results//map_pts_and_keyframes.txt");
-	// Save camera trajectory
+	SLAM.getMap()->Save("results//map_pts_out.obj");
+	SLAM.getMap()->SaveWithTimestamps("results//map_pts_and_keyframes.txt");
+	// Save camera trajectory
 	SLAM.SaveKeyFrameTrajectoryTUM("results//key_frame_trajectory.txt");
 
+	cout << "Press 'q' in the Frame Window to quit!" << endl;
+	while (cv::waitKey(0) != 'q') { }
 
 	// Stop all threads
 	SLAM.Shutdown();
@@ -161,7 +165,7 @@ int main(int argc, char **argv){
 }
 
 void publish(ORB_SLAM2::System &SLAM, ros::Publisher &pub_pts_and_pose,
-	ros::Publisher &pub_all_kf_and_pts, int frame_id) {
+			 ros::Publisher &pub_all_kf_and_pts, ros::Publisher &pub_cur_camera_pose, int frame_id) {
 	if (all_pts_pub_gap>0 && pub_count >= all_pts_pub_gap) {
 		pub_all_pts = true;
 		pub_count = 0;
@@ -321,6 +325,28 @@ void publish(ORB_SLAM2::System &SLAM, ros::Publisher &pub_pts_and_pose,
 		pub_pts_and_pose.publish(pt_array);
 		//pub_kf.publish(camera_pose);
 	}
+	// Publish current camera pose
+	if (!SLAM.getTracker()->mCurrentFrame.mTcw.empty())
+	{
+		cv::Mat Tcw = SLAM.getTracker()->mCurrentFrame.mTcw; 
+		cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t();
+		cv::Mat twc = -Rwc*Tcw.rowRange(0, 3).col(3);
+
+		vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);
+
+		geometry_msgs::Pose camera_pose;
+
+		camera_pose.position.x = twc.at<float>(0);
+		camera_pose.position.y = twc.at<float>(1);
+		camera_pose.position.z = twc.at<float>(2);
+
+		camera_pose.orientation.x = q[0];
+		camera_pose.orientation.y = q[1];
+		camera_pose.orientation.z = q[2];
+		camera_pose.orientation.w = q[3];
+
+		pub_cur_camera_pose.publish(camera_pose);
+	}
 }
 
 inline bool isInteger(const std::string & s){
@@ -334,7 +360,7 @@ inline bool isInteger(const std::string & s){
 
 void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps){
 	ifstream fTimes;
-	string strPathTimeFile = strPathToSequence + "/times.txt";
+	string strPathTimeFile = strPathToSequence + "/timestamps.txt";
 	fTimes.open(strPathTimeFile.c_str());
 	while (!fTimes.eof()){
 		string s;
@@ -348,15 +374,15 @@ void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilena
 		}
 	}
 
-	string strPrefixLeft = strPathToSequence + "/image_0/";
+	string strPrefixLeft = strPathToSequence + "/data/";
 
 	const int nTimes = vTimestamps.size();
 	vstrImageFilenames.resize(nTimes);
 
 	for (int i = 0; i < nTimes; i++)
 	{
 		stringstream ss;
-		ss << setfill('0') << setw(6) << i;
+		ss << setfill('0') << setw(10) << i;
 		vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
 	}
 }
@@ -372,7 +398,7 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg){
 		return;
 	}
 	SLAM.TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());
-	publish(SLAM, pub_pts_and_pose, pub_all_kf_and_pts, frame_id);
+	publish(SLAM, pub_pts_and_pose, pub_all_kf_and_pts, pub_cur_camera_pose, frame_id);
 	++frame_id;
 }