2. How to load multiple maps, key frames, map points and other data structures from binary files in ORB-SLAM3

Table of contents

1.3 Load keyframes and map points Map::PostLoad

1.4 Restore map point information MapPoint::PostLoad

1.5 Restore key frame information KeyFrame::PostLoad

1 Why save & load (visual) maps

        Because we are going to do navigation, navigation requires a priori map. Therefore it is necessary to save the map for navigation.

        In the last blog we explained how to save map data. This is the file we have saved.

         to explain how to load map data.

1.1 Main function to load multiple maps

Let’s look at the code in the System.cc file:

    // 查看配置文件版本，不同版本有不同处理方法
    cv::FileNode node = fsSettings["File.version"];
    if(!node.empty() && node.isString() && node.string() == "1.0")
    {
        settings_ = new Settings(strSettingsFile,mSensor);

        // 保存及加载地图的名字
        mStrLoadAtlasFromFile = settings_->atlasLoadFile();
        mStrSaveAtlasToFile = settings_->atlasSaveFile();

        cout << (*settings_) << endl;
    }
    // 我们的配置文件版本为1.0版本  LoadAtlasFromFile = ‘akm’  SaveAtlasToFile = ‘akm’
    // mStrLoadAtlasFromFile = ‘akm’
    // mStrSaveAtlasToFile = ‘akm’
    else
    {
        settings_ = nullptr;
        cv::FileNode node = fsSettings["System.LoadAtlasFromFile"];
        if(!node.empty() && node.isString())
        {
            mStrLoadAtlasFromFile = (string)node;
        }

        node = fsSettings["System.SaveAtlasToFile"];
        if(!node.empty() && node.isString())
        {
            mStrSaveAtlasToFile = (string)node;
        }
    }

Our configuration file version is version 1.0.

mStrSaveAtlasToFile

In the setting constructor we read the following parameters:

void Settings::readLoadAndSave(cv::FileStorage &fSettings)
{
    bool found;

    sLoadFrom_ = readParameter<string>(fSettings, "System.LoadAtlasFromFile", found, false);
    sSaveto_ = readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false);
}

Therefore, the variables mStrLoadAtlasFromFile and mStrSaveAtlasToFile are characters String'akm'.

Then look at this paragraph:

    // 如果没有先验地图的话 mStrLoadAtlasFromFile = ‘akm’
    if(mStrLoadAtlasFromFile.empty())
    {
        //Load ORB Vocabulary
        cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

        // 建立一个新的ORB字典
        mpVocabulary = new ORBVocabulary();
        // 读取预训练好的ORB字典并返回成功/失败标志
        bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
        // 如果加载失败，就输出错误信息
        if(!bVocLoad)
        {
            cerr << "Wrong path to vocabulary. " << endl;
            cerr << "Falied to open at: " << strVocFile << endl;
            exit(-1);
        }
        cout << "Vocabulary loaded!" << endl << endl;

        //Create KeyFrame Database
        // Step 4 创建关键帧数据库
        mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

        //Create the Atlas
        // Step 5 创建多地图，参数0表示初始化关键帧id为0
        cout << "Initialization of Atlas from scratch " << endl;
        mpAtlas = new Atlas(0);
    }

    // 如果有先验地图的话
    else
    {
        //Load ORB Vocabulary
        cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

        mpVocabulary = new ORBVocabulary();
        bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
        if(!bVocLoad)
        {
            cerr << "Wrong path to vocabulary. " << endl;
            cerr << "Falied to open at: " << strVocFile << endl;
            exit(-1);
        }
        cout << "Vocabulary loaded!" << endl << endl;

        //Create KeyFrame Database
        mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

        cout << "Load File" << endl;

        // Load the file with an earlier session
        //clock_t start = clock();
        cout << "Initialization of Atlas from file: " << mStrLoadAtlasFromFile << endl;
         加载地图
        bool isRead = LoadAtlas(FileType::BINARY_FILE);

        if(!isRead)
        {
            cout << "Error to load the file, please try with other session file or vocabulary file" << endl;
            exit(-1);
        }
        //mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);


        //cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " << mpKeyFrameDatabase->mnNumWords << endl;

        loadedAtlas = true;
        //如果选择杶定位模式 先用我们写的方式加载地图
        // mode2 pure location
        if(mode2)
        {
            mpAtlas->LoadMap();
        }

        else
            mpAtlas->CreateNewMap();

        //clock_t timeElapsed = clock() - start;
        //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000);
        //cout << "Binary file read in " << msElapsed << " ms" << endl;

        //usleep(10*1000*1000);
    }

We have a priori map (mStrLoadAtlasFromFile is not empty), so we use the else statement.

1. We first read the ORB dictionary

2. Create a keyframe database using ORB dictionary

3. Load the map

This is our highlight this time, let’s see how to load the map:

/**
 * @brief 加载地图
 * @param type 保存类型
 */
bool System::LoadAtlas(int type)
{
     1. 加载地图文件 这里的pathLoadFileName就是 ./ + akm + .osa
    // /home/liuhongwei/Desktop/final/catkin_nav/src/akm_nav/thirdparty/ORB_SLAM3_relocation_nav/akm.osa
    string strFileVoc, strVocChecksum;
    bool isRead = false;

    string pathLoadFileName = "./";
    pathLoadFileName = pathLoadFileName.append(mStrLoadAtlasFromFile);
    pathLoadFileName = pathLoadFileName.append(".osa");

    if(type == TEXT_FILE) // File text
    {
        cout << "Starting to read the save text file " << endl;
        std::ifstream ifs(pathLoadFileName, std::ios::binary);
        if(!ifs.good())
        {
            cout << "Load file not found" << endl;
            return false;
        }
        // strVocabularyName
        boost::archive::text_iarchive ia(ifs);
        ia >> strFileVoc;
        ia >> strVocChecksum;
        ia >> mpAtlas;
        cout << "End to load the save text file " << endl;
        isRead = true;
    }
    // 这里我们是二值地图
    else if(type == BINARY_FILE) // File binary
    {
        cout << "Starting to read the save binary file"  << endl;
        // 这段代码是用 C++ 语言中的 ifstream 类来创建一个文件输入流对象。
        // 它打开一个文件以供读取，并使用 `std::ios::binary` 标志表示以二进制模式打开文件，而不是文本模式。
        // 在这个例子中，`pathLoadFileName` 是文件的路径和名称。
        std::ifstream ifs(pathLoadFileName, std::ios::binary);

        //这段代码首先检查文件输入流 ifs 是否成功打开并准备好进行读取。
        // 如果文件未能成功打开（即 !ifs.good()），它会输出错误信息 "Load file not found"，然后返回 false，表示加载文件失败。
        if(!ifs.good())
        {
            cout << "Load file not found" << endl;
            return false;
        }
        // 如果文件成功打开，它使用 Boost 库中的 boost::archive::binary_iarchive 对象 ia 对打开的文件进行反序列化操作。
        boost::archive::binary_iarchive ia(ifs);
        // 通过 ia 从文件中读取了 strFileVoc、strVocChecksum 和 mpAtlas。
        ia >> strFileVoc;
        ia >> strVocChecksum;
        ia >> mpAtlas;
        cout << "End to load the save binary file" << endl;
        isRead = true;
    }

     2. 如果加载成功
    if(isRead)
    {
        //Check if the vocabulary is the same
        // 校验词典是否一样
        string strInputVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE);

        if(strInputVocabularyChecksum.compare(strVocChecksum) != 0)
        {
            cout << "The vocabulary load isn't the same which the load session was created " << endl;
            cout << "-Vocabulary name: " << strFileVoc << endl;
            return false; // Both are differents
        }

        // 加载对应数据
        mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase);
        mpAtlas->SetORBVocabulary(mpVocabulary);
        mpAtlas->PostLoad();

        return true;
    }
    return false;
}

1. First we read the binary file we saved:

pathLoadFileName = ./akm.osa

2. Read the map from the binary file:

Let’s recall the process of storing maps:

We store the location of the ORB dictionary, checksum checksum, and multi-map database.

Therefore, we also read in these three orders when reading.

This data structure contains the following contents:

It is a pointer that stores all maps and keyframes (Map structure that stores the pointers to all KeyFrames and MapPoints.)

3. If the reading is successful, we set the keyframe database to the keyframe database initialized in System.cc, and the ORB dictionary to the ORB dictionary initialized in System.cc.

Execute the loading data function mpAtlas->PostLoad(); Description in Sections 1.2-1.5

If the execution is successful, then we have restored all the maps and start the next step.

1.2 Load each map Atlas::PostLoad

/**
 * @brief 后加载，意思是读取地图文件后加载各个信息
 */
void Atlas::PostLoad()
{
     1. 读取当前所有相机
    map<unsigned int, GeometricCamera *> mpCams;
    for (GeometricCamera *pCam : mvpCameras)
    {
        mpCams[pCam->GetId()] = pCam;
    }

    mspMaps.clear();
    unsigned long int numKF = 0, numMP = 0;
     2. 加载各个地图
    for (Map *pMi : mvpBackupMaps)
    {
        mspMaps.insert(pMi);
        pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams);
        numKF += pMi->GetAllKeyFrames().size();
        numMP += pMi->GetAllMapPoints().size();
    }
    //mvpBackupMaps.clear();
}

Let’s first look at the variables read into the Atlas class:

protected:

    // 地图集合
    std::set<Map*> mspMaps;
    // 坏的地图集合
    std::set<Map*> mspBadMaps;
    // Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer
    // 备份地图集合
    std::vector<Map*> mvpBackupMaps;

    // 当前地图
    Map* mpCurrentMap;

    // 相机模型
    std::vector<GeometricCamera*> mvpCameras;

    unsigned long int mnLastInitKFidMap;

    Viewer* mpViewer;
    bool mHasViewer;

    // Class references for the map reconstruction from the save file
    KeyFrameDatabase* mpKeyFrameDB;
    ORBVocabulary* mpORBVocabulary;

    // Mutex
    std::mutex mMutexAtlas;

1. First read all camera models into the variable map<unsigned int, GeometricCamera *> mpCams. (The camera number, the corresponding camera abstract model)

2. Load the map from the backup mapmvpBackupMaps:

Store the map into the map data structuremspMaps (save the data structure of all maps), use numKF and numMP to obtain the key of each map Number of frames and map points. (The debug information seems to indicate that these two temporary variables are of no use).

1.3 Load keyframes and map points Map::PostLoad

Our function is called once to restore each map, so it restores the key frames and map points of a map.

    for (Map *pMi : mvpBackupMaps)
    {
        mspMaps.insert(pMi);
        pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams);
        numKF += pMi->GetAllKeyFrames().size();
        numMP += pMi->GetAllMapPoints().size();
    }

Let’s take a look at the code:

/** 后加载，也就是把备份的变量恢复到正常变量中
 * @param spCams 相机
 */
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()));

     1. 恢复map中的地图点，注意此时mp中只恢复了保存的量
    map<long unsigned int, MapPoint *> mpMapPointId;
    for (MapPoint *pMPi : mspMapPoints)
    {
        if (!pMPi || pMPi->isBad())
            continue;

        pMPi->UpdateMap(this);
        mpMapPointId[pMPi->mnId] = pMPi;
    }

     2. 恢复map中的kf，注意此时kf中只恢复了保存的量
    map<long unsigned int, KeyFrame *> mpKeyFrameId;
    for (KeyFrame *pKFi : mspKeyFrames)
    {
        if (!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateMap(this);
        pKFi->SetORBVocabulary(pORBVoc);
        pKFi->SetKeyFrameDatabase(pKFDB);
        mpKeyFrameId[pKFi->mnId] = pKFi;
    }

    // References reconstruction between different instances
     3. 使用mp中的备份变量恢复正常变量
    for (MapPoint *pMPi : mspMapPoints)
    {
        if (!pMPi || pMPi->isBad())
            continue;

        pMPi->PostLoad(mpKeyFrameId, mpMapPointId);
    }

     4. 使用kf中的备份变量恢复正常变量
    for (KeyFrame *pKFi : mspKeyFrames)
    {
        if (!pKFi || pKFi->isBad())
            continue;

        pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);
        pKFDB->add(pKFi);
    }

     5. 恢复ID
    if (mnBackupKFinitialID != -1)
    {
        mpKFinitial = mpKeyFrameId[mnBackupKFinitialID];
    }

    if (mnBackupKFlowerID != -1)
    {
        mpKFlowerID = mpKeyFrameId[mnBackupKFlowerID];
    }

    mvpKeyFrameOrigins.clear();
    mvpKeyFrameOrigins.reserve(mvBackupKeyFrameOriginsId.size());
    for (int i = 0; i < mvBackupKeyFrameOriginsId.size(); ++i)
    {
        mvpKeyFrameOrigins.push_back(mpKeyFrameId[mvBackupKeyFrameOriginsId[i]]);
    }

    mvpBackupMapPoints.clear();
}

1. First backup all key framesmvpBackupKeyFrames

Information and all map pointsmvpBackupMapPoints are extracted, mspMapPoints, mspKeyFrames stores all map points and key frames in the running state.

2. Cacheallmap pointsmspMapPoints: use mpMapPointId Save the data structure (map point ID, map point pointer) and update this map point to be visible on this map.

     1. 恢复map中的地图点，注意此时mp中只恢复了保存的量
    map<long unsigned int, MapPoint *> mpMapPointId;
    for (MapPoint *pMPi : mspMapPoints)
    {
        if (!pMPi || pMPi->isBad())
            continue;

        pMPi->UpdateMap(this);
        mpMapPointId[pMPi->mnId] = pMPi;
    }

void MapPoint::UpdateMap(Map* pMap)
{
    unique_lock<mutex> lock(mMutexMap);
    mpMap = pMap;
}

3. Cacheall keyframes: usempKeyFrameId Data structures are saved (frame ID, frame pointer).

     2. 恢复map中的kf，注意此时kf中只恢复了保存的量
    map<long unsigned int, KeyFrame *> mpKeyFrameId;
    for (KeyFrame *pKFi : mspKeyFrames)
    {
        if (!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateMap(this);
        pKFi->SetORBVocabulary(pORBVoc);
        pKFi->SetKeyFrameDatabase(pKFDB);
        mpKeyFrameId[pKFi->mnId] = pKFi;
    }

Restore the map’s observation of this keyframe:

void KeyFrame::UpdateMap(Map *pMap)
{
    unique_lock<mutex> lock(mMutexMap);
    mpMap = pMap;
}

Set the ORB dictionary of the frame (must be set, available in the constructor of each frame)

void KeyFrame::SetORBVocabulary(ORBVocabulary *pORBVoc)
{
    mpORBvocabulary = pORBVoc;
}

Set the keyframe database for this frame:

void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB)
{
    mpKeyFrameDB = pKFDB;
}

4. RestoreLocal mapMap point information: Detailed introduction in Section 1.4

     3. 使用mp中的备份变量恢复正常变量
    for (MapPoint *pMPi : mspMapPoints)
    {
        if (!pMPi || pMPi->isBad())
            continue;

        pMPi->PostLoad(mpKeyFrameId, mpMapPointId);
    }

5. RestoreLocal mapKeyframe information: Detailed introduction in Section 1.5

     4. 使用kf中的备份变量恢复正常变量
    for (KeyFrame *pKFi : mspKeyFrames)
    {
        if (!pKFi || pKFi->isBad())
            continue;

        pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);
        pKFDB->add(pKFi);
    }

1.4 Restore map point information MapPoint::PostLoad

/**
 * @brief 后加载
 */
void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid)
{
    // 1. 根据保存的ID加载参考关键帧与替代点
    mpRefKF = mpKFid[mBackupRefKFId];
    if(!mpRefKF)
    {
        cout << "ERROR: MP without KF reference " << mBackupRefKFId << "; Num obs: " << nObs << endl;
    }
    mpReplaced = static_cast<MapPoint*>(NULL);
    if(mBackupReplacedId>=0)
    {
        map<long unsigned int, MapPoint*>::iterator it = mpMPid.find(mBackupReplacedId);
        if (it != mpMPid.end())
            mpReplaced = it->second;
    }

    // 2. 加载观测
    mObservations.clear();

    for(map<long unsigned int, int>::const_iterator it = mBackupObservationsId1.begin(), end = mBackupObservationsId1.end(); it != end; ++it)
    {
        KeyFrame* pKFi = mpKFid[it->first];
        map<long unsigned int, int>::const_iterator it2 = mBackupObservationsId2.find(it->first);
        std::tuple<int, int> indexes = tuple<int,int>(it->second,it2->second);
        if(pKFi)
        {
           mObservations[pKFi] = indexes;
        }
    }

    mBackupObservationsId1.clear();
    mBackupObservationsId2.clear();
}

1. First restore the reference keyframes and alternative points of the map points: Recall the ORB-SLAM2/3 algorithm, the reference keyframe of a map is the keyframe that established it.

2. Load observations: mObservations stores which frame the map point can be observed and what the sequence number is in that frame.

1.5 Restore key frame information KeyFrame::PostLoad

The main reason is to restore key frames, map points, and map connections. Here are explained in the previous blog and no longer explain:

void KeyFrame::PostLoad(map<long unsigned int, KeyFrame *> &mpKFid, map<long unsigned int, MapPoint *> &mpMPid, map<unsigned int, GeometricCamera *> &mpCamId)
{
    // Rebuild the empty variables

     1.恢复关键帧位姿
    SetPose(mTcw);

    mTrl = mTlr.inverse();

    // Reference reconstruction
     2.恢复关键帧地图点
    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];
    }
    else
    {
        cout << "ERROR: There is not a main camera in KF " << mnId << endl;
    }
    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();
}