2.ORB-SLAM3中如何从二进制文件中加载多地图、关键帧、地图点等数据结构

目录

1 为什么保存&加载(视觉)地图

1.1 加载多地图的主函数

1.2 加载各个地图 Atlas::PostLoad

1.3 加载关键帧及地图点Map::PostLoad

1.4 恢复地图点信息 MapPoint::PostLoad

1.5 恢复关键帧信息KeyFrame::PostLoad

---

1 为什么保存&加载(视觉)地图

        因为我们要去做导航，导航需要先验地图。因此需要保存地图供导航使用。

        上篇博客我们讲解了如何保存地图数据，这是我们已经保存下来的文件。

        下面来为大家讲解如何加载地图数据。

1.1 加载多地图的主函数

        我们看System.cc文件中的代码：

    // 查看配置文件版本，不同版本有不同处理方法
    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;
        }
    }

        我们的配置文件版本时1.0版本。

mStrSaveAtlasToFile

        在setting构造函数中我们读取了下面的参数：

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

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

        因此，mStrLoadAtlasFromFile和mStrSaveAtlasToFile变量都是字符串'akm'。

        接着看这段：

    // 如果没有先验地图的话 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);
    }

        我们有先验地图（mStrLoadAtlasFromFile不为空），因此走else语句。

        1.我们首先读取ORB词典        

        2.利用ORB词典创建关键帧数据库

        3.加载地图

        这是我们这次的重头戏，看看如何加载地图：

/**
 * @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.首先我们读取我们保存的二进制文件：

        pathLoadFileName = ./akm.osa

        2.从二进制文件中读取地图：

        我们回忆一下存储地图的过程：

        我们存储了ORB词典的位置、checksum校验和以及多地图数据库。

        因此我们在读取时候也是按这三个顺序读取。

        这个数据结构包含以下内容：

        它就是一个指针保存了所有地图和关键帧（Map structure that stores the pointers to all KeyFrames and MapPoints.）

        3.如果读取成功，我们设置关键帧的数据库为System.cc中初始化的关键帧数据库，ORB词典为System.cc初始化的ORB词典。

        执行加载数据函数mpAtlas->PostLoad(); 1.2-1.5节说明

        如果执行成功，那么我们已经恢复了所有的地图了，开始下一步的工作

1.2 加载各个地图 Atlas::PostLoad

/**
 * @brief 后加载，意思是读取地图文件后加载各个信息
 */
void Atlas::PostLoad()
{
     1. 读取当前所有相机
    map 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();
}

        我们先来看读取到Atlas类中的变量：

protected:

    // 地图集合
    std::set mspMaps;
    // 坏的地图集合
    std::set 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 mvpBackupMaps;

    // 当前地图
    Map* mpCurrentMap;

    // 相机模型
    std::vector 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.首先把所有的相机模型读取到变量map mpCams里面。（第几个相机，对应的相机抽象模型）

        2.从备份地图mvpBackupMaps中加载地图：

        将地图存储进地图数据结构mspMaps（保存所有地图的数据结构），用numKF和numMP获取每个地图的关键帧和地图点数量。（dubug信息貌似，这两个临时变量没什么用呀）。

1.3 加载关键帧及地图点Map::PostLoad

        我们这个函数是恢复每个地图都被调用一次，因此是恢复一张地图的关键帧及地图点。

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

        我们来看代码：

/** 后加载，也就是把备份的变量恢复到正常变量中
 * @param spCams 相机
 */
void Map::PostLoad(KeyFrameDatabase *pKFDB, ORBVocabulary *pORBVoc /*, map& mpKeyFrameId*/, map &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 mpMapPointId;
    for (MapPoint *pMPi : mspMapPoints)
    {
        if (!pMPi || pMPi->isBad())
            continue;

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

     2. 恢复map中的kf，注意此时kf中只恢复了保存的量
    map 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.首先将备份的所有关键帧mvpBackupKeyFrames

信息和所有的地图点mvpBackupMapPoints提取出来，mspMapPoints、mspKeyFrames存储了运行状态的所有地图点和关键帧。

        2.缓存所有地图点mspMapPoints：用mpMapPointId数据结构保存（地图点ID、地图点的指针）并更新这个地图点对本地图可视。

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

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

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

        3.缓存所有关键帧：用mpKeyFrameId数据结构保存（帧ID、帧指针）。

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

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

        恢复地图对该关键帧的观测：

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

        设置该帧的ORB词典（必须设置的，每一帧的构造函数中有）

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

        设置该帧的关键帧数据库：

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

        4.恢复本地图地图点信息：1.4节有详细介绍

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

        pMPi->PostLoad(mpKeyFrameId, mpMapPointId);
    }

        5.恢复本地图关键帧信息：1.5节有详细介绍

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

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

1.4 恢复地图点信息 MapPoint::PostLoad

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

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

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

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

          1.首先恢复地图点的参考关键帧与替代点：回忆ORB-SLAM2/3算法，一个地图的参考关键帧是建立它的关键帧。

        2.加载观测：mObservations存储地图点可以被哪一帧观测，在该帧的序号是多少。

1.5 恢复关键帧信息KeyFrame::PostLoad

        主要是恢复关键帧位姿、地图点、图连接，这里在上篇博客说明了，不再阐述：

void KeyFrame::PostLoad(map &mpKFid, map &mpMPid, map &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(NULL);
    }

    // Conected KeyFrames with him weight
    mConnectedKeyFrameWeights.clear();
    for (map::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::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it != end; ++it)
    {
        mspChildrens.insert(mpKFid[*it]);
    }

    // Loop edge KeyFrame
    mspLoopEdges.clear();
    for (vector::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it)
    {
        mspLoopEdges.insert(mpKFid[*it]);
    }

    // Merge edge KeyFrame
    mspMergeEdges.clear();
    for (vector::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();
}