Add files via upload

src/orb_slam/src/KeyFrame.cc

@@ -65,13 +65,18 @@ KeyFrameConstInfo::KeyFrameConstInfo(cv::FileStorage &fs, KeyFrameDatabase *pKFD
     fs["mbFirstConnection"] >> mbFirstConnection;
     fs["mHalfBaseline"] >> mHalfBaseline;
     fs["mvpMapPointsId"] >> mvpMapPointsId;
+    fs["mvpCorrKeyPointsId"] >> mvpCorrKeyPointsId;
     fs["mvpOrderedConnectedKeyFramesId"] >> mvpOrderedConnectedKeyFramesId;
     fs["mvOrderedWeights"] >> mvOrderedWeights;
     fs["mpParentId"] >> mpParentId;
     fs["mspChildrensId"] >> mspChildrensId;
     fs["mspLoopEdgesId"] >> mspLoopEdgesId;
     fs["Pose"] >> Tcw;
     fs.release();
+    assert(mvpMapPointsId.size() == mvpCorrKeyPointsId.size());
+    for(decltype(mvpMapPointsId)::const_iterator mpt_it = mvpMapPointsId.begin(), kpt_it = mvpCorrKeyPointsId.begin();
+     mpt_it != mvpMapPointsId.end(), kpt_it != mvpCorrKeyPointsId.end(); ++mpt_it, ++kpt_it)
+        mmapMpt2KptId.insert(std::make_pair(*(mpt_it), *(kpt_it)));
 }
 
 KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
@@ -120,6 +125,7 @@ KeyFrame::KeyFrame(boost::archive::binary_iarchive &ira, const KeyFrameConstInfo
                 continue;
             }
             mvpMapPoints.push_back(it->second);
+            mmapMpt2Kpt[it->second] = info->mmapMpt2KptId.at(mapId);
         }
         SetPose(info->Tcw);
 
@@ -160,14 +166,23 @@ void KeyFrame::GlobalConnection(const KeyFrameConstInfo *info, const unordered_m
         mpParent = it->second;
 }
 
-void KeyFrame::saveData(const string baseName, const unordered_map<KeyFrame*, int32_t> &mapKeyFrameId,
-        const unordered_map<MapPoint*, int32_t> &mapMapPointId)
+void KeyFrame::saveData(const string baseName, const unordered_map<KeyFrame*, int> &mapKeyFrameId,
+        const unordered_map<MapPoint*, int> &mapMapPointId)
 {
-    vector<int32_t> mvpMapPointsId, mvpOrderedConnectedKeyFramesId, mspChildrensId, mspLoopEdgesId, vOrderedWeights;
+    vector<int> mvpMapPointsId, mvpCorrKeyPointsId;
+    vector<int> mvpOrderedConnectedKeyFramesId, mspChildrensId, mspLoopEdgesId, vOrderedWeights;
+
     for(MapPoint* mpt:mvpMapPoints){
+        if(!mpt || mpt->isBad())
+            continue;
         auto it = mapMapPointId.find(mpt);
-        if(it!=mapMapPointId.end())
-            mvpMapPointsId.push_back(it->second);
+        if(it==mapMapPointId.end())
+            continue;
+        auto obsKF = it->first->GetObservations();
+        if(obsKF.count(this) == 0)
+            continue;
+        mvpMapPointsId.push_back(it->second);
+        mvpCorrKeyPointsId.push_back(obsKF[this]);
     }
     for(KeyFrame* kf:mvpOrderedConnectedKeyFrames){
         auto it = mapKeyFrameId.find(kf);
@@ -227,6 +242,7 @@ void KeyFrame::saveData(const string baseName, const unordered_map<KeyFrame*, in
     of << "mK" << mK;
     of << "Pose" << GetPose();
     of << "mvpMapPointsId" << mvpMapPointsId;
+    of << "mvpCorrKeyPointsId" << mvpCorrKeyPointsId;
     of << "mvpOrderedConnectedKeyFramesId" << mvpOrderedConnectedKeyFramesId;
     of << "mvOrderedWeights" << vOrderedWeights;
     of << "mbFirstConnection" << mbFirstConnection;
@@ -280,6 +296,16 @@ cv::Mat KeyFrame::GetPoseInverse()
     return Twc.clone();
 }
 
+cv::Mat KeyFrame::GetPoseSafe() const
+{
+    return Tcw.clone();
+}
+
+cv::Mat KeyFrame::GetPoseInverseSafe() const
+{
+    return Twc.clone();
+}
+
 cv::Mat KeyFrame::GetCameraCenter()
 {
     unique_lock<mutex> lock(mMutexPose);
@@ -383,6 +409,35 @@ vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)
     }
 }
 
+vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFramesSafe() const
+{
+    return mvpOrderedConnectedKeyFrames;
+}
+
+vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFramesSafe(const int &N) const
+{
+    if((int)mvpOrderedConnectedKeyFrames.size()<N)
+        return mvpOrderedConnectedKeyFrames;
+    else
+        return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);
+
+}
+
+vector<KeyFrame*> KeyFrame::GetCovisiblesByWeightSafe(const int &w) const
+{
+    if(mvpOrderedConnectedKeyFrames.empty())
+        return vector<KeyFrame*>();
+    auto mvOrderedWeightsCopy = mvOrderedWeights;
+    vector<int>::iterator it = upper_bound(mvOrderedWeightsCopy.begin(),mvOrderedWeightsCopy.end(),w,KeyFrame::weightComp);
+    if(it==mvOrderedWeightsCopy.end())
+        return vector<KeyFrame*>();
+    else
+    {
+        int n = it-mvOrderedWeightsCopy.begin();
+        return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
+    }
+}
+
 int KeyFrame::GetWeight(KeyFrame *pKF)
 {
     unique_lock<mutex> lock(mMutexConnections);
@@ -465,6 +520,24 @@ vector<MapPoint*> KeyFrame::GetMapPointMatches()
     return mvpMapPoints;
 }
 
+vector<MapPoint*> KeyFrame::GetMapPointMatchesSafe() const
+{
+    return mvpMapPoints;
+}
+
+map<int, int> KeyFrame::GetMatchedKptIds(const KeyFrame* pKF) const
+{
+    map<int, int> matchedKptIds;
+    for(auto pair_it = mmapMpt2Kpt.begin(); pair_it != mmapMpt2Kpt.end(); ++ pair_it){
+        if(pKF->mmapMpt2Kpt.count(pair_it->first) > 0)
+        {
+            matchedKptIds[pair_it->second] = pKF->mmapMpt2Kpt.at(pair_it->first);
+        }
+    }
+    return matchedKptIds;
+}
+
+
 MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
 {
     unique_lock<mutex> lock(mMutexFeatures);

src/orb_slam/src/KeyFrameDatabase.cc

@@ -21,7 +21,7 @@
 #include "KeyFrameDatabase.h"
 
 #include "KeyFrame.h"
-#include"Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "../Thirdparty/DBoW2/DBoW2/BowVector.h"
 
 #include<mutex>
 

src/orb_slam/src/Map.cc

@@ -88,8 +88,11 @@ vector<KeyFrame*> Map::GetAllKeyFrames(bool onlygood)
         vector<KeyFrame*> goodKeyFrames;
         goodKeyFrames.reserve(mspKeyFrames.size());
         for(auto it=mspKeyFrames.begin(); it != mspKeyFrames.end(); ++it){
-            if(!(*it)->isBad())
-                goodKeyFrames.push_back(*it);
+            if((*it) == NULL)
+                continue;
+            if((*it)->isBad())
+                continue;
+            goodKeyFrames.push_back(*it);
         }
         return goodKeyFrames;
     }
@@ -104,8 +107,11 @@ vector<MapPoint*> Map::GetAllMapPoints(bool onlygood)
         vector<MapPoint*> goodMapPoints;
         goodMapPoints.reserve(mspMapPoints.size());
         for(auto it=mspMapPoints.begin(); it != mspMapPoints.end(); ++it){
-            if(!(*it)->isBad())
-                goodMapPoints.push_back(*it);
+            if((*it) == NULL)
+                continue;
+            if((*it)->isBad())
+                continue;
+            goodMapPoints.push_back(*it);
         }
         return goodMapPoints;
     }
@@ -163,37 +169,39 @@ void Map::RestoreMap(cv::FileStorage &fs, vector<int> &vkFId)
     fs.release();
 }
 
-void Map::RestoreMapPointsConnection(cv::FileStorage &fs, unordered_map<int, KeyFrame*> &mapKFId, unordered_map<int, MapPoint*> &mapMptId)
+void Map::RestoreMapPointsConnection(cv::FileStorage &fs, unordered_map<int, KeyFrame*> &KeyFrameQuery, unordered_map<int, MapPoint*> &MapPointQuery)
 {
     cv::FileNode mptsfn = fs["mspMapPoints"];
-    for(cv::FileNodeIterator NodeIt = mptsfn.begin(); NodeIt != mptsfn.end(); ++NodeIt)  // MapPoint FileNode
+    for(cv::FileNodeIterator NodeIt = mptsfn.begin(); NodeIt != mptsfn.end(); ++ NodeIt)  // MapPoint FileNode
     {
         vector<int> mobsMapKFId, mMapKFInId;
         int MptId;
-        (*NodeIt)["mobsMapKFId"] >> mobsMapKFId;
-        (*NodeIt)["mMapKFInId"] >> mMapKFInId;
-        (*NodeIt)["mnId"] >> MptId;
+        (*NodeIt)["mobsMapKFId"] >> mobsMapKFId; // id of KeyFrames by which this MapPoint is observed
+        (*NodeIt)["mMapKFInId"] >> mMapKFInId; // id of Corresponding KeyPoints in above KeyFrames
+        (*NodeIt)["mnId"] >> MptId; // id of this obeserved MapPoints
         assert(mobsMapKFId.size() == mMapKFInId.size());
-        auto MptIt = mapMptId.find(MptId); // verify the MapPoint
-        if(MptIt == mapMptId.end()){
+        auto MptIt = MapPointQuery.find(MptId); // verify the MapPoint
+        if(MptIt == MapPointQuery.end()){
             std::cout << "[Warning] " <<__FILE__ << " Line " << __LINE__ <<": \033[33;1mLost MapPoint in Hash Table\033[0m" << std::endl;
                 continue;
         }
-        MapPoint* pMpt = mapMptId[MptId]; // retrive the valid MapPoint
+        MapPoint* pMpt = MapPointQuery[MptId]; // retrive the valid MapPoint
         for(int i = 0; i < (int) mobsMapKFId.size(); ++i){
-            int KFId = mobsMapKFId[i], KFInId = mMapKFInId[i];
-            auto KFIt = mapKFId.find(KFId);   // verify the KeyFrame
-            if(KFIt == mapKFId.end()){
+            const int KFId = mobsMapKFId[i], KFInId = mMapKFInId[i];
+            auto KFIt = KeyFrameQuery.find(KFId);   
+            if(KFIt == KeyFrameQuery.end()) // ensure the KeyFrame Id is in mobsMapKFId
+            {
                 std::cout << "[Warning] " <<__FILE__ << " Line " << __LINE__ <<": \033[33;1mUnconnected MapPoint for KeyFrame\033[0m" << std::endl;
                 continue;
             }
-            if(KFInId > (int)(KFIt->second->mvKeysUn.size()-1) || KFInId < 0){
+            if(KFInId > (int)(KFIt->second->mvKeysUn.size()-1) || KFInId < 0) // ensure the Keypoint Id is valid
+            {
                 std::cout << "[Warning] " <<__FILE__ << " Line " << __LINE__ <<": \033[33;1mIndex of Connected MapPoint Overflow the size of Keypoints\033[0m" << std::endl;
                 continue;
             }
-            KFIt->second->mmapMpt2Kpt[pMpt] = KFInId;   
+            // KFIt->second->mmapMpt2Kpt[pMpt] = KFInId;   // mmapMpt2Kpt has been restored by KeyFrame()
             if(!pMpt->IsInKeyFrame(KFIt->second))
-                pMpt->AddObservationStatic(KFIt->second, (size_t)KFInId);    // why Segment Fault??
+                pMpt->AddObservationStatic(KFIt->second, (std::size_t)KFInId);
         }
     }
     fs.release();
@@ -206,11 +214,11 @@ void Map::RestoreMapPointsConnection(cv::FileStorage &fs, unordered_map<int, Key
 cv::FileStorage& operator<<(cv::FileStorage &fs, ORB_SLAM2::Map &map){
     fs << "mspMapPoints" << "{";
     vector<ORB_SLAM2::MapPoint*> vmpts = map.GetAllMapPoints();
-    for(int i = 0; i < (int)vmpts.size();++i){
+    for(ORB_SLAM2::MapPoint* mpt:vmpts){
         char index[15];
-        sprintf(index, "%06d", i);
+        sprintf(index, "%ld", mpt->mnId);
         fs << "MapPoint_"+std::string(index) << "{";
-        fs << *(vmpts[i]) << "}";
+        fs << *(mpt) << "}";
     }
     fs << "}";
     vector<ORB_SLAM2::KeyFrame*> vKFs = map.GetAllKeyFrames();

src/orb_slam/src/ORBmatcher.cc

@@ -25,7 +25,7 @@
 #include<opencv2/core/core.hpp>
 #include<opencv2/features2d/features2d.hpp>
 
-#include"Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+#include "../Thirdparty/DBoW2/DBoW2/FeatureVector.h"
 
 #include <stdint.h>
 

src/orb_slam/src/System.cc

@@ -531,6 +531,12 @@ vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()
     return mTrackedKeyPointsUn;
 }
 
+std::vector<KeyFrame*> System::GetAllKeyFrames(bool only_good)
+{
+    unique_lock<mutex> lock(mMutexState);
+    return mpMap->GetAllKeyFrames(only_good);
+}
+
 std::vector<MapPoint*> System::GetAllMapPoints(bool only_good)
 {
     unique_lock<mutex> lock(mMutexState);
@@ -557,7 +563,7 @@ void System::SaveMap(const std::string &filename) const
 
 void System::SaveKeyFrames(const string &dirname) const
 {
-    std::string KeyFrameDir(dirname+"KeyFrames/");
+    std::string KeyFrameDir(dirname);
     makedir(KeyFrameDir.c_str());
     vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames(true);
     sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
@@ -614,10 +620,10 @@ void System::RestoreSystemFromFile(const string &keyFrameDir, const string &mapF
     mpMap->RestoreMap(fs, KFinMapId);
     vector<MapPoint*> goodMapPoints = mpMap->GetAllMapPoints(true);
     std::cout << "Found " << goodMapPoints.size() << " MapPoints." << std::endl;
-    unordered_map<int, MapPoint*> mapMapPointId;
-    mapMapPointId.reserve(goodMapPoints.size());
+    unordered_map<int, MapPoint*> MapPointQuery;
+    MapPointQuery.reserve(goodMapPoints.size());
     for(MapPoint* mpt : goodMapPoints)
-        mapMapPointId[mpt->mnId] = mpt;
+        MapPointQuery[mpt->mnId] = mpt;
     std::cout << "" << goodMapPoints.size() << " MapPoints Restored." << std::endl;
     vector<string> InfoFile, FeatFile;
     DIR *dirp = nullptr;
@@ -645,31 +651,32 @@ void System::RestoreSystemFromFile(const string &keyFrameDir, const string &mapF
     vpKF.resize(FeatFile.size());
     mapKeyFrameId.reserve(FeatFile.size());
     int cnt = 0;
-    #pragma omp parallel for ordered schedule(dynamic)      // open it for multithread reading, but sometimes it causes failed points
+    std::mutex FileMutex;
+    #pragma omp parallel for schedule(static)     // open it for multithread reading, but sometimes it causes failed points
     for(std::size_t fi = 0; fi < FeatFile.size(); ++ fi)
     {
         string featFilename = FeatFile[fi], infoFilename = InfoFile[fi];
         cv::FileStorage cvfs(infoFilename, cv::FileStorage::READ);
         ifstream ifs(featFilename, ios::binary);
         boost::archive::binary_iarchive iar(ifs);
         KeyFrameConstInfo* constInfo(new KeyFrameConstInfo(cvfs, mpKeyFrameDatabase, mpVocabulary));
-        KeyFrame* newKF(new KeyFrame(std::ref(iar), constInfo, mpMap, goodMapPoints, mapMapPointId));
-    #pragma omp ordered  // single thread
-    {
+        KeyFrame* newKF(new KeyFrame(std::ref(iar), constInfo, mpMap, goodMapPoints, MapPointQuery));
         vConstInfo[fi] = constInfo;
         vpKF[fi] = newKF;
-        if(FeatFile.size() > 100)  // May wait some time. A simple Process Indicator is added.
         {
-            ++ cnt;
-            if((cnt) % (FeatFile.size() / 10) == 0)
+            std::unique_lock<std::mutex> lock(FileMutex);
+            if(FeatFile.size() > 100)  // May wait some time. A simple Process Indicator is added.
             {
-                char msg[30];
-                sprintf(msg, "%0.2lf %% Saved.", 100.0*(cnt+1)/FeatFile.size());
-                std::cout << msg << std::endl;
+                ++ cnt;
+                if((cnt) % (FeatFile.size() / 10) == 0)
+                {
+                    char msg[30];
+                    sprintf(msg, "%0.2lf %% loaded.", 100.0*(cnt+1)/FeatFile.size());
+                    std::cout << msg << std::endl;
+                }
             }
         }
     }
-    }
     std::cout << vpKF.size() << " KeyFrames Restored." << std::endl;
     // May be unnecessary to sort
     std::sort(vpKF.begin(),vpKF.end(),KeyFrame::lId);
@@ -680,7 +687,7 @@ void System::RestoreSystemFromFile(const string &keyFrameDir, const string &mapF
         vpKF[kFi]->GlobalConnection(vConstInfo[kFi], mapKeyFrameId);
     std::cout << "KeyFrame-to-KeyFrame Connections Restored." << std::endl;
     cv::FileStorage MapFs(mapFilename, cv::FileStorage::READ);
-    mpMap->RestoreMapPointsConnection(MapFs, mapKeyFrameId, mapMapPointId);
+    mpMap->RestoreMapPointsConnection(MapFs, mapKeyFrameId, MapPointQuery);
     std::cout << "MapPoint-to-KeyFrame Connections Restored." << std::endl;
     for(KeyFrame* pKF: vpKF)
         mpMap->AddKeyFrame(pKF);