Save/Load Atlas Functionality

src/Atlas.cc

@@ -259,6 +259,81 @@ bool Atlas::isImuInitialized()
     return mpCurrentMap->isImuInitialized();
 }
 
+void Atlas::PreSave()
+{
+    if(mpCurrentMap){
+        if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
+            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum
+    }
+
+    struct compFunctor
+    {
+        inline bool operator()(Map* elem1 ,Map* elem2)
+        {
+            return elem1->GetId() < elem2->GetId();
+        }
+    };
+    std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps));
+    sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());
+
+    std::set<GeometricCamera*> spCams(mvpCameras.begin(), mvpCameras.end());
+    cout << "There are " << spCams.size() << " cameras in the atlas" << endl;
+    for(Map* pMi : mvpBackupMaps)
+    {
+        cout << "Pre-save of map " << pMi->GetId() << endl;
+        pMi->PreSave(spCams);
+    }
+    cout << "Maps stored" << endl;
+    for(GeometricCamera* pCam : mvpCameras)
+    {
+        cout << "Pre-save of camera " << pCam->GetId() << endl;
+        if(pCam->GetType() == pCam->CAM_PINHOLE)
+        {
+            mvpBackupCamPin.push_back((Pinhole*) pCam);
+        }
+        else if(pCam->GetType() == pCam->CAM_FISHEYE)
+        {
+            mvpBackupCamKan.push_back((KannalaBrandt8*) pCam);
+        }
+    }
+
+}
+
+void Atlas::PostLoad()
+{
+    mvpCameras.clear();
+    map<unsigned int,GeometricCamera*> mpCams;
+    for(Pinhole* pCam : mvpBackupCamPin)
+    {
+        //mvpCameras.push_back((GeometricCamera*)pCam);
+        mvpCameras.push_back(pCam);
+        mpCams[pCam->GetId()] = pCam;
+    }
+    for(KannalaBrandt8* pCam : mvpBackupCamKan)
+    {
+        //mvpCameras.push_back((GeometricCamera*)pCam);
+        mvpCameras.push_back(pCam);
+        mpCams[pCam->GetId()] = pCam;
+    }
+
+    mspMaps.clear();
+    unsigned long int numKF = 0, numMP = 0;
+    map<long unsigned int, KeyFrame*> mpAllKeyFrameId;
+    for(Map* pMi : mvpBackupMaps)
+    {
+        cout << "Map id:" << pMi->GetId() << endl;
+        mspMaps.insert(pMi);
+        map<long unsigned int, KeyFrame*> mpKeyFrameId;
+        pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpKeyFrameId, mpCams);
+        mpAllKeyFrameId.insert(mpKeyFrameId.begin(), mpKeyFrameId.end());
+        numKF += pMi->GetAllKeyFrames().size();
+        numMP += pMi->GetAllMapPoints().size();
+    }
+
+    cout << "Number KF:" << numKF << "; number MP:" << numMP << endl;
+    mvpBackupMaps.clear();
+}
+
 void Atlas::SetKeyFrameDababase(KeyFrameDatabase* pKFDB)
 {
     mpKeyFrameDB = pKFDB;

src/KeyFrame.cc

@@ -867,6 +867,177 @@ void KeyFrame::UpdateMap(Map* pMap)
     mpMap = pMap;
 }
 
+void KeyFrame::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP, set<GeometricCamera*>& spCam)
+{
+    /*
+    // Save the id of each MapPoint in this KF, there can be null pointer in the vector
+    mvBackupMapPointsId.clear();
+    mvBackupMapPointsId.reserve(N);
+    for(int i = 0; i < N; ++i)
+    {
+
+        if(mvpMapPoints[i] && spMP.find(mvpMapPoints[i]) != spMP.end()) // Checks if the element is not null
+            mvBackupMapPointsId.push_back(mvpMapPoints[i]->mnId);
+        else // If the element is null his value is -1 because all the id are positives
+            mvBackupMapPointsId.push_back(-1);
+    }
+    //cout << "KeyFrame: ID from MapPoints stored" << endl;
+    */
+
+    // Save the id of each connected KF with it weight
+    mBackupConnectedKeyFrameIdWeights.clear();
+    for(std::map<KeyFrame*,int>::const_iterator it = mConnectedKeyFrameWeights.begin(), end = mConnectedKeyFrameWeights.end(); it != end; ++it)
+    {
+        if(spKF.find(it->first) != spKF.end())
+            mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second;
+    }
+    //cout << "KeyFrame: ID from connected KFs stored" << endl;
+    // Save the parent id
+    mBackupParentId = -1;
+    if(mpParent && spKF.find(mpParent) != spKF.end())
+        mBackupParentId = mpParent->mnId;
+    //cout << "KeyFrame: ID from Parent KF stored" << endl;
+    // Save the id of the childrens KF
+    mvBackupChildrensId.clear();
+    mvBackupChildrensId.reserve(mspChildrens.size());
+    for(KeyFrame* pKFi : mspChildrens)
+    {
+        if(spKF.find(pKFi) != spKF.end())
+            mvBackupChildrensId.push_back(pKFi->mnId);
+    }
+    //cout << "KeyFrame: ID from Children KFs stored" << endl;
+    // Save the id of the loop edge KF
+    mvBackupLoopEdgesId.clear();
+    mvBackupLoopEdgesId.reserve(mspLoopEdges.size());
+    for(KeyFrame* pKFi : mspLoopEdges)
+    {
+        if(spKF.find(pKFi) != spKF.end())
+            mvBackupLoopEdgesId.push_back(pKFi->mnId);
+    }
+    //cout << "KeyFrame: ID from Loop KFs stored" << endl;
+    // Save the id of the merge edge KF
+    mvBackupMergeEdgesId.clear();
+    mvBackupMergeEdgesId.reserve(mspMergeEdges.size());
+    for(KeyFrame* pKFi : mspMergeEdges)
+    {
+        if(spKF.find(pKFi) != spKF.end())
+            mvBackupMergeEdgesId.push_back(pKFi->mnId);
+    }
+    //cout << "KeyFrame: ID from Merge KFs stored" << endl;
+
+    //Camera data
+    mnBackupIdCamera = -1;
+    if(mpCamera && spCam.find(mpCamera) != spCam.end())
+        mnBackupIdCamera = mpCamera->GetId();
+    //cout << "KeyFrame: ID from Camera1 stored; " << mnBackupIdCamera << endl;
+
+    mnBackupIdCamera2 = -1;
+    if(mpCamera2 && spCam.find(mpCamera2) != spCam.end())
+        mnBackupIdCamera2 = mpCamera2->GetId();
+    //cout << "KeyFrame: ID from Camera2 stored; " << mnBackupIdCamera2 << endl;
+
+    //Inertial data
+    mBackupPrevKFId = -1;
+    if(mPrevKF && spKF.find(mPrevKF) != spKF.end())
+        mBackupPrevKFId = mPrevKF->mnId;
+    //cout << "KeyFrame: ID from Prev KF stored" << endl;
+    mBackupNextKFId = -1;
+    if(mNextKF && spKF.find(mNextKF) != spKF.end())
+        mBackupNextKFId = mNextKF->mnId;
+    //cout << "KeyFrame: ID from NextKF stored" << endl;
+    if(mpImuPreintegrated)
+        mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated);
+    //cout << "KeyFrame: Imu Preintegrated stored" << endl;
+}
+
+void KeyFrame::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid, map<unsigned int, GeometricCamera*>& mpCamId){
+    // Rebuild the empty variables
+
+    // Pose
+    SetPose(Tcw);
+
+    /*
+    // Reference reconstruction
+    // Each MapPoint sight from this KeyFrame
+    mvpMapPoints.clear();
+    mvpMapPoints.resize(N);
+    for(int i=0; i<N; ++i)
+    {
+        if(mvBackupMapPointsId[i] != -1)
+            mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]];
+        else
+            mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+    }
+
+    */
+
+    // Conected KeyFrames with him weight
+    mConnectedKeyFrameWeights.clear();
+    for(map<long unsigned int, int>::const_iterator it = mBackupConnectedKeyFrameIdWeights.begin(), end = mBackupConnectedKeyFrameIdWeights.end();
+        it != end; ++it)
+    {
+        KeyFrame* pKFi = mpKFid[it->first];
+        mConnectedKeyFrameWeights[pKFi] = it->second;
+    }
+
+    // Restore parent KeyFrame
+    if(mBackupParentId>=0)
+        mpParent = mpKFid[mBackupParentId];
+
+    // KeyFrame childrens
+    mspChildrens.clear();
+    for(vector<long unsigned int>::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it!=end; ++it)
+    {
+        mspChildrens.insert(mpKFid[*it]);
+    }
+
+    // Loop edge KeyFrame
+    mspLoopEdges.clear();
+    for(vector<long unsigned int>::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it)
+    {
+        mspLoopEdges.insert(mpKFid[*it]);
+    }
+
+    // Merge edge KeyFrame
+    mspMergeEdges.clear();
+    for(vector<long unsigned int>::const_iterator it = mvBackupMergeEdgesId.begin(), end = mvBackupMergeEdgesId.end(); it != end; ++it)
+    {
+        mspMergeEdges.insert(mpKFid[*it]);
+    }
+
+    //Camera data
+    if(mnBackupIdCamera >= 0)
+    {
+        mpCamera = mpCamId[mnBackupIdCamera];
+    }
+    if(mnBackupIdCamera2 >= 0)
+    {
+        mpCamera2 = mpCamId[mnBackupIdCamera2];
+    }
+
+    //Inertial data
+    if(mBackupPrevKFId != -1)
+    {
+        mPrevKF = mpKFid[mBackupPrevKFId];
+    }
+    if(mBackupNextKFId != -1)
+    {
+        mNextKF = mpKFid[mBackupNextKFId];
+    }
+    mpImuPreintegrated = &mBackupImuPreintegrated;
+
+
+    // Remove all backup container
+    mvBackupMapPointsId.clear();
+    mBackupConnectedKeyFrameIdWeights.clear();
+    mvBackupChildrensId.clear();
+    mvBackupLoopEdgesId.clear();
+
+    UpdateBestCovisibles();
+
+    //ComputeSceneMedianDepth();
+}
+
 bool KeyFrame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v)
 {
 

src/Map.cc

@@ -489,5 +489,112 @@ void Map::SetLastMapChange(int currentChangeId)
     mnMapChangeNotified = currentChangeId;
 }
 
+void Map::PreSave(std::set<GeometricCamera*> &spCams)
+{
+    int nMPWithoutObs = 0;
+    for(MapPoint* pMPi : mspMapPoints)
+    {
+        if(pMPi->GetObservations().size() == 0)
+        {
+            nMPWithoutObs++;
+        }
+        map<KeyFrame*, std::tuple<int,int>> mpObs = pMPi->GetObservations();
+        for(map<KeyFrame*, std::tuple<int,int>>::iterator it= mpObs.begin(), end=mpObs.end(); it!=end; ++it)
+        {
+            if(it->first->GetMap() != this)
+            {
+                pMPi->EraseObservation(it->first); //We need to find where the KF is set as Bad but the observation is not removed
+            }
+
+        }
+    }
+    cout << "  Bad MapPoints removed" << endl;
+
+    // Saves the id of KF origins
+    mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size());
+    for(int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i)
+    {
+        mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId);
+    }
+
+    mvpBackupMapPoints.clear();
+    // Backup of set container to vector
+    //std::copy(mspMapPoints.begin(), mspMapPoints.end(), std::back_inserter(mvpBackupMapPoints));
+    for(MapPoint* pMPi : mspMapPoints)
+    {
+        //cout << "Pre-save of mappoint " << pMPi->mnId << endl;
+        //mvpBackupMapPoints.push_back(pMPi);
+        pMPi->PreSave(mspKeyFrames,mspMapPoints);
+    }
+    cout << "  MapPoints back up done!!" << endl;
+
+    mvpBackupKeyFrames.clear();
+    //std::copy(mspKeyFrames.begin(), mspKeyFrames.end(), std::back_inserter(mvpBackupKeyFrames));
+    for(KeyFrame* pKFi : mspKeyFrames)
+    {
+        //mvpBackupKeyFrames.push_back(pKFi);
+        pKFi->PreSave(mspKeyFrames,mspMapPoints, spCams);
+    }
+    cout << "  KeyFrames back up done!!" << endl;
+
+    mnBackupKFinitialID = -1;
+    if(mpKFinitial)
+    {
+        mnBackupKFinitialID = mpKFinitial->mnId;
+    }
+
+    mnBackupKFlowerID = -1;
+    if(mpKFlowerID)
+    {
+        mnBackupKFlowerID = mpKFlowerID->mnId;
+    }
+
+}
+
+void Map::PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc, map<long unsigned int, KeyFrame*>& mpKeyFrameId, map<unsigned int, GeometricCamera*> &mpCams)
+{
+    /*
+    std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin()));
+    std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), std::inserter(mspKeyFrames, mspKeyFrames.begin()));
+    */
+
+    map<long unsigned int,MapPoint*> mpMapPointId;
+    for(MapPoint* pMPi : mspMapPoints)
+    {
+        pMPi->UpdateMap(this);
+        mpMapPointId[pMPi->mnId] = pMPi;
+    }
+
+    //map<long unsigned int, KeyFrame*> mpKeyFrameId;
+    for(KeyFrame* pKFi : mspKeyFrames)
+    {
+        pKFi->UpdateMap(this);
+        pKFi->SetORBVocabulary(pORBVoc);
+        pKFi->SetKeyFrameDatabase(pKFDB);
+        mpKeyFrameId[pKFi->mnId] = pKFi;
+    }
+
+    cout << "Number of KF: " << mspKeyFrames.size() << endl;
+    cout << "Number of MP: " << mspMapPoints.size() << endl;
+
+    // References reconstruction between different instances
+    for(MapPoint* pMPi : mspMapPoints)
+    {
+        //cout << "Post-Load of mappoint " << pMPi->mnId << endl;
+        pMPi->PostLoad(mpKeyFrameId, mpMapPointId);
+    }
+    cout << "End to rebuild MapPoint references" << endl;
+
+    for(KeyFrame* pKFi : mspKeyFrames)
+    {
+        pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);
+        pKFDB->add(pKFi);
+    }
+
+    cout << "End to rebuild KeyFrame references" << endl;
+
+    mvpBackupMapPoints.clear();
+}
+
 
 } //namespace ORB_SLAM3