Adding the initial map save load changes

CMakeLists.txt

@@ -7,8 +7,8 @@ ENDIF()
 
 MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3 -march=native")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall -O0 -g  -march=native ")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O0 -g -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)
@@ -31,6 +31,10 @@ LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 find_package(OpenCV 2.4.3 REQUIRED)
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
+find_package(Boost  1.54.0  	     # Minimum or EXACT version e.g. 1.36.0
+  REQUIRED             					# Fail with error if Boost is not found
+  COMPONENTS serialization 				# Boost libraries by their canonical name
+  )          
 
 include_directories(
 ${PROJECT_SOURCE_DIR}
@@ -66,9 +70,10 @@ src/Viewer.cc
 target_link_libraries(${PROJECT_NAME}
 ${OpenCV_LIBS}
 ${EIGEN3_LIBS}
-${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
+${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
+${Boost_LIBRARIES}
 )
 
 # Build examples
@@ -95,3 +100,8 @@ add_executable(mono_kitti
 Examples/Monocular/mono_kitti.cc)
 target_link_libraries(mono_kitti ${PROJECT_NAME})
 
+# Build tools
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/tools)
+add_executable(bin_vocabulary
+tools/bin_vocabulary.cc)
+target_link_libraries(bin_vocabulary ${PROJECT_NAME})

Examples/ROS/ORB_SLAM2/CMakeLists.txt

@@ -9,8 +9,8 @@ ENDIF()
 
 MESSAGE("Build type: " ${ROS_BUILD_TYPE})
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -g -march=native ")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -g -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)
@@ -59,6 +59,7 @@ target_link_libraries(Mono
 ${LIBS}
 )
 
+
 # Node for stereo camera
 rosbuild_add_executable(Stereo
 src/ros_stereo.cc

Examples/ROS/ORB_SLAM2/src/ros_mono.cc

@@ -26,8 +26,12 @@
 
 #include<ros/ros.h>
 #include <cv_bridge/cv_bridge.h>
+#include "geometry_msgs/PoseStamped.h"
+#include <tf/transform_broadcaster.h>
 
 #include<opencv2/core/core.hpp>
+#include "Converter.h"
+
 
 #include"../../../include/System.h"
 
@@ -40,37 +44,101 @@ class ImageGrabber
 
     void GrabImage(const sensor_msgs::ImageConstPtr& msg);
 
+    void PublishPose(cv::Mat Tcw);
+
     ORB_SLAM2::System* mpSLAM;
+    ros::Publisher* pPosPub;
+
 };
 
+//ros::Publisher pPosPub;
+
+void ImageGrabber::PublishPose(cv::Mat Tcw)
+{
+    geometry_msgs::PoseStamped poseMSG;
+    if(!Tcw.empty())
+    {
+
+        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);
+
+
+        /*
+            cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
+            cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
+            tf::Matrix3x3 M(Rwc.at<float>(0,0),Rwc.at<float>(0,1),Rwc.at<float>(0,2),
+                            Rwc.at<float>(1,0),Rwc.at<float>(1,1),Rwc.at<float>(1,2),
+                            Rwc.at<float>(2,0),Rwc.at<float>(2,1),Rwc.at<float>(2,2));
+            tf::Vector3 V(twc.at<float>(0), twc.at<float>(1), twc.at<float>(2));
+
+            tf::Transform tfTcw(M,V);
+
+            //mTfBr.sendTransform(tf::StampedTransform(tfTcw,ros::Time::now(), "ORB_SLAM/World", "ORB_SLAM/Camera"));
+        */
+        poseMSG.pose.position.x = twc.at<float>(0);
+        poseMSG.pose.position.y = twc.at<float>(2);
+        poseMSG.pose.position.z = twc.at<float>(1);
+        poseMSG.pose.orientation.x = q[0];
+        poseMSG.pose.orientation.y = q[1];
+        poseMSG.pose.orientation.z = q[2];
+        poseMSG.pose.orientation.w = q[3];
+        poseMSG.header.frame_id = "VSLAM";
+        poseMSG.header.stamp = ros::Time::now();
+        //cout << "PublishPose position.x = " << poseMSG.pose.position.x << endl;
+
+        (pPosPub)->publish(poseMSG);
+
+        //mlbLost.push_back(mState==LOST);
+    }
+}
+
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "Mono");
     ros::start();
-
-    if(argc != 3)
+	bool bReuseMap = false;
+    if(argc < 4)
     {
         cerr << endl << "Usage: rosrun ORB_SLAM2 Mono 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::MONOCULAR,true);
+	if (!strcmp(argv[3], "true"))
+    {
+		bReuseMap = true;
+	}
+   	ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, bReuseMap);
+
+    //if (bReuseMap)
+		//SLAM.LoadMap("Slam_Map.bin");
+
+	ImageGrabber igb(&SLAM);
 
-    ImageGrabber igb(&SLAM);
 
     ros::NodeHandle nodeHandler;
     ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
+    // Pose broadcaster
+    //pPosPub = new ros::Publisher;
+    ros::Publisher PosPub = nodeHandler.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5);
+
+        igb.pPosPub = &(PosPub);
 
     ros::spin();
 
     // Stop all threads
     SLAM.Shutdown();
 
+
+    // Save map
+    SLAM.SaveMap("Slam_latest_Map.bin");
+
+
     // Save camera trajectory
     SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
-
     ros::shutdown();
 
     return 0;
@@ -90,7 +158,9 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
         return;
     }
 
-    mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
+    cv::Mat Tcw= mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
+    PublishPose(Tcw);
+    //usleep(10000);
 }
 
 

Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h

@@ -246,6 +246,19 @@ class TemplatedVocabulary
    */
   void saveToTextFile(const std::string &filename) const;  
 
+  /**
+   * Loads the vocabulary from a binary file
+   * @param filename
+   */
+  bool loadFromBinaryFile(const std::string &filename);
+
+  /**
+   * Saves the vocabulary into a binary file
+   * @param filename
+   */
+  void saveToBinaryFile(const std::string &filename) const;  
+
+
   /**
    * Saves the vocabulary into a file
    * @param filename
@@ -1450,6 +1463,78 @@ void TemplatedVocabulary<TDescriptor,F>::saveToTextFile(const std::string &filen
 
 // --------------------------------------------------------------------------
 
+template<class TDescriptor, class F>
+bool TemplatedVocabulary<TDescriptor,F>::loadFromBinaryFile(const std::string &filename) {
+  fstream f;
+  f.open(filename.c_str(), ios_base::in|ios::binary);
+  unsigned int nb_nodes, size_node;
+  f.read((char*)&nb_nodes, sizeof(nb_nodes));
+  f.read((char*)&size_node, sizeof(size_node));
+  f.read((char*)&m_k, sizeof(m_k));
+  f.read((char*)&m_L, sizeof(m_L));
+  f.read((char*)&m_scoring, sizeof(m_scoring));
+  f.read((char*)&m_weighting, sizeof(m_weighting));
+  createScoringObject();
+
+  m_words.clear();
+  m_words.reserve(pow((double)m_k, (double)m_L + 1));
+  m_nodes.clear();
+  m_nodes.resize(nb_nodes+1);
+  m_nodes[0].id = 0;
+  char buf[size_node]; int nid = 1;
+  while (!f.eof()) {
+	f.read(buf, size_node);
+	m_nodes[nid].id = nid;
+	// FIXME
+	const int* ptr=(int*)buf;
+	m_nodes[nid].parent = *ptr;
+	//m_nodes[nid].parent = *(const int*)buf;
+	m_nodes[m_nodes[nid].parent].children.push_back(nid);
+	m_nodes[nid].descriptor = cv::Mat(1, F::L, CV_8U);
+	memcpy(m_nodes[nid].descriptor.data, buf+4, F::L);
+	m_nodes[nid].weight = *(float*)(buf+4+F::L);
+	if (buf[8+F::L]) { // is leaf
+	  int wid = m_words.size();
+	  m_words.resize(wid+1);
+	  m_nodes[nid].word_id = wid;
+	  m_words[wid] = &m_nodes[nid];
+	}
+	else
+	  m_nodes[nid].children.reserve(m_k);
+	nid+=1;
+  }
+  f.close();
+  return true;
+}
+
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::saveToBinaryFile(const std::string &filename) const {
+  fstream f;
+  f.open(filename.c_str(), ios_base::out|ios::binary);
+  unsigned int nb_nodes = m_nodes.size();
+  float _weight;
+  unsigned int size_node = sizeof(m_nodes[0].parent) + F::L*sizeof(char) + sizeof(_weight) + sizeof(bool);
+  f.write((char*)&nb_nodes, sizeof(nb_nodes));
+  f.write((char*)&size_node, sizeof(size_node));
+  f.write((char*)&m_k, sizeof(m_k));
+  f.write((char*)&m_L, sizeof(m_L));
+  f.write((char*)&m_scoring, sizeof(m_scoring));
+  f.write((char*)&m_weighting, sizeof(m_weighting));
+  for(size_t i=1; i<nb_nodes;i++) {
+	const Node& node = m_nodes[i];
+	f.write((char*)&node.parent, sizeof(node.parent));
+	f.write((char*)node.descriptor.data, F::L);
+	_weight = node.weight; f.write((char*)&_weight, sizeof(_weight));
+	bool is_leaf = node.isLeaf(); f.write((char*)&is_leaf, sizeof(is_leaf)); // i put this one at the end for alignement....
+  }
+  f.close();
+}
+
+// --------------------------------------------------------------------------
+
 template<class TDescriptor, class F>
 void TemplatedVocabulary<TDescriptor,F>::save(const std::string &filename) const
 {

build.sh

@@ -28,4 +28,8 @@ echo "Configuring and building ORB_SLAM2 ..."
 mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j
+make -j$(nproc)
+cd ..
+
+echo "Converting vocabulary to binary"
+./tools/bin_vocabulary

include/FrameDrawer.h

@@ -40,7 +40,7 @@ class Viewer;
 class FrameDrawer
 {
 public:
-    FrameDrawer(Map* pMap);
+    FrameDrawer(Map* pMap, bool bReuse);
 
     // Update info from the last processed frame.
     void Update(Tracking *pTracker);