From bc752637f83d02e57e8accfca9d52957d72ce1a8 Mon Sep 17 00:00:00 2001 From: Yifei Kang Date: Fri, 22 Jan 2021 14:35:03 +0800 Subject: [PATCH] save and load with a mini map --- CMakeLists.txt | 2 + Thirdparty/DBoW2/DBoW2/BowVector.h | 1 + Thirdparty/DBoW2/DBoW2/FeatureVector.h | 1 + include/Frame.h | 6 ++ include/KeyFrame.h | 8 +- include/MapPoint.h | 18 +++- include/Serialize.h | 85 ++++++++++++++++ include/System.h | 10 ++ include/Tracking.h | 1 + src/Frame.cc | 63 ++++++++++++ src/KeyFrame.cc | 46 +++++++++ src/MapPoint.cc | 25 +++++ src/System.cc | 135 +++++++++++++++++++++++++ src/Tracking.cc | 11 +- src/Viewer.cc | 5 + 15 files changed, 411 insertions(+), 6 deletions(-) create mode 100644 include/Serialize.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 08a8af468e..4d0e01a97e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ endif() find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) +find_library(BOOST_SERIALIZATION boost_serialization) include_directories( ${PROJECT_SOURCE_DIR} @@ -74,6 +75,7 @@ target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} +${BOOST_SERIALIZATION} ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so ) diff --git a/Thirdparty/DBoW2/DBoW2/BowVector.h b/Thirdparty/DBoW2/DBoW2/BowVector.h index f559811deb..196658b7a6 100644 --- a/Thirdparty/DBoW2/DBoW2/BowVector.h +++ b/Thirdparty/DBoW2/DBoW2/BowVector.h @@ -58,6 +58,7 @@ class BowVector: { public: + typedef std::map super; /** * Constructor */ diff --git a/Thirdparty/DBoW2/DBoW2/FeatureVector.h b/Thirdparty/DBoW2/DBoW2/FeatureVector.h index 08a91def7c..d24f8fe3cf 100644 --- a/Thirdparty/DBoW2/DBoW2/FeatureVector.h +++ b/Thirdparty/DBoW2/DBoW2/FeatureVector.h @@ -23,6 +23,7 @@ class FeatureVector: { public: + typedef std::map > super; /** * Constructor */ diff --git a/include/Frame.h b/include/Frame.h index a6a8032f57..98c9c07422 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -57,6 +57,12 @@ class Frame // Constructor for Monocular cameras. Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth); + // for Load Map + Frame(const cv::Size& imgSize, const cv::Mat& Tcw, const std::vector& keypoint, + const std::vector& keypoint_un, const cv::Mat& descriptor, const double timeStamp, + ORBextractor* const extractor, ORBVocabulary* const voc, const cv::Mat& K, const float bf, + const float thDepth, const unsigned long frame_id); + // Extract ORB on the image. 0 for left image and 1 for right image. void ExtractORB(int flag, const cv::Mat &im); diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 67f4348273..926d2b06f6 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -28,6 +28,7 @@ #include "ORBextractor.h" #include "Frame.h" #include "KeyFrameDatabase.h" +#include "Serialize.h" #include @@ -44,6 +45,8 @@ class KeyFrame { public: KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); + // for serizalize + KeyFrame(); // Pose functions void SetPose(const cv::Mat &Tcw); @@ -116,7 +119,10 @@ class KeyFrame return pKF1->mnIdmnId; } - +private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); // The following variables are accesed from only 1 thread or never change (no mutex needed). public: diff --git a/include/MapPoint.h b/include/MapPoint.h index f26893d89d..8be55b404e 100644 --- a/include/MapPoint.h +++ b/include/MapPoint.h @@ -24,6 +24,7 @@ #include"KeyFrame.h" #include"Frame.h" #include"Map.h" +#include "Serialize.h" #include #include @@ -41,6 +42,8 @@ class MapPoint public: MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap); MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF); + // for serizalize + MapPoint(); void SetWorldPos(const cv::Mat &Pos); cv::Mat GetWorldPos(); @@ -81,6 +84,19 @@ class MapPoint int PredictScale(const float ¤tDist, KeyFrame*pKF); int PredictScale(const float ¤tDist, Frame* pF); + void SetRefKF(ORB_SLAM2::KeyFrame *pKF){ + mpRefKF = pKF; + }; + + void SetMap(ORB_SLAM2::Map *pMap){ + mpMap = pMap; + }; + +private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + public: long unsigned int mnId; static long unsigned int nNextId; @@ -109,6 +125,7 @@ class MapPoint cv::Mat mPosGBA; long unsigned int mnBAGlobalForKF; + KeyFrame* mpRefKF = nullptr; static std::mutex mGlobalMutex; @@ -127,7 +144,6 @@ class MapPoint cv::Mat mDescriptor; // Reference KeyFrame - KeyFrame* mpRefKF; // Tracking counters int mnVisible; diff --git a/include/Serialize.h b/include/Serialize.h new file mode 100644 index 0000000000..f6e49cfdae --- /dev/null +++ b/include/Serialize.h @@ -0,0 +1,85 @@ +/* +This file contains the necessary serialize functions for complex datatypes +used by ORBSLAM2 and which are not available in boost serialization library +*/ + +#ifndef SERIALIZE_H +#define SERIALIZE_H +#include +#include +#include +#include +// KeyFrame::mConnectedKeyFrameWeights +#include +// #include +// DBoW2::BowVector and DBoW2::FeatureVector +#include +// KeyFrame::mspChildrens +#include +#include + +#include "Thirdparty/DBoW2/DBoW2/BowVector.h" +#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" + +BOOST_SERIALIZATION_SPLIT_FREE(::cv::Mat) +namespace boost { +namespace serialization { + +template +void serialize(Archive &ar, DBoW2::BowVector &BowVec, + const unsigned int file_version) { + // Derived classes should include serializations of their base classes. + ar &boost::serialization::base_object(BowVec); +} + +template +void serialize(Archive &ar, DBoW2::FeatureVector &FeatVec, + const unsigned int file_version) { + ar &boost::serialization::base_object(FeatVec); +} + +template +void save(Archive &ar, const ::cv::Mat &m, const unsigned int file_version) { + cv::Mat m_ = m; + if (!m.isContinuous()) m_ = m.clone(); + 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); +} + +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); +} + +template +void serialize(Archive &ar, ::cv::KeyPoint &kf, + const unsigned int file_version) { + ar &kf.angle; + ar &kf.class_id; + ar &kf.octave; + ar &kf.response; + ar &kf.pt.x; + ar &kf.pt.y; +} +} +} +#endif // SERIALIZE_H \ No newline at end of file diff --git a/include/System.h b/include/System.h index b377b453d1..a2594e1a6e 100644 --- a/include/System.h +++ b/include/System.h @@ -122,6 +122,16 @@ class System std::vector GetTrackedMapPoints(); std::vector GetTrackedKeyPointsUn(); + bool SaveMap(const string& file_name); + bool LoadMap(const string& file_name); + +private: + bool SaveKeyFrames(const std::vector keyframes, const string& file_name); + + void LoadKeyFrames(const std::vector& vpKFs, Map* const pMap); + void LoadMapPoints(Map* const pMap); + void AddKeyFrame(ORB_SLAM2::KeyFrame *keyframe, ORB_SLAM2::Map *pMap); + private: // Input sensor diff --git a/include/Tracking.h b/include/Tracking.h index 5aaa93ef26..9e36df0176 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -144,6 +144,7 @@ class Tracking bool NeedNewKeyFrame(); void CreateNewKeyFrame(); +public: // In case of performing only localization, this flag is true when there are no matches to // points in the map. Still tracking will continue if there are enough matches with temporal points. // In that case we are doing visual odometry. The system will try to do relocalization to recover diff --git a/src/Frame.cc b/src/Frame.cc index 0e37d49335..db3543f1ce 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -227,6 +227,69 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra AssignFeaturesToGrid(); } +Frame::Frame(const cv::Size& imgSize, const cv::Mat& Tcw, const std::vector& keypoint, + const std::vector& keypoint_un, const cv::Mat& descriptor, const double timeStamp, + ORBextractor* const extractor, ORBVocabulary* const voc, const cv::Mat& K, const float bf, + const float thDepth, const unsigned long frame_id) + :mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(K.clone()), mbf(bf), mThDepth(thDepth) +{ + // Frame ID + // mnId=nNextId++; + mnId = frame_id; + + // Scale Level Info + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ORB extraction + mvKeys = keypoint; + mvKeysUn = keypoint_un; + mDescriptors = descriptor; + + N = mvKeysUn.size(); + SetPose(Tcw); + /*if(mvKeys.empty()) + return;*/ + + //UndistortKeyPoints(); + + // Set no stereo information + mvuRight = vector(N,-1); + mvDepth = vector(N,-1); + + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); + + // This is done only for the first Frame (or after a change in the calibration) + { + mnMinX = 0.0f; + mnMaxX = imgSize.width; + mnMinY = 0.0f; + mnMaxY = imgSize.height; + + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + } + + mb = mbf/fx; + if(mvKeysUn.size()>0) + AssignFeaturesToGrid(); +} + void Frame::AssignFeaturesToGrid() { int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS); diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 4ef1e78e0f..d3c8097a95 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -662,4 +662,50 @@ float KeyFrame::ComputeSceneMedianDepth(const int q) return vDepths[(vDepths.size()-1)/q]; } +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) {} + +// For boost serialize +template +void KeyFrame::serialize(Archive &ar, const unsigned int version) +{ + ar & mnId; + ar & const_cast &>(mvKeysUn); + ar & const_cast(mDescriptors); + ar & Tcw; + ar & mvpMapPoints; +} +template void KeyFrame::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void KeyFrame::serialize(boost::archive::binary_oarchive&, const unsigned int); } //namespace ORB_SLAM diff --git a/src/MapPoint.cc b/src/MapPoint.cc index 3b2921197b..334cd4d82a 100644 --- a/src/MapPoint.cc +++ b/src/MapPoint.cc @@ -416,6 +416,31 @@ int MapPoint::PredictScale(const float ¤tDist, Frame* pF) return nScale; } +MapPoint::MapPoint() + : mnFirstKFid(-1), + 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) + {} + +template +void MapPoint::serialize(Archive& ar, const unsigned int version) { + ar& mWorldPos; +} +template void MapPoint::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void MapPoint::serialize(boost::archive::binary_oarchive&, const unsigned int); } //namespace ORB_SLAM diff --git a/src/System.cc b/src/System.cc index 8df4157095..c977882e80 100644 --- a/src/System.cc +++ b/src/System.cc @@ -111,6 +111,16 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mpLoopCloser->SetTracker(mpTracker); mpLoopCloser->SetLocalMapper(mpLocalMapper); + + { + bool isLoadSucess = LoadMap("test.map"); + if(isLoadSucess) + { + ActivateLocalizationMode(); + mpTracker->mState = ORB_SLAM2::Tracking::eTrackingState::LOST; + //mTrackingState = mpTracker->mState; + } + } } cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) @@ -489,4 +499,129 @@ vector System::GetTrackedKeyPointsUn() return mTrackedKeyPointsUn; } +bool System::SaveMap(const string &file_name) +{ + std::vector vpKFs = mpMap->GetAllKeyFrames(); + // TODO erase null keyframe + return SaveKeyFrames(vpKFs,file_name); +} + +bool System::SaveKeyFrames(const std::vector keyframes, + const string &file_name) { + cout << "save keyframe size: " << keyframes.size() << endl; + std::ofstream out(file_name, std::ios_base::binary); + if (!out) { + cerr << "file_name is empty" << endl; + return false; + } + + cout << "Saving Map " << file_name << std::flush; + boost::archive::binary_oarchive oa(out, boost::archive::no_header); + oa << keyframes; + cout << "...Done!" << endl; + out.close(); + return true; +} + +bool System::LoadMap(const string &file_name) +{ + ifstream in(file_name,std::ios_base::binary); + if(!in) + { + cerr << "Load Map " << file_name << " is empty" << endl; + return false; + } + cout<<"Loading Map from "< vpKFs; + + boost::archive::binary_iarchive ia(in,boost::archive::no_header); + ia >> vpKFs; + + size_t total_size = 0; + if (vpKFs.size() == 0) { + cerr << "Load Map has no Keyframes!" << endl; + // return true, will not continue SLAM. + return true; + } else { + total_size = vpKFs.size() - 1; + } + + // LoadKeyFrames + LoadKeyFrames(vpKFs, mpMap); + // LoadMapPoints + LoadMapPoints(mpMap); + // Update KeyFrame Connections + vector vpKFs_in_map = mpMap->GetAllKeyFrames(); + for (size_t i = 0; i < vpKFs_in_map.size(); ++i) { + std::cout << "\rUpdate Connections Current/Total: " << i << " / " << total_size << std::flush; + vpKFs_in_map[i]->UpdateConnections(); + } + + std::cout<<"\nMap Reconstructing ... Done." << std::endl; + in.close(); + return true; +} + +void System::LoadMapPoints(Map* const pMap){ + vector vMPs = pMap->GetAllMapPoints(); + for (size_t i = 0; i < vMPs.size(); ++i) { + vMPs[i]->ComputeDistinctiveDescriptors(); + vMPs[i]->UpdateNormalAndDepth(); + } + std::cout << "\nLoad Map Points Finished." << std::endl; +} + +void System::LoadKeyFrames(const std::vector& vpKFs, Map* const pMap){ + for (auto& pKF:vpKFs) { + if(pKF == nullptr || pKF->isBad()){ + continue; + } + AddKeyFrame(pKF, pMap); + } +} + +void System::AddKeyFrame(ORB_SLAM2::KeyFrame *keyframe, ORB_SLAM2::Map *pMap) +{ + // TODO be value + Frame frame(cv::Size(640, 480), + keyframe->GetPose(), + keyframe->mvKeys, + keyframe->mvKeysUn, + keyframe->mDescriptors, + keyframe->mTimeStamp, + mpTracker->mpORBextractorLeft, + mpVocabulary, + mpTracker->mK, + mpTracker->mbf, + mpTracker->mThDepth, + keyframe->mnFrameId); + + KeyFrame* pKF = new KeyFrame(frame,pMap,mpKeyFrameDatabase); + pKF->SetPose(keyframe->GetPose()); + vector vDesc = Converter::toDescriptorVector(pKF->mDescriptors); + mpVocabulary->transform(vDesc,pKF->mBowVec,pKF->mFeatVec,4); + + pMap->AddKeyFrame(pKF); + mpKeyFrameDatabase->add(pKF); + + vector vpMps = keyframe->GetMapPointMatches(); + + for (unsigned i = 0; i < vpMps.size(); ++i) { + MapPoint* pMP = vpMps[i]; + if (pMP == nullptr || pMP->isBad()) { + continue; + } + if(pMP->mpRefKF == nullptr || pMP->mnFirstKFid > static_cast(keyframe->mnId)) { + pMP->SetRefKF(pKF); + pMP->SetMap(pMap); + pMP->mnFirstKFid = static_cast(pKF->mnId); + pMP->mnFirstFrame = static_cast(pKF->mnFrameId); + } + pMP->AddObservation(pKF,i); + pKF->AddMapPoint(pMP,i); + + pMap->AddMapPoint(pMP); + } +} + } //namespace ORB_SLAM diff --git a/src/Tracking.cc b/src/Tracking.cc index 2273b2ce48..2e66326a1a 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -497,10 +497,13 @@ void Tracking::Track() else { // This can happen if tracking is lost - mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); - mlpReferences.push_back(mlpReferences.back()); - mlFrameTimes.push_back(mlFrameTimes.back()); - mlbLost.push_back(mState==LOST); + + if(!mlRelativeFramePoses.empty()){ + mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); + mlpReferences.push_back(mlpReferences.back()); + mlFrameTimes.push_back(mlFrameTimes.back()); + mlbLost.push_back(mState==LOST); + } } } diff --git a/src/Viewer.cc b/src/Viewer.cc index dec3204f53..f69867a285 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -72,6 +72,7 @@ void Viewer::Run() pangolin::Var menuShowGraph("menu.Show Graph",true,true); pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); pangolin::Var menuReset("menu.Reset",false,false); + pangolin::Var menuSaveMap("menu.Save Map",false,false); // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam( @@ -153,6 +154,10 @@ void Viewer::Run() menuReset = false; } + if(menuSaveMap){ + mpSystem->SaveMap("test.map"); + menuSaveMap = false; + } if(Stop()) { while(isStopped())