diff --git a/CMakeLists.txt b/CMakeLists.txt index d832750163..3b742cce64 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt index 3c032fe8ab..edea78e97d 100644 --- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt +++ b/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 diff --git a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc index 2913525bfc..44ede17f1a 100644 --- a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc +++ b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc @@ -26,8 +26,12 @@ #include #include +#include "geometry_msgs/PoseStamped.h" +#include #include +#include "Converter.h" + #include"../../../include/System.h" @@ -40,15 +44,62 @@ 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 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(0,0),Rwc.at(0,1),Rwc.at(0,2), + Rwc.at(1,0),Rwc.at(1,1),Rwc.at(1,2), + Rwc.at(2,0),Rwc.at(2,1),Rwc.at(2,2)); + tf::Vector3 V(twc.at(0), twc.at(1), twc.at(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(0); + poseMSG.pose.position.y = twc.at(2); + poseMSG.pose.position.z = twc.at(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(); @@ -56,21 +107,38 @@ int main(int argc, char **argv) } // 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("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); } diff --git a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h index 01959344ed..7904b147ee 100644 --- a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h +++ b/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::saveToTextFile(const std::string &filen // -------------------------------------------------------------------------- +template +bool TemplatedVocabulary::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 +void TemplatedVocabulary::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 void TemplatedVocabulary::save(const std::string &filename) const { diff --git a/build.sh b/build.sh index 69d4ba228c..019877d489 100755 --- a/build.sh +++ b/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 diff --git a/include/FrameDrawer.h b/include/FrameDrawer.h index 95c1df9d9e..ad92242c71 100644 --- a/include/FrameDrawer.h +++ b/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); diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 67f4348273..be95547efd 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -20,7 +20,8 @@ #ifndef KEYFRAME_H #define KEYFRAME_H - +#include +using namespace std; #include "MapPoint.h" #include "Thirdparty/DBoW2/DBoW2/BowVector.h" #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" @@ -29,23 +30,36 @@ #include "Frame.h" #include "KeyFrameDatabase.h" +#include +#include +#include +#include +#include + +#include #include namespace ORB_SLAM2 { - +struct id_map +{ + bool is_valid; + long unsigned int id; +}; class Map; class MapPoint; class Frame; class KeyFrameDatabase; - +struct id_map; class KeyFrame { public: KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); - // Pose functions + KeyFrame(); /* Default constructor for serialization */ + + // Pose functions void SetPose(const cv::Mat &Tcw); cv::Mat GetPose(); cv::Mat GetPoseInverse(); @@ -116,6 +130,14 @@ class KeyFrame return pKF1->mnIdmnId; } + void SetMap(Map* map); + void SetKeyFrameDatabase(KeyFrameDatabase* pKeyFrameDB); + void SetORBvocabulary(ORBVocabulary* pORBvocabulary); + void SetMapPoints(std::vector spMapPoints); + void SetSpanningTree(std::vector vpKeyFrames); + void SetGridParams(std::vector vpKeyFrames); + + // The following variables are accesed from only 1 thread or never change (no mutex needed). public: @@ -201,8 +223,9 @@ class KeyFrame // MapPoints associated to keypoints std::vector mvpMapPoints; + std::map mmMapPoints_nId; - // BoW + // BoW KeyFrameDatabase* mpKeyFrameDB; ORBVocabulary* mpORBvocabulary; @@ -210,14 +233,20 @@ class KeyFrame std::vector< std::vector > > mGrid; std::map mConnectedKeyFrameWeights; + std::map mConnectedKeyFrameWeights_nId; std::vector mvpOrderedConnectedKeyFrames; + std::map mvpOrderedConnectedKeyFrames_nId; std::vector mvOrderedWeights; // Spanning Tree and Loop Edges bool mbFirstConnection; + KeyFrame* mpParent; + id_map mparent_KfId_map; std::set mspChildrens; + std::map mmChildrens_nId; std::set mspLoopEdges; + std::map mmLoopEdges_nId; // Bad flags bool mbNotErase; @@ -228,9 +257,27 @@ class KeyFrame Map* mpMap; + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + boost::serialization::split_member(ar, *this, version); + } + + template + void save(Archive & ar, const unsigned int version) const; + + + template + void load(Archive & ar, const unsigned int version); + + std::mutex mMutexPose; std::mutex mMutexConnections; std::mutex mMutexFeatures; + + + }; } //namespace ORB_SLAM diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h index fa3735762d..89238c241d 100644 --- a/include/KeyFrameDatabase.h +++ b/include/KeyFrameDatabase.h @@ -31,6 +31,12 @@ #include +#include +#include +#include +#include +#include +#include namespace ORB_SLAM2 { @@ -43,10 +49,13 @@ class KeyFrameDatabase { public: - KeyFrameDatabase(const ORBVocabulary &voc); + KeyFrameDatabase(ORBVocabulary &voc); + KeyFrameDatabase() {;} void add(KeyFrame* pKF); + void set_vocab(ORBVocabulary* pvoc); + void erase(KeyFrame* pKF); void clear(); @@ -60,13 +69,28 @@ class KeyFrameDatabase protected: // Associated vocabulary - const ORBVocabulary* mpVoc; + ORBVocabulary* mpVoc; // Inverted file std::vector > mvInvertedFile; // Mutex std::mutex mMutex; + + friend class boost::serialization::access; + + template + void serialize(Archive & ar, const unsigned int version) + { + boost::serialization::split_member(ar, *this, version); + } + + template + void save(Archive & ar, const unsigned int version) const; + + + template + void load(Archive & ar, const unsigned int version); }; } //namespace ORB_SLAM diff --git a/include/Map.h b/include/Map.h index 49ffab7374..1b2f30f40f 100644 --- a/include/Map.h +++ b/include/Map.h @@ -25,6 +25,13 @@ #include "KeyFrame.h" #include +#include +#include +#include +#include +#include +#include + #include @@ -59,10 +66,10 @@ class Map vector mvpKeyFrameOrigins; - std::mutex mMutexMapUpdate; + mutable std::mutex mMutexMapUpdate; // This avoid that two points are created simultaneously in separate threads (id conflict) - std::mutex mMutexPointCreation; + mutable std::mutex mMutexPointCreation; protected: std::set mspMapPoints; @@ -73,8 +80,28 @@ class Map long unsigned int mnMaxKFid; std::mutex mMutexMap; + + friend class boost::serialization::access; + + template + void serialize(Archive & ar, const unsigned int version) + { + boost::serialization::split_member(ar, *this, version); + } + + template + void save(Archive & ar, const unsigned int version) const; + + + template + void load(Archive & ar, const unsigned int version); + + }; -} //namespace ORB_SLAM + + + +} //namespace ORB_SLAM #endif // MAP_H diff --git a/include/MapPoint.h b/include/MapPoint.h index 11568a504a..a374b7d858 100644 --- a/include/MapPoint.h +++ b/include/MapPoint.h @@ -25,6 +25,17 @@ #include"Frame.h" #include"Map.h" +#include +#include +#include +#include +#include +#include +#include +#include + + + #include #include @@ -39,6 +50,7 @@ class Frame; class MapPoint { public: + MapPoint(); /* Default constructor for serialization */ MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap); MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF); @@ -79,6 +91,9 @@ class MapPoint float GetMinDistanceInvariance(); float GetMaxDistanceInvariance(); int PredictScale(const float ¤tDist, const float &logScaleFactor); + void SetMap(Map* map); + void SetObservations(std::vector); + public: long unsigned int mnId; @@ -110,7 +125,6 @@ class MapPoint static std::mutex mGlobalMutex; - protected: // Position in absolute coordinates @@ -118,6 +132,7 @@ class MapPoint // Keyframes observing the point and associated index in keyframe std::map mObservations; + std::map mObservations_nId; // Mean viewing direction cv::Mat mNormalVector; @@ -138,14 +153,104 @@ class MapPoint // Scale invariance distances float mfMinDistance; - float mfMaxDistance; + float mfMaxDistance; - Map* mpMap; + Map* mpMap; - std::mutex mMutexPos; + std::pair mref_KfId_pair; + + //id_map mref_KfId_map; + std::mutex mMutexPos; std::mutex mMutexFeatures; + + + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + boost::serialization::split_member(ar, *this, version); + } + + + + + template + void save(Archive & ar, const unsigned int version) const; + + + template + void load(Archive & ar, const unsigned int version); + }; } //namespace ORB_SLAM + + +#if 1 + +BOOST_SERIALIZATION_SPLIT_FREE(::cv::Mat) + +namespace boost { +namespace serialization { + +/*** CV KeyFrame ***/ +template +void serialize(Archive & ar, cv::KeyPoint & kf, const unsigned int version) +{ + ar & kf.angle; + ar & kf.class_id; + ar & kf.octave; + ar & kf.response; + ar & kf.response; + ar & kf.pt.x; + ar & kf.pt.y; + +} + + + +/*** Mat ***/ +/** Serialization support for cv::Mat */ +template +void save(Archive & ar, const ::cv::Mat& m, const unsigned int version) +{ + size_t elem_size = m.elemSize(); + size_t elem_type = m.type(); + + ar & m.cols; + ar & m.rows; + ar & elem_size; + ar & elem_type; + + const size_t data_size = m.cols * m.rows * elem_size; + + ar & boost::serialization::make_array(m.ptr(), data_size); +} + +/** Serialization support for cv::Mat */ +template +void load(Archive & ar, ::cv::Mat& m, const unsigned int version) +{ + int cols, rows; + size_t elem_size, elem_type; + + ar & cols; + ar & rows; + ar & elem_size; + ar & elem_type; + + m.create(rows, cols, elem_type); + size_t data_size = m.cols * m.rows * elem_size; + + ar & boost::serialization::make_array(m.ptr(), data_size); +} + + + +} +} +#endif + + #endif // MAPPOINT_H diff --git a/include/System.h b/include/System.h index dec008c437..4e9a938c33 100644 --- a/include/System.h +++ b/include/System.h @@ -26,6 +26,12 @@ #include #include +#include +#include +#include +#include +#include + #include "Tracking.h" #include "FrameDrawer.h" #include "MapDrawer.h" @@ -57,9 +63,11 @@ class System }; public: + // Enable serialization + friend class boost::serialization::access; // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. - System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true); + System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const bool reuse= false); // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. @@ -90,6 +98,14 @@ class System // This function must be called before saving the trajectory. void Shutdown(); + // Save / Load the current map for Mono Execution + void SaveMap(const string &filename); + void LoadMap(const string &filename); + + // Get map with tracked frames and points. + // Call first Shutdown() + //Map *GetMap(); + // Save camera trajectory in the TUM RGB-D dataset format. // Call first Shutdown() // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset diff --git a/include/Tracking.h b/include/Tracking.h index 5aaa93ef26..236cda5e10 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -25,6 +25,7 @@ #include #include + #include"Viewer.h" #include"FrameDrawer.h" #include"Map.h" @@ -55,7 +56,7 @@ class Tracking public: Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, - KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); + KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, const bool bReuse); // Preprocess the input and call Track(). Extract features and performs stereo matching. cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); @@ -212,8 +213,8 @@ class Tracking //Color order (true RGB, false BGR, ignored if grayscale) bool mbRGB; - list mlpTemporalPoints; + bool is_preloaded; }; } //namespace ORB_SLAM diff --git a/include/Viewer.h b/include/Viewer.h index 251e223c54..897ed1f42a 100644 --- a/include/Viewer.h +++ b/include/Viewer.h @@ -40,7 +40,7 @@ class System; class Viewer { public: - Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); + Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath, bool bReuse); // Main thread function. Draw points, keyframes, the current camera pose and the last processed // frame. Drawing is refreshed according to the camera fps. We use Pangolin. @@ -59,7 +59,7 @@ class Viewer private: bool Stop(); - + bool mbReuse; System* mpSystem; FrameDrawer* mpFrameDrawer; MapDrawer* mpMapDrawer; diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc index a325c31c2a..c713076615 100644 --- a/src/FrameDrawer.cc +++ b/src/FrameDrawer.cc @@ -29,9 +29,12 @@ namespace ORB_SLAM2 { -FrameDrawer::FrameDrawer(Map* pMap):mpMap(pMap) +FrameDrawer::FrameDrawer(Map* pMap, bool bReuse):mpMap(pMap) { mState=Tracking::SYSTEM_NOT_READY; + if (bReuse) + mState=Tracking::LOST; + mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0)); } diff --git a/src/Initializer.cc b/src/Initializer.cc index 6094b8f51b..ad058a6a09 100644 --- a/src/Initializer.cc +++ b/src/Initializer.cc @@ -113,10 +113,15 @@ bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatc // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45) if(RH>0.40) + { + //cout << __FUNCTION__ << " : Homography Mode Computing.." << endl; return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50); + } else //if(pF_HF>0.6) + { + //cout << __FUNCTION__ << "Fundamental Mode Computing.." << endl; return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50); - + } return false; } @@ -725,9 +730,15 @@ bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv: vP3D = bestP3D; vbTriangulated = bestTriangulated; + //cout << __FUNCTION__ << " Homography Mode Parameters satisfies as " << endl + //<< secondBestGood << " < " << 0.75*bestGood << "\n" << bestParallax << " >= " << minParallax << "\n" + //<< bestGood << " > " << minTriangulated << "\n" << bestGood << " > " << 0.9*N; return true; } - + + //cout << __FUNCTION__ << " Homography Mode Parameters should be " << endl + // << secondBestGood << " < " << 0.75*bestGood << "\n" << bestParallax << " >= " << minParallax << "\n" + // << bestGood << " > " << minTriangulated << "\n" << bestGood << " > " << 0.9*N; return false; } diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 4ef1e78e0f..cf0efe4da2 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -56,6 +56,728 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB): SetPose(F.mTcw); } +// Default serializing Constructor +KeyFrame::KeyFrame(): + mnFrameId(0), mTimeStamp(0.0), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), + mfGridElementWidthInv(0.0), mfGridElementHeightInv(0.0), + mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), + mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), + fx(0.0), fy(0.0), cx(0.0), cy(0.0), invfx(0.0), invfy(0.0), + mbf(0.0), mb(0.0), mThDepth(0.0), N(0), mnScaleLevels(0), mfScaleFactor(0), + mfLogScaleFactor(0.0), + mnMinX(0), mnMinY(0), mnMaxX(0), + mnMaxY(0) +{ +#if 0 + mnId=nNextId++; + + mGrid.resize(mnGridCols); + for(int i=0; i + void KeyFrame::save(Archive & ar, const unsigned int version) const + { + int nItems;bool is_id;bool has_parent = false; + long unsigned int t_nId; + //int ordered_weight; + ar & nNextId; + int ConKfWeight; + + //if (mbToBeErased) + //return; + //if (mbBad) + //return; + ar & const_cast (mnId); + ar & const_cast (mnFrameId); + ar & const_cast (mTimeStamp); + ar & const_cast (mnGridCols); + ar & const_cast (mnGridRows); + ar & const_cast (mfGridElementWidthInv); + ar & const_cast (mfGridElementHeightInv); + ar & const_cast (mnTrackReferenceForFrame); + ar & const_cast (mnFuseTargetForKF); + ar & const_cast (mnBALocalForKF); + ar & const_cast (mnBAFixedForKF); + ar & const_cast (mnLoopQuery); + ar & const_cast (mnLoopWords); + ar & const_cast (mLoopScore); + ar & const_cast (mnRelocQuery); + ar & const_cast (mnRelocWords); + ar & const_cast (mRelocScore); + ar & const_cast (mTcwGBA); + ar & const_cast (mTcwBefGBA); + ar & const_cast (mnBAGlobalForKF); + ar & const_cast (fx); + ar & const_cast (fy); + ar & const_cast (cx); + ar & const_cast (cy); + ar & const_cast (invfx); + ar & const_cast (invfy); + ar & const_cast (mbf); + ar & const_cast (mb); + ar & const_cast (mThDepth); + ar & const_cast (N); + ar & const_cast &> (mvKeys); + ar & const_cast &> (mvKeysUn); + ar & const_cast &> (mvuRight); + ar & const_cast &> (mvDepth); + ar & const_cast (mDescriptors); + ar & const_cast (mTcp); + ar & const_cast (mnScaleLevels); + ar & const_cast (mfScaleFactor); + ar & const_cast (mfLogScaleFactor); + ar & const_cast &> (mvScaleFactors); + ar & const_cast &> (mvLevelSigma2); + ar & const_cast &> (mvInvLevelSigma2); + + ar & const_cast (mnMinX); + ar & const_cast (mnMinY); + ar & const_cast (mnMaxX); + ar & const_cast (mnMaxY); + ar & const_cast (mK); + ar & const_cast (Tcw); + ar & const_cast (Twc); + ar & const_cast (Ow); + ar & const_cast (Cw); + // Save each map point id + nItems = mvpMapPoints.size(); + ar & nItems; + + //cout << "{INFO}mvpMapPoints nItems -" << nItems << endl; + for (std::vector::const_iterator it = mvpMapPoints.begin(); it != mvpMapPoints.end(); ++it) { + if (*it == NULL) + { + is_id = false; + ar & is_id; + continue; + } + else + { + is_id = true; + ar & is_id; + t_nId = (**it).mnId; + //cout << "[" << t_nId <<"]"; + + ar & t_nId; + + } + + + } + + // Grid + ar & const_cast > > &> (mGrid); + nItems = mConnectedKeyFrameWeights.size(); + ar & nItems; + + for (std::map::const_iterator it = mConnectedKeyFrameWeights.begin(); + it != mConnectedKeyFrameWeights.end(); + ++it) + { + if (it->first == NULL) + { + is_id = false; + ar & is_id; + continue; + } + else + { + is_id = true; + ar & is_id; + t_nId = it->first->mnId; + ar & t_nId; + ConKfWeight = it->second; + ar & ConKfWeight; + } + } + // Save each mvpOrderedConnectedKeyFrames + nItems = mvpOrderedConnectedKeyFrames.size(); + ar & nItems; + + for (std::vector::const_iterator it = mvpOrderedConnectedKeyFrames.begin(); + it != mvpOrderedConnectedKeyFrames.end(); + ++it) { + if (*it == NULL) + { + is_id = false; + ar & is_id; + continue; + } + else + { + is_id = true; + ar & is_id; + t_nId = (**it).mnId; + ar & t_nId; + } + } + // Save Each mvOrderedWeights +#if 0 + + nItems = mvOrderedWeights.size(); + + ar & nItems; + + for (std::vector::const_iterator it = mvOrderedWeights.begin(); + it != mvOrderedWeights.end(); + ++it) + { + ordered_weight = (*it); + ar & ordered_weight; + + } +#endif + ar & const_cast &>(mvOrderedWeights); + + // Spanning Tree + ar & const_cast (mbFirstConnection); + + if (mpParent) + { + has_parent = true; + ar & has_parent; + ar & mpParent->mnId; + } + else + { + has_parent = false; + ar & has_parent; + //ar & mpParent->mnId; + } + // Save each child Frame id + nItems = mspChildrens.size(); + ar & nItems; + for (std::set::const_iterator it = mspChildrens.begin(); it != mspChildrens.end(); ++it) { + if (*it == NULL) + { + is_id = false; + ar & is_id; + continue; + } + else + { + is_id = true; + ar & is_id; + t_nId = (**it).mnId; + //cout << "[" << t_nId <<"]"; + ar & t_nId; + } + + } + // Save each Loop Edge id + nItems = mspLoopEdges.size(); + ar & nItems; + for (std::set::const_iterator it = mspLoopEdges.begin(); it != mspLoopEdges.end(); ++it) { + if (*it == NULL) + { + is_id = false; + ar & is_id; + continue; + } + else + { + is_id = true; + ar & is_id; + t_nId = (**it).mnId; + //cout << "[" << t_nId <<"]"; + ar & t_nId; + } + + } + + ar & const_cast (mbNotErase); + ar & const_cast (mbToBeErased); + ar & const_cast (mbBad); + ar & const_cast (mHalfBaseline); + // cout << "{INFO}mvpMapPoints nItems fin"<< endl; + //cout << "Save Map : KF stat is : " << endl; + //t_nId = has_parent?mpParent->mnId:0; + //cout << "KF mnId = " << mnId << " Parent ID " < (mnFrameId); + ar & const_cast (mTimeStamp); + ar & const_cast (mnGridCols); + ar & const_cast (mnGridRows); + ar & const_cast (mfGridElementWidthInv); + ar & const_cast (mfGridElementHeightInv); + ar & const_cast (mnTrackReferenceForFrame); + ar & const_cast (mnFuseTargetForKF); + ar & const_cast (mnBALocalForKF); + ar & const_cast (mnBAFixedForKF); + ar & const_cast (mnLoopQuery); + ar & const_cast (mnLoopWords); + ar & const_cast (mLoopScore); + ar & const_cast (mnRelocQuery); + ar & const_cast (mnRelocWords); + ar & const_cast (mRelocScore); + ar & const_cast (mTcwGBA); + ar & const_cast (mTcwBefGBA); + ar & const_cast (mnBAGlobalForKF); + ar & const_cast (fx); + ar & const_cast (fy); + ar & const_cast (cx); + ar & const_cast (cy); + ar & const_cast (invfx); + ar & const_cast (invfy); + ar & const_cast (mbf); + ar & const_cast (mb); + ar & const_cast (mThDepth); + ar & const_cast (N); + ar & const_cast &> (mvKeys); + ar & const_cast &> (mvKeysUn); + ar & const_cast &> (mvuRight); + ar & const_cast &> (mvDepth); + ar & const_cast (mDescriptors); + ar & const_cast (mTcp); + ar & const_cast (mnScaleLevels); + ar & const_cast (mfScaleFactor); + ar & const_cast (mfLogScaleFactor); + ar & const_cast &> (mvScaleFactors); + ar & const_cast &> (mvLevelSigma2); + ar & const_cast &> (mvInvLevelSigma2); + + ar & const_cast (mnMinX); + ar & const_cast (mnMinY); + ar & const_cast (mnMaxX); + ar & const_cast (mnMaxY); + ar & const_cast (mK); + ar & const_cast (Tcw); + ar & const_cast (Twc); + ar & const_cast (Ow); + ar & const_cast (Cw); + // Load each map point id + ar & nItems; + mvpMapPoints.resize(nItems); + //mmMapPoints_nId.resize(nItems); + int j=0; + for (int i = 0; i < nItems; ++i) { + + ar & is_id; + if (is_id) + { + j++; + ar & t_nId; + storer.is_valid = true; + storer.id= t_nId; + mmMapPoints_nId[i] = storer; + } + else + { + storer.is_valid = false; + storer.id= 0; + mmMapPoints_nId[i] = storer; + } + } + + //cout << "KF " << mnId <<" valid points = " << j << "invalid points = " << (nItems - j) << endl; + // Grid + ar & const_cast > > &> (mGrid); + + ar & nItems; + //mConnectedKeyFrameWeights_nId.resize(nItems); + //mConnectedKeyFrameWeights.resize(nItems); + for (int i = 0; i < nItems; ++i) { + + ar & is_id; + if (is_id) + { + ar & t_nId; + ar & ConKfWeight; + mConnectedKeyFrameWeights_nId[t_nId] = ConKfWeight; + } + else + { + + } + } + + // Load each mvpOrderedConnectedKeyFrames id + ar & nItems; + j = 0; + //mvpOrderedConnectedKeyFrames.resize(nItems); + //mvpOrderedConnectedKeyFrames_nId.resize(nItems); + for (int i = 0; i < nItems; ++i) { + ar & is_id; + if (is_id) + { + j++; + ar & t_nId; + storer.is_valid = true; + storer.id= t_nId; + mvpOrderedConnectedKeyFrames_nId[i] = storer; + } + else + { + storer.is_valid = false; + storer.id= 0; + mvpOrderedConnectedKeyFrames_nId[i] = storer; + } + } + // Load each mvOrderedWeights + ar & const_cast &>(mvOrderedWeights); + + ar & const_cast (mbFirstConnection); + + // Spanning Tree + ar & has_parent; + if (has_parent) + { + mparent_KfId_map.is_valid = true; + ar & mparent_KfId_map.id; + } + else + { + mparent_KfId_map.is_valid = false; + mparent_KfId_map.id = 0; + } + // load each child Frame id + ar & nItems; + //mspChildrens.resize(nItems); + //mmChildrens_nId.resize(nItems); + + for (int i = 0; i < nItems; ++i) { + + ar & is_id; + if (is_id) + { + ar & t_nId; + storer.is_valid = true; + storer.id= t_nId; + mmChildrens_nId[i] = storer; + } + else + { + storer.is_valid = false; + storer.id= 0; + mmChildrens_nId[i] = storer; + } + } + + // Load each Loop Edge Frame id + ar & nItems; + //mspLoopEdges.resize(nItems); + //mmLoopEdges_nId.resize(nItems); + + for (int i = 0; i < nItems; ++i) { + + ar & is_id; + if (is_id) + { + ar & t_nId; + storer.is_valid = true; + storer.id= t_nId; + mmLoopEdges_nId[i] = storer; + } + else + { + storer.is_valid = false; + storer.id= 0; + mmLoopEdges_nId[i] = storer; + } + } + + ar & const_cast (mbNotErase); + ar & const_cast (mbToBeErased); + ar & const_cast (mbBad); + ar & const_cast (mHalfBaseline); + //cout << "Load Map : KF stat is : " << endl; + //cout << "KF mnId = " << mnId << " Parent ID " <::iterator mit=spMapPoints.begin(); mit !=spMapPoints.end(); mit++) + { + MapPoint* pMp = *mit; + + if(id == pMp->mnId) + { + ctr ++; + mvpMapPoints[j] = pMp; + mapp_found = true; + break; + } + } + if (mapp_found == false) + { + cout << " map point [" << id <<"] not found in KF " << mnId << endl; + mvpMapPoints[j] = static_cast(NULL); + } + + } + + } +} + +void KeyFrame::SetSpanningTree(std::vector vpKeyFrames) +{ + // We assume the mvpMapPoints_nId list has been initialized and contains the Map point IDS + // With nid, Search the KetFrame List and populate mvpMapPoints + long unsigned int id; + bool is_valid = false; + bool kf_found = false; + + int j = 0, ctr = 0; + // Search Parent + if (mparent_KfId_map.is_valid) + { + for(std::vector::iterator mit=vpKeyFrames.begin(); mit !=vpKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + id = pKf->mnId; + //if (mnId == 10 && 964 == id) + //cout << "[" << pMp->mnId <<"]"; + if(id == mparent_KfId_map.id) + { + ctr ++; + mpParent = pKf; + kf_found = true; + break; + } + } + if (kf_found == false) + { + cout << endl << "Parent KF [" << mparent_KfId_map.id <<"] not found for KF " << mnId << endl; + //mpParent = new KeyFrame(); + //mpParent->mbBad = true; + mpParent = static_cast(NULL); + } + } + // Search Child + kf_found = false; + j = 0; ctr = 0; + is_valid = false; + for (std::map::iterator it = mmChildrens_nId.begin(); + it != mmChildrens_nId.end(); + j++,++it) + { + is_valid = it->second.is_valid; + if (!is_valid) + { + //j--; + continue; + //mspChildrens.insert(NULL); + } + + else + { + id = it->second.id; + + kf_found = false; + for(std::vector::iterator mit=vpKeyFrames.begin(); mit !=vpKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + //if (mnId == 10 && 964 == id) + //cout << "[" << pMp->mnId <<"]"; + if(id == pKf->mnId) + { + ctr ++; + mspChildrens.insert(pKf); + kf_found = true; + break; + } + } + if (kf_found == false) + cout << endl << "Child [" << id <<"] not found for KF " << mnId << endl; + + } + + } + + // Search Loop Edges + kf_found = false; + j = 0; ctr = 0; + is_valid = false; + for (std::map::iterator it = mmLoopEdges_nId.begin(); + it != mmLoopEdges_nId.end(); + j++,++it) + { + is_valid = it->second.is_valid; + + + if (!is_valid) + { + //j--; + continue; + //mspLoopEdges.insert(NULL); + } + + else + { + id = it->second.id; + + kf_found = false; + for(std::vector::iterator mit=vpKeyFrames.begin(); mit !=vpKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + + if(id == pKf->mnId) + { + ctr ++; + mspLoopEdges.insert(pKf); + kf_found = true; + break; + } + } + if (kf_found == false) + cout << endl << "Loop Edge [" << id <<"] not found for KF " << mnId << endl; + + } + + } + +} +void KeyFrame::SetGridParams(std::vector vpKeyFrames) +{ + long unsigned int id; int weight; + bool Kf_found = false; + //cout << "KF" << mnId <<" valid indexes-" << endl; + int j = 0; + int ctr = 0; + bool is_valid = false; + + + // Set up mConnectedKeyFrameWeights + for (map::iterator it = mConnectedKeyFrameWeights_nId.begin(); + it != mConnectedKeyFrameWeights_nId.end(); + j++,++it) + { + id = it->first; + weight = it->second; + { + + for(std::vector::iterator mit=vpKeyFrames.begin(); mit !=vpKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + + if(id == pKf->mnId) + { + mConnectedKeyFrameWeights[pKf] = weight; + break; + } + } + + } + } + + // Set up mvpOrderedConnectedKeyFrames + j = 0; + for (std::map::iterator it = mvpOrderedConnectedKeyFrames_nId.begin(); + it != mvpOrderedConnectedKeyFrames_nId.end(); + ++it) + { + is_valid = it->second.is_valid; + if (!is_valid) + { + continue; + ;//mvpOrderedConnectedKeyFrames[j] = NULL; + } + else + { + id = it->second.id; + + Kf_found = false; + for(std::vector::iterator mit=vpKeyFrames.begin(); mit !=vpKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + + if(id == pKf->mnId) + { + ctr ++; + mvpOrderedConnectedKeyFrames.push_back(pKf); + Kf_found = true; + break; + } + } + if (Kf_found == false) + { + //cout << "[" << id <<"] not found in KF " << mnId << endl; + + } + + } + + } +} + +void KeyFrame::SetMap(Map* map) +{ + mpMap = map; +} + +void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase* pKeyFrameDB) +{ + mpKeyFrameDB = pKeyFrameDB; +} + +void KeyFrame::SetORBvocabulary(ORBVocabulary* pORBvocabulary) +{ + mpORBvocabulary = pORBvocabulary; +} + void KeyFrame::ComputeBoW() { if(mBowVec.empty() || mFeatVec.empty()) @@ -107,7 +829,6 @@ cv::Mat KeyFrame::GetStereoCenter() return Cw.clone(); } - cv::Mat KeyFrame::GetRotation() { unique_lock lock(mMutexPose); @@ -452,6 +1173,7 @@ void KeyFrame::SetErase() void KeyFrame::SetBadFlag() { + { unique_lock lock(mMutexConnections); if(mnId==0) @@ -478,7 +1200,8 @@ void KeyFrame::SetBadFlag() // Update Spanning Tree set sParentCandidates; - sParentCandidates.insert(mpParent); + if(mpParent) + sParentCandidates.insert(mpParent); // Assign at each iteration one children with a parent (the pair with highest covisibility weight) // Include that children as new parent candidate for the rest @@ -492,18 +1215,30 @@ void KeyFrame::SetBadFlag() for(set::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++) { + KeyFrame* pKF = *sit; + if (!pKF) + continue; if(pKF->isBad()) continue; + // Check if a parent candidate is connected to the keyframe vector vpConnected = pKF->GetVectorCovisibleKeyFrames(); + + for(size_t i=0, iend=vpConnected.size(); i::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++) { + + if(vpConnected[i]->mnId == (*spcit)->mnId) { + + int w = pKF->GetWeight(vpConnected[i]); if(w>max) { @@ -519,6 +1254,8 @@ void KeyFrame::SetBadFlag() if(bContinue) { + + pC->ChangeParent(pP); sParentCandidates.insert(pC); mspChildrens.erase(pC); @@ -531,11 +1268,16 @@ void KeyFrame::SetBadFlag() if(!mspChildrens.empty()) for(set::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++) { - (*sit)->ChangeParent(mpParent); + + if (mpParent) + (*sit)->ChangeParent(mpParent); } - - mpParent->EraseChild(this); - mTcp = Tcw*mpParent->GetPoseInverse(); + if (mpParent) + { + mpParent->EraseChild(this); + + mTcp = Tcw*mpParent->GetPoseInverse(); + } mbBad = true; } diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc index 826860cab9..cf24f17836 100644 --- a/src/KeyFrameDatabase.cc +++ b/src/KeyFrameDatabase.cc @@ -30,12 +30,87 @@ using namespace std; namespace ORB_SLAM2 { -KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc): +KeyFrameDatabase::KeyFrameDatabase (ORBVocabulary &voc): mpVoc(&voc) { mvInvertedFile.resize(voc.size()); } +void KeyFrameDatabase::set_vocab(ORBVocabulary *voc) +{ + mpVoc = voc; +} +template + void KeyFrameDatabase::save(Archive & ar, const unsigned int version) const + { + int nItems_a, nItems_b; + nItems_a = mvInvertedFile.size(); + ar & nItems_a; + cout << "{INFO}Database elmnts = %d " << nItems_a << endl; + + for (vector>::const_iterator it = mvInvertedFile.begin(); it != mvInvertedFile.end(); ++it) { + nItems_b = (*it).size(); + cout << "{INFO}kfs no elmnts = %d " << nItems_b << endl; + + ar & nItems_b; + for (list::const_iterator lit = (*it).begin(); lit != (*it).end(); ++lit) { + ar & ((**lit)); + } + } + #if 0 + std::for_each(mvInvertedFile.begin(), mvInvertedFile.end(), [&ar](list* plKeyFrame) { + + nItems_b = (*plKeyFrame).size(); + ar & nItems_b; + std::for_each((*plKeyFrame).begin(), (*plKeyFrame).end(), [&ar](KeyFrame* pKeyFrame) { + ar & *pKeyFrame; + }); + + + + }); + #endif + + } + + template + void KeyFrameDatabase::load(Archive & ar, const unsigned int version) + { + int nItems_a, nItems_b; + int j , i; + list temp_list; + ar & nItems_a; + cout << "{INFO}Database elmnts = %d " << nItems_a << endl; + + for (i = 0; i < nItems_a; ++i) { + + ar & nItems_b; + cout << "{INFO}kfs no elmnts = %d " << nItems_b << endl; + + for (j = 0; j < nItems_b; ++j) { + KeyFrame* pKeyFrame = new KeyFrame; + ar & *pKeyFrame; + temp_list.push_back(pKeyFrame); + } + mvInvertedFile.push_back(temp_list); + } + } + + +// Explicit template instantiation +template void KeyFrameDatabase::save( + boost::archive::binary_oarchive &, + const unsigned int) const; +template void KeyFrameDatabase::save( + boost::archive::binary_iarchive &, + const unsigned int) const; +template void KeyFrameDatabase::load( + boost::archive::binary_oarchive &, + const unsigned int); +template void KeyFrameDatabase::load( + boost::archive::binary_iarchive &, + const unsigned int); + void KeyFrameDatabase::add(KeyFrame *pKF) { @@ -203,16 +278,21 @@ vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) // Search all keyframes that share a word with current frame { unique_lock lock(mMutex); - + for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++) { + list &lKFs = mvInvertedFile[vit->first]; for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { + + KeyFrame* pKFi=*lit; if(pKFi->mnRelocQuery!=F->mnId) { + + pKFi->mnRelocWords=0; pKFi->mnRelocQuery=F->mnId; lKFsSharingWords.push_back(pKFi); diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 6879125fdd..4093693783 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -665,6 +665,7 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) { usleep(1000); } + // Get Map Mutex unique_lock lock(mpMap->mMutexMapUpdate); @@ -674,16 +675,34 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) while(!lpKFtoCheck.empty()) { + KeyFrame* pKF = lpKFtoCheck.front(); + if (!pKF) + continue; + const set sChilds = pKF->GetChilds(); + cv::Mat Twc = pKF->GetPoseInverse(); + + for(set::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) { + + KeyFrame* pChild = *sit; + if (!pChild) + continue; + if(pChild->mnBAGlobalForKF!=nLoopKF) { + + cv::Mat Tchildc = pChild->GetPose()*Twc; + + pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA; + + pChild->mnBAGlobalForKF=nLoopKF; } @@ -694,39 +713,63 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) pKF->SetPose(pKF->mTcwGBA); lpKFtoCheck.pop_front(); } + // Correct MapPoints const vector vpMPs = mpMap->GetAllMapPoints(); for(size_t i=0; iisBad()) continue; if(pMP->mnBAGlobalForKF==nLoopKF) { + // If optimized by Global BA, just update pMP->SetWorldPos(pMP->mPosGBA); } else { + + // Update according to the correction of its reference keyframe KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); - + if(!pRefKF) + continue; if(pRefKF->mnBAGlobalForKF!=nLoopKF) continue; + cout << "RunGlobalBundleAdjustment CP " << pRefKF->mnId + << " " << pRefKF->mTcwBefGBA.rows + << " " << pRefKF->mTcwBefGBA.cols << endl; + fflush(stdout); + + /* TODO : Stop-Gap for Loop Closure. Size coming as Zero! */ + if (!pRefKF->mTcwBefGBA.rows || !pRefKF->mTcwBefGBA.cols) + continue; + // Map to non-corrected camera cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3); + + cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3); + + cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw; + // Backproject using corrected camera cv::Mat Twc = pRefKF->GetPoseInverse(); cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3); cv::Mat twc = Twc.rowRange(0,3).col(3); + pMP->SetWorldPos(Rwc*Xc+twc); } diff --git a/src/Map.cc b/src/Map.cc index f285161644..c3008aa62b 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -19,9 +19,8 @@ */ #include "Map.h" - +#define TEST_DATA 0xdeadbeef #include - namespace ORB_SLAM2 { @@ -29,6 +28,106 @@ Map::Map():mnMaxKFid(0) { } +template + void Map::save(Archive & ar, const unsigned int version) const + { + unsigned int test_data = TEST_DATA; + int nItems = mspMapPoints.size(); + ar & nItems; + cout << "{INFO}mspMapPoints size = " << nItems << endl; + + std::for_each(mspMapPoints.begin(), mspMapPoints.end(), [&ar](MapPoint* pMapPoint) { + ar & *pMapPoint; + }); + + nItems = mspKeyFrames.size(); + cout << "{INFO}mspKeyFrames size = " << nItems << endl; + ar & nItems; + std::for_each(mspKeyFrames.begin(), mspKeyFrames.end(), [&ar](KeyFrame* pKeyFrame) { + ar & *pKeyFrame; + }); + + nItems = mvpKeyFrameOrigins.size(); + cout << "{INFO}mvpKeyFrameOrigins size = " << nItems << endl; + ar & nItems; + std::for_each(mvpKeyFrameOrigins.begin(), mvpKeyFrameOrigins.end(), [&ar](KeyFrame* pKeyFrameOrigin) { + ar & *pKeyFrameOrigin; + }); + // Pertaining to map drawing + //nItems = mvpReferenceMapPoints.size(); + //cout << "$${INFO}mvpReferenceMapPoints size = %d " << nItems << endl; + //ar & nItems; + //std::for_each(mvpReferenceMapPoints.begin(), mvpReferenceMapPoints.end(), [&ar](MapPoint* pMapPointReference) { + // ar & *pMapPointReference; + //}); + ar & const_cast (mnMaxKFid); + + ar & test_data; + } + + template + void Map::load(Archive & ar, const unsigned int version) + { + unsigned int test_data; + + int nItems; + ar & nItems; + cout << "{INFO}mspMapPoints size = " << nItems << endl; + + for (int i = 0; i < nItems; ++i) { + + MapPoint* pMapPoint = new MapPoint(); + ar & *pMapPoint; + mspMapPoints.insert(pMapPoint); + } + + ar & nItems; + cout << "{INFO}mspKeyFrames size = " << nItems << endl; + + for (int i = 0; i < nItems; ++i) { + + KeyFrame* pKeyFrame = new KeyFrame; + ar & *pKeyFrame; + mspKeyFrames.insert(pKeyFrame); + } + + + ar & nItems; + cout << "{INFO}mvpKeyFrameOrigins size = " << nItems << endl; + + for (int i = 0; i < nItems; ++i) { + + KeyFrame* pKeyFrame = new KeyFrame; + ar & *pKeyFrame; + /* TODO : VerifyHere*/ + mvpKeyFrameOrigins.push_back(*mspKeyFrames.begin()); + } + + ar & const_cast (mnMaxKFid); + + ar & test_data; + if (test_data == TEST_DATA) + cout <<">>Map Loading Validated as True" << endl; + else + cout <<"ERROR Map Loading Validated as False: Got -" << test_data << " :( Check Load Save sequence" << endl; + + } + + +// Explicit template instantiation +template void Map::save( + boost::archive::binary_oarchive &, + const unsigned int) const; +template void Map::save( + boost::archive::binary_iarchive &, + const unsigned int) const; +template void Map::load( + boost::archive::binary_oarchive &, + const unsigned int); +template void Map::load( + boost::archive::binary_iarchive &, + const unsigned int); + void Map::AddKeyFrame(KeyFrame *pKF) { unique_lock lock(mMutexMap); diff --git a/src/MapPoint.cc b/src/MapPoint.cc index ec41ddbc8b..e70f7d84a1 100644 --- a/src/MapPoint.cc +++ b/src/MapPoint.cc @@ -28,7 +28,17 @@ namespace ORB_SLAM2 long unsigned int MapPoint::nNextId=0; mutex MapPoint::mGlobalMutex; - +MapPoint::MapPoint(): + nObs(0), mnTrackReferenceForFrame(0), + mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0),mnVisible(1), mnFound(1), mbBad(false), + mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0) + { + //mNormalVector = cv::Mat::zeros(3,1,CV_32F); + //unique_lock lock(mpMap->mMutexPointCreation); + //mpMap = new Map(); + // mpRefKF = new KeyFrame(); + } MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap): mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), @@ -38,11 +48,240 @@ MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap): Pos.copyTo(mWorldPos); mNormalVector = cv::Mat::zeros(3,1,CV_32F); - // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. + // MapPoints can be created from Tracking and Local Mapping. This recursive_mutex avoid conflicts with id. unique_lock lock(mpMap->mMutexPointCreation); mnId=nNextId++; } +template + void MapPoint::save(Archive & ar, const unsigned int version) const + { + unsigned int nItems;bool is_id = false, is_valid = false;; + size_t t_size; + long unsigned int t_nId; + + if (mbBad) + return; + + ar & const_cast (mnId ); + //cout << "[" << mnId << "]" ; + ar & nNextId; + ar & const_cast (mnFirstKFid); + ar & const_cast (mnFirstFrame); + ar & const_cast (nObs); + ar & const_cast (mTrackProjX); + ar & const_cast (mTrackProjY); + ar & const_cast (mTrackProjXR); + ar & const_cast (mbTrackInView); + ar & const_cast (mnTrackScaleLevel); + ar & const_cast (mTrackViewCos); + ar & const_cast (mnTrackReferenceForFrame); + ar & const_cast (mnLastFrameSeen); + ar & const_cast (mnBALocalForKF); + ar & const_cast (mnFuseCandidateForKF); + ar & const_cast (mnLoopPointForKF); + ar & const_cast (mnCorrectedByKF); + ar & const_cast (mnCorrectedReference); + + ar & const_cast (mPosGBA); + ar & const_cast (mnBAGlobalForKF); + ar & const_cast (mWorldPos); + + // Save each KF point id + nItems = mObservations.size(); + ar & nItems; + //cout << "{INFO}mvpMapPoints nItems -" << nItems << endl; + + for (std::map::const_iterator it = mObservations.begin(); it != mObservations.end(); ++it) { + if (it->first == NULL) + { + cout << "{INFO}Map POint Save - Empty observation " << mnId << endl; + + is_id = false; + ar & is_id; + continue; + } + else + { + is_id = true; + ar & is_id; + t_nId = it->first->mnId; + ar & t_nId; + t_size = it->second; + ar & t_size; + } + + + } + + + ar & const_cast (mNormalVector); + ar & const_cast (mDescriptor); + if (mpRefKF) { + is_valid = true; + ar & is_valid; + ar & mpRefKF->mnId; + } + else + { + is_valid = false; + ar & is_valid; + } + + + ar & const_cast (mnVisible); + ar & const_cast (mnFound); + ar & const_cast (mbBad); + ar & const_cast (mfMinDistance); + ar & const_cast (mfMaxDistance); + + } + + template + void MapPoint::load(Archive & ar, const unsigned int version) + { + unsigned int nItems;bool is_id = false, is_valid = false; + size_t t_size; + + long unsigned int t_nId; + + ar & const_cast (mnId ); + //cout << "[" << mnId << "]" ; + ar & nNextId; + ar & const_cast (mnFirstKFid); + ar & const_cast (mnFirstFrame); + ar & const_cast (nObs); + ar & const_cast (mTrackProjX); + ar & const_cast (mTrackProjY); + ar & const_cast (mTrackProjXR); + ar & const_cast (mbTrackInView); + ar & const_cast (mnTrackScaleLevel); + ar & const_cast (mTrackViewCos); + ar & const_cast (mnTrackReferenceForFrame); + ar & const_cast (mnLastFrameSeen); + ar & const_cast (mnBALocalForKF); + ar & const_cast (mnFuseCandidateForKF); + ar & const_cast (mnLoopPointForKF); + ar & const_cast (mnCorrectedByKF); + ar & const_cast (mnCorrectedReference); + + ar & const_cast (mPosGBA); + ar & const_cast (mnBAGlobalForKF); + ar & const_cast (mWorldPos); + + // Load each map point id + ar & nItems; + //mObservations.resize(nItems); + //mObservations_nId.resize(nItems); + for (int i = 0; i < nItems; ++i) { + + ar & is_id; + if (is_id) + { + ar & t_nId; + ar & t_size; + mObservations_nId[t_nId] = t_size; + } + else + { + + } + } + + ar & const_cast (mNormalVector); + ar & const_cast (mDescriptor); + + //mpRefKF = new KeyFrame(); + //Reference Keyframe + ar & is_valid; + if (is_valid) + { + ar & t_nId; + } + else + t_nId = 0; + + mref_KfId_pair = std::make_pair(t_nId,is_valid); + + ar & const_cast (mnVisible); + ar & const_cast (mnFound); + ar & const_cast (mbBad); + ar & const_cast (mfMinDistance); + ar & const_cast (mfMaxDistance); + } + + +// Explicit template instantiation +template void MapPoint::save( + boost::archive::binary_oarchive &, + const unsigned int) const; +template void MapPoint::save( + boost::archive::binary_iarchive &, + const unsigned int) const; +template void MapPoint::load( + boost::archive::binary_oarchive &, + const unsigned int); +template void MapPoint::load( + boost::archive::binary_iarchive &, + const unsigned int); + +void MapPoint::SetMap(Map* map) +{ + mpMap = map; +} + +void MapPoint::SetObservations(std::vector spKeyFrames) +{ + + long unsigned int id, kfRef_id; size_t size; + //cout << "KF" << mnId <<" valid indexes-" << endl; + int j = 0; + bool found_reference = false; + kfRef_id = mref_KfId_pair.first; + bool is_ref_valid = mref_KfId_pair.second; + + + for (map::iterator it = mObservations_nId.begin(); it != mObservations_nId.end(); j++,++it) { + id = it->first; + size = it->second; + { + for(std::vector::iterator mit=spKeyFrames.begin(); mit !=spKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + //cout << "[" << pKf->mnId << "]"; + if(id == pKf->mnId) + { + //cout << "[" << id <<"]"; + mObservations[pKf] = size; + //id = -1; + break; + } + } + + } + + } + + for(std::vector::iterator mit=spKeyFrames.begin(); mit !=spKeyFrames.end(); mit++) + { + KeyFrame* pKf = *mit; + if (is_ref_valid && kfRef_id == pKf->mnId ) + { + // Set the refernce Keyframe + mpRefKF = pKf; + found_reference = true; + } + } + + if (!found_reference) + { + mpRefKF = static_cast(NULL); + cout << "refernce KF - " << kfRef_id << "is not found for mappoint " << mnId << endl; + // Dummy KF + //mpRefKF = new KeyFrame(); + } +} + MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF): mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0), @@ -65,7 +304,7 @@ MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF pFrame->mDescriptors.row(idxF).copyTo(mDescriptor); - // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. + // MapPoints can be created from Tracking and Local Mapping. This recursive_mutex avoid conflicts with id. unique_lock lock(mpMap->mMutexPointCreation); mnId=nNextId++; } @@ -356,12 +595,19 @@ void MapPoint::UpdateNormalAndDepth() n++; } + if (!pRefKF) + { + + return; + } + cv::Mat PC = Pos - pRefKF->GetCameraCenter(); const float dist = cv::norm(PC); const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave; const float levelScaleFactor = pRefKF->mvScaleFactors[level]; const int nLevels = pRefKF->mnScaleLevels; + { unique_lock lock3(mMutexPos); mfMaxDistance = dist*levelScaleFactor; diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc index 14be2efae5..c219865260 100644 --- a/src/ORBextractor.cc +++ b/src/ORBextractor.cc @@ -59,7 +59,6 @@ #include #include #include - #include "ORBextractor.h" diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index be31e8487e..582fcd9297 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -160,12 +160,15 @@ int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPoin { const vector vpMapPointsKF = pKF->GetMapPointMatches(); + //cout << "SearchByBoW: ref KF points size = " << vpMapPointsKF.size(); vpMapPointMatches = vector(F.N,static_cast(NULL)); const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec; int nmatches=0; + //cout << "SearchByBoW: CP1 nmatches = " << nmatches << endl; + vector rotHist[HISTO_LENGTH]; for(int i=0;iisBad()) continue; g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); @@ -919,6 +921,8 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p e->setMeasurement(Sji); e->information() = matLambda; + //cout << "OptimizeEssentialGraph. nIDj = "<< nIDj << "nIDi =" << nIDi << endl; + optimizer.addEdge(e); } @@ -944,6 +948,8 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); el->setMeasurement(Sli); el->information() = matLambda; + //cout << "OptimizeEssentialGraph. CP Loop" << endl; + optimizer.addEdge(el); } } @@ -953,10 +959,14 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p for(vector::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++) { KeyFrame* pKFn = *vit; - if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) + //cout << "OptimizeEssentialGraph. Covisibility" << endl; + + if(pParentKF && pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) { + if(!pKFn->isBad() && pKFn->mnIdmnId) - { + { + if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId)))) continue; @@ -968,14 +978,19 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p Snw = itn->second; else Snw = vScw[pKFn->mnId]; + g2o::Sim3 Sni = Snw * Swi; g2o::EdgeSim3* en = new g2o::EdgeSim3(); + + en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); en->setMeasurement(Sni); en->information() = matLambda; + + optimizer.addEdge(en); } } @@ -1025,7 +1040,10 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p else { KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); - nIDr = pRefKF->mnId; + if(pRefKF) + nIDr = pRefKF->mnId; + else + continue; } diff --git a/src/PnPsolver.cc b/src/PnPsolver.cc index 42a2065f4f..3952a367ad 100644 --- a/src/PnPsolver.cc +++ b/src/PnPsolver.cc @@ -206,6 +206,8 @@ cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector &vbInlie // Check inliers CheckInliers(); + //cout << "iterate: CP mnInliersi = " << mnInliersi << " mRansacMinInliers = "<< mRansacMinInliers << endl; + if(mnInliersi>=mRansacMinInliers) { // If it is the best solution so far, save it @@ -328,6 +330,7 @@ void PnPsolver::CheckInliers() if(error2 #include +bool has_suffix(const std::string &str, const std::string &suffix) { + std::size_t index = str.find(suffix, str.size() - suffix.size()); + return (index != std::string::npos); +} + namespace ORB_SLAM2 { System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, - const bool bUseViewer):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(false), + const bool bUseViewer, const bool bReuse):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(bReuse), mbDeactivateLocalizationMode(false) { // Output welcome message @@ -59,10 +64,16 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS //Load ORB Vocabulary - cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; - mpVocabulary = new ORBVocabulary(); - bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; + bool bVocLoad = false; // chose loading method based on file extension + if (has_suffix(strVocFile, ".txt")) + bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + else + bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile); + + + //bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); if(!bVocLoad) { cerr << "Wrong path to vocabulary. " << endl; @@ -70,21 +81,60 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS exit(-1); } cout << "Vocabulary loaded!" << endl << endl; - //Create KeyFrame Database mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); //Create the Map - mpMap = new Map(); + if (!bReuse) + { + mpMap = new Map(); + } + + if (bReuse) + { + LoadMap("Slam_Map.bin"); + + //mpKeyFrameDatabase->set_vocab(mpVocabulary); + + vector vpKFs = mpMap->GetAllKeyFrames(); + for (vector::iterator it = vpKFs.begin(); it != vpKFs.end(); ++it) { + (*it)->SetKeyFrameDatabase(mpKeyFrameDatabase); + (*it)->SetORBvocabulary(mpVocabulary); + (*it)->SetMap(mpMap); + (*it)->ComputeBoW(); + mpKeyFrameDatabase->add(*it); + (*it)->SetMapPoints(mpMap->GetAllMapPoints()); + (*it)->SetSpanningTree(vpKFs); + (*it)->SetGridParams(vpKFs); + + // Reconstruct map points Observation + + } + + vector vpMPs = mpMap->GetAllMapPoints(); + for (vector::iterator mit = vpMPs.begin(); mit != vpMPs.end(); ++mit) { + (*mit)->SetMap(mpMap); + (*mit)->SetObservations(vpKFs); + + } + + for (vector::iterator it = vpKFs.begin(); it != vpKFs.end(); ++it) { + (*it)->UpdateConnections(); + } + + + } + cout << endl << mpMap <<" : is the created map address" << endl; + //Create Drawers. These are used by the Viewer - mpFrameDrawer = new FrameDrawer(mpMap); + mpFrameDrawer = new FrameDrawer(mpMap, bReuse); mpMapDrawer = new MapDrawer(mpMap, strSettingsFile); //Initialize the Tracking thread //(it will live in the main thread of execution, the one that called this constructor) mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, - mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor); + mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor, bReuse); //Initialize the Local Mapping thread and launch mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); @@ -95,7 +145,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser); //Initialize the Viewer thread and launch - mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile); + mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile, bReuse); if(bUseViewer) mptViewer = new thread(&Viewer::Run, mpViewer); @@ -204,6 +254,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) { + cv::Mat Tcw; if(mSensor!=MONOCULAR) { cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl; @@ -244,7 +295,9 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) } } - return mpTracker->GrabImageMonocular(im,timestamp); + //return (mpTracker->GrabImageMonocular(im,timestamp)).clone(); + return (mpTracker->GrabImageMonocular(im,timestamp)); + } void System::ActivateLocalizationMode() @@ -281,6 +334,36 @@ void System::Shutdown() pangolin::BindToContext("ORB-SLAM2: Map Viewer"); } +void System::LoadMap(const string &filename) +{ + { + std::ifstream is(filename); + + + boost::archive::binary_iarchive ia(is, boost::archive::no_header); + //ia >> mpKeyFrameDatabase; + ia >> mpMap; + + } + + cout << endl << filename <<" : Map Loaded!" << endl; + + +} + +void System::SaveMap(const string &filename) +{ + std::ofstream os(filename); + { + ::boost::archive::binary_oarchive oa(os, ::boost::archive::no_header); + //oa << mpKeyFrameDatabase; + oa << mpMap; + } + cout << endl << "Map saved to " << filename << endl; + +} + + void System::SaveTrajectoryTUM(const string &filename) { cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; diff --git a/src/Tracking.cc b/src/Tracking.cc index be6e12b935..41104492aa 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -43,8 +43,8 @@ using namespace std; namespace ORB_SLAM2 { -Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor): - mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), +Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, const bool bReuse): + mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(bReuse), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) { @@ -82,6 +82,11 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, if(fps==0) fps=30; + is_preloaded = bReuse; + if (is_preloaded) + { + mState = LOST; + } // Max/Min Frames to insert keyframes and to check relocalisation mMinFrames = 0; mMaxFrames = fps; @@ -130,7 +135,7 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, cout << "- Scale Factor: " << fScaleFactor << endl; cout << "- Initial Fast Threshold: " << fIniThFAST << endl; cout << "- Minimum Fast Threshold: " << fMinThFAST << endl; - + cout << "- Reuse Map ?: " << is_preloaded << endl; if(sensor==System::STEREO || sensor==System::RGBD) { mThDepth = mbf*(float)fSettings["ThDepth"]/fx; @@ -261,7 +266,34 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) Track(); - return mCurrentFrame.mTcw.clone(); + /* Do Pose calculation */ + if(mState==OK && !mCurrentFrame.mTcw.empty() && mCurrentFrame.mpReferenceKF) + { + + vector vpKFs = mpMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + + cv::Mat Two = vpKFs[0]->GetPoseInverse(); + + ORB_SLAM2::KeyFrame* pKF = mpReferenceKF; + + cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); + while(pKF->isBad()) + { + // cout << "bad parent" << endl; + Trw = Trw*pKF->mTcp; + pKF = pKF->GetParent(); + } + Trw = Trw*pKF->GetPose()*Two; + cv::Mat Tcr = mlRelativeFramePoses.back(); + cv::Mat Tcw = Tcr*Trw; + return Tcw.clone(); + } + else + return mCurrentFrame.mTcw.clone(); } void Tracking::Track() @@ -278,6 +310,11 @@ void Tracking::Track() if(mState==NOT_INITIALIZED) { + if (is_preloaded) + { + mState = LOST; + return; + } if(mSensor==System::STEREO || mSensor==System::RGBD) StereoInitialization(); else @@ -486,7 +523,7 @@ void Tracking::Track() } // Store frame pose information to retrieve the complete camera trajectory afterwards. - if(!mCurrentFrame.mTcw.empty()) + if(!mCurrentFrame.mTcw.empty() && mCurrentFrame.mpReferenceKF) { cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse(); mlRelativeFramePoses.push_back(Tcr); @@ -502,8 +539,13 @@ void Tracking::Track() mlFrameTimes.push_back(mlFrameTimes.back()); mlbLost.push_back(mState==LOST); } - } +#if 0 +cv::Mat Tracking::getTransformData() +{ + return mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse(); +} +#endif void Tracking::StereoInitialization() @@ -583,12 +625,16 @@ void Tracking::MonocularInitialization() return; } + else + cout << __FUNCTION__ << "The Key Frame-s points are less: " << mCurrentFrame.mvKeys.size() << endl; } else { // Try to initialize if((int)mCurrentFrame.mvKeys.size()<=100) { + + //cout << __FUNCTION__ << "old.The Key Frame-s points are less: " << mCurrentFrame.mvKeys.size() << endl; delete mpInitializer; mpInitializer = static_cast(NULL); fill(mvIniMatches.begin(),mvIniMatches.end(),-1); @@ -602,6 +648,7 @@ void Tracking::MonocularInitialization() // Check if there are enough correspondences if(nmatches<100) { + cout << __FUNCTION__ << "ORB extraction : No enough correspondesnces(<100) " << nmatches << endl; delete mpInitializer; mpInitializer = static_cast(NULL); return; @@ -636,6 +683,8 @@ void Tracking::MonocularInitialization() void Tracking::CreateInitialMapMonocular() { + + cout << __FUNCTION__ << ": Starting Initial Map Creation..." << endl; // Create KeyFrames KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB); KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); @@ -691,7 +740,9 @@ void Tracking::CreateInitialMapMonocular() if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100) { - cout << "Wrong initialization, reseting..." << endl; + cout << "Wrong initialization, reseting... map points(100):" + << pKFcur->TrackedMapPoints(1) + << " medianDepth = " << medianDepth << endl; Reset(); return; } @@ -1355,6 +1406,7 @@ bool Tracking::Relocalization() { // Compute Bag of Words Vector mCurrentFrame.ComputeBoW(); + //cout << "Relocalization Initiated" << endl; // Relocalization is performed when tracking is lost // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation @@ -1363,8 +1415,11 @@ bool Tracking::Relocalization() if(vpCandidateKFs.empty()) return false; + const int nKFs = vpCandidateKFs.size(); + //cout << "Relocalization: candidates = " << nKFs << endl; + // We perform first an ORB matching with each candidate // If enough matches are found we setup a PnP solver ORBmatcher matcher(0.75,true); @@ -1380,6 +1435,8 @@ bool Tracking::Relocalization() int nCandidates=0; + + for(int i=0; iSetRansacParameters(0.99,10,300,4,0.5,5.991); vpPnPsolvers[i] = pSolver; @@ -1403,6 +1461,7 @@ bool Tracking::Relocalization() } } + // Alternatively perform some iterations of P4P RANSAC // Until we found a camera pose supported by enough inliers bool bMatch = false; @@ -1410,6 +1469,7 @@ bool Tracking::Relocalization() while(nCandidates>0 && !bMatch) { + for(int i=0; i menuShowPoints("menu.Show Points",true,true); pangolin::Var menuShowKeyFrames("menu.Show KeyFrames",true,true); pangolin::Var menuShowGraph("menu.Show Graph",true,true); - pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); + pangolin::Var menuLocalizationMode("menu.Localization Mode",mbReuse,true); pangolin::Var menuReset("menu.Reset",false,false); // Define Camera Render Object (for view / scene browsing) @@ -89,7 +90,7 @@ void Viewer::Run() cv::namedWindow("ORB-SLAM2: Current Frame"); bool bFollow = true; - bool bLocalizationMode = false; + bool bLocalizationMode = mbReuse; while(1) { diff --git a/tools/bin_vocabulary.cc b/tools/bin_vocabulary.cc new file mode 100644 index 0000000000..fe8cf2c054 --- /dev/null +++ b/tools/bin_vocabulary.cc @@ -0,0 +1,52 @@ +#include + +#include "ORBVocabulary.h" +using namespace std; + +bool load_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) { + clock_t tStart = clock(); + bool res = voc->loadFromTextFile(infile); + printf("Loading fom text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); + return res; +} + +void load_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) { + clock_t tStart = clock(); + voc->load(infile); + printf("Loading fom xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); +} + +void load_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) { + clock_t tStart = clock(); + voc->loadFromBinaryFile(infile); + printf("Loading fom binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); +} + +void save_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) { + clock_t tStart = clock(); + voc->save(outfile); + printf("Saving as xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); +} + +void save_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) { + clock_t tStart = clock(); + voc->saveToTextFile(outfile); + printf("Saving as text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); +} + +void save_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) { + clock_t tStart = clock(); + voc->saveToBinaryFile(outfile); + printf("Saving as binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC); +} + + +int main(int argc, char **argv) { + cout << "BoW load/save benchmark" << endl; + ORB_SLAM2::ORBVocabulary* voc = new ORB_SLAM2::ORBVocabulary(); + + load_as_text(voc, "Vocabulary/ORBvoc.txt"); + save_as_binary(voc, "Vocabulary/ORBvoc.bin"); + + return 0; +}