1.ORB-SLAM3中如何保存多地图、关键帧、地图点到二进制文件中

1 保存多地图

1.1 为什么保存(视觉)地图

        因为我们要去做导航，导航需要先验地图。因此需要保存地图供导航使用，下面来为大家讲解如何保存多地图。

 1.2 保存多地图的主函数SaveAtlas

2051

        mStrSaveAtlasToFile是配置文件中传递的参数：

        这里我们赋值成了akm（string mStrSaveAtlasToFile;）。

        即我们如果不传入mStrSaveAtlasToFile这个参数的话，就不会保存地图。

        我们执行预保存的代码 1.2.1 - 1.2.4节有详细的推导。

        我们确定要保存的文件名字：mStrSaveAtlasToFile（akm） + .osa即akm.osa

        保存词典的校验结果及名字

 3. 保存词典的校验结果及名字
// 计算给定文件路径 mStrVocabularyFilePath 对应文件的校验和或者哈希值，并将结果保存在 strVocabularyChecksum 变量中。
string strVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE);
// 找到了 mStrVocabularyFilePath 中最后一个目录分隔符（/ 或者 \）的位置，并将其索引保存在 found 变量中。这个操作通常用于从文件路径中提取文件名。
std::size_t found = mStrVocabularyFilePath.find_last_of("/\\");
// 这行代码利用之前找到的目录分隔符的位置 found，从 mStrVocabularyFilePath 中提取文件名部分，并将提取的文件名存储在 strVocabularyName 变量中。
string strVocabularyName = mStrVocabularyFilePath.substr(found+1);

        方便后续的读取。

        我们用二进制保存地图：

         4.用txt保存 / 用二进制保存
        if(type == TEXT_FILE)
        {
            cout << "Starting to write the save text file " << endl;
            std::remove(pathSaveFileName.c_str());
            std::ofstream ofs(pathSaveFileName, std::ios::binary);
            boost::archive::text_oarchive oa(ofs);

            oa << strVocabularyName;
            oa << strVocabularyChecksum;
            oa << mpAtlas;
            cout << "End to write the save text file" << endl;
        }
        else if(type == BINARY_FILE) // File binary
        {
            cout << "Starting to write the save binary file" << endl;
            std::remove(pathSaveFileName.c_str());
            std::ofstream ofs(pathSaveFileName, std::ios::binary);
            boost::archive::binary_oarchive oa(ofs);
            oa << strVocabularyName;
            oa << strVocabularyChecksum;
            oa << mpAtlas;
            cout << "End to write save binary file" << endl;
        }
    }

        在1.2.1-1.2.4中我们已经准备好了所有的保存变量（含Backup的变量）

        至此地图被保存：

1.2.1 预保存想要保存的数据Atlas::PreSave

/**
 * @brief 预保存，意思是在保存成地图文件之前，要保存到对应变量里面
 */
void Atlas::PreSave()
{
     1. 更新mnLastInitKFidMap
    // mpCurrentMap是当前的地图 Map *Atlas::mpCurrentMap
    if (mpCurrentMap)
    {
        // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1
        // mspMaps保存了每一个地图
        // 如果地图集不为空且 前地图创建时第1个关键帧的id小于当前地图的最大关键帧
        // 言外之意就是当前地图的关键帧的数量大于1 更新mnLastInitKFidMap为下一个关键帧的索引
        if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; 
    }

    // 如果 elem1 所指向的 Map 对象的 Id 小于 elem2 所指向的 Map 对象的 Id，则返回 true，
    // 表示 elem1 应该排在 elem2 前面；否则返回 false，表示 elem2 应该排在 elem1 前面或它们相等。
    struct compFunctor
    {
        inline bool operator()(Map *elem1, Map *elem2)
        {
            return elem1->GetId() < elem2->GetId();
        }
    };
    
    // vector

        我们要先明白几个变量的含义：

        1.mpCurrentMap是当前的地图 Map *Atlas::mpCurrentMap

        2.mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1

        3.mspMaps保存了所有地图：set

        if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1;

        即如果地图集不为空 且 前地图创建时第1个关键帧的id小于当前地图的最大关键帧则更新mnLastInitKFidMap为下一个关键帧的索引。那么也就是说如果多地图系统中有地图且当前地图的关键帧的数量大于1 更新mnLastInitKFidMap为下一个关键帧的索引。

    struct compFunctor
    {
        inline bool operator()(Map *elem1, Map *elem2)
        {
            return elem1->GetId() < elem2->GetId();
        }
    };

        如果 elem1 所指向的 Map 对象的 Id 小于 elem2 所指向的 Map 对象的 Id，则返回 true，表示 elem1 应该排在 elem2 前面；否则返回 false，表示 elem2 应该排在 elem1 前面或它们相等。

    std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps));
    sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());

        因此这两行的代码的含义就是将mspMaps（set

        随后我们遍历每一个地图，执行每张地图的预保存：

     3. 遍历所有地图，执行每个地图的预保存
    for (Map *pMi : mvpBackupMaps)
    {
        if (!pMi || pMi->IsBad())
            continue;

        // 如果地图为空，则跳过
        if (pMi->GetAllKeyFrames().size() == 0)
        {
            // Empty map, erase before of save it.
            SetMapBad(pMi);
            continue;
        }
        pMi->PreSave(spCams);
    }

        如果地图是不好的我们处理下一张地图。

        如果地图关键帧为空的话，我们去将这个地图设置为不好的。       

void Atlas::SetMapBad(Map *pMap)
{
    mspMaps.erase(pMap);
    pMap->SetBad();

    mspBadMaps.insert(pMap);
}

        我们在地图集删去这张地图并且在坏地图中加入这张地图。

        如果这张地图有关键帧存在，那么我们继续执行预保存代码。

1.2.2 预保存地图 Map::PreSave

/** 预保存，也就是把想保存的信息保存到备份的变量中
 * @param spCams 相机
 */
void Map::PreSave(std::set &spCams)
{
    int nMPWithoutObs = 0;  // 统计用

     1. 剔除一下无效观测
    // 遍历每一个地图点
    for (MapPoint *pMPi : mspMapPoints)
    {
        // 地图点是坏的（优化线程）
        if (!pMPi || pMPi->isBad())
            continue;

        // 如果这个地图点没有被观测到
        if (pMPi->GetObservations().size() == 0)
        {
            nMPWithoutObs++;
        }
        // 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引
        // it->first 观测到地图点的关键帧
        // 观测到地图点的关键帧不存在 或 观测到地图点的地图不是当前地图 或 观测到地图点的关键帧是被优化掉的关键帧
        // 也就是说 这里去除这个地图点在别的地图的观测以及在坏掉的关键帧的观测
        map> mpObs = pMPi->GetObservations();
        for (map>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it)
        {
            if (!it->first || it->first->GetMap() != this || it->first->isBad())
            {
                // 删除某个关键帧对当前地图点的观测
                pMPi->EraseObservation(it->first, false);
            }
        }
    }

    // Saves the id of KF origins
     2. 保存最开始的帧的id，貌似一个map的mvpKeyFrameOrigins里面只有一个，可以验证一下
    // vector Map::mvBackupKeyFrameOriginsId
    mvBackupKeyFrameOriginsId.clear();
    mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size());
    for (int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i)
    {
        mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId);
    }

    // Backup of MapPoints
     3. 保存一下对应的mp
    mvpBackupMapPoints.clear();
    for (MapPoint *pMPi : mspMapPoints)
    {
        if (!pMPi || pMPi->isBad())
            continue;

        mvpBackupMapPoints.push_back(pMPi);
        pMPi->PreSave(mspKeyFrames, mspMapPoints);
    }

    // Backup of KeyFrames
     4. 保存一下对应的KF
    mvpBackupKeyFrames.clear();
    for (KeyFrame *pKFi : mspKeyFrames)
    {
        if (!pKFi || pKFi->isBad())
            continue;

        mvpBackupKeyFrames.push_back(pKFi);
        pKFi->PreSave(mspKeyFrames, mspMapPoints, spCams);
    }

     保存一些id
    mnBackupKFinitialID = -1;
    if (mpKFinitial)
    {
        mnBackupKFinitialID = mpKFinitial->mnId;
    }

    mnBackupKFlowerID = -1;
    if (mpKFlowerID)
    {
        mnBackupKFlowerID = mpKFlowerID->mnId;
    }
}

        在这里我们预保存每张地图。

        1.遍历此张地图的所有地图点，先剔除一下无效观测：当观测到地图点的关键帧不存在 或 观测到地图点的地图不是当前地图 或 观测到地图点的关键帧是被优化掉的关键帧时，我们将这个地图点在相应帧的观测删除。也就是说这里去除这个地图点在别的地图的观测以及在坏掉的关键帧的观测。

        2.第二步保存这张最开始的帧的id 保存到变量mvBackupKeyFrameOriginsId中。

        3.第三步再次遍历这个地图的所有地图点，将所有地图点保存到mvpBackupMapPoints中。        

        4.第四步预保存地图点。

        5.第五步预保存关键帧。

1.2.3 预保存地图点 MapPoint::PreSave

/**
 * @brief 预保存
 * @param spKF 地图中所有关键帧
 * @param spMP 地图中所有三维点
 */
void MapPoint::PreSave(set& spKF,set& spMP)
{
    // 1. 备份替代的MP id
    mBackupReplacedId = -1;
    if(mpReplaced && spMP.find(mpReplaced) != spMP.end())
        mBackupReplacedId = mpReplaced->mnId;

    // 2. 备份观测
    mBackupObservationsId1.clear();
    mBackupObservationsId2.clear();
    // Save the id and position in each KF who view it
    std::vector erase_kfs;
    for(std::map >::const_iterator it = mObservations.begin(), 
        end = mObservations.end(); it != end; ++it)
    {
        KeyFrame* pKFi = it->first;
        if(spKF.find(pKFi) != spKF.end())
        {
            mBackupObservationsId1[it->first->mnId] = get<0>(it->second);
            mBackupObservationsId2[it->first->mnId] = get<1>(it->second);
        }
        else
        {
            erase_kfs.push_back(pKFi); 
        }
    }
    for (auto pKFi : erase_kfs)
        EraseObservation(pKFi, false);
    // Save the id of the reference KF
    // 3. 备份参考关键帧ID
    if(spKF.find(mpRefKF) != spKF.end())
    {
        mBackupRefKFId = mpRefKF->mnId;
    }
}

        1.备份替换的地图点：如果这个地图点被替换了，我们在地图点集合spMP中寻找这个地图点所对应的ID赋值给mBackupReplacedId。

        2.备份预测：mObservations存放所有地图点的预测（被哪一个关键帧观测到 + 在该帧特征点的索引），因此遍历该地图点可以看到的所有关键帧。

        如果这个关键帧存在（没有在优化节点被优化）：备份地图点的观测到mBackupObservationsId1、mBackupObservationsId2变量中。（map MapPoint::mBackupObservationsId1）

        如果这个关键帧不存在，删除掉关键帧对地图点的观测。

        3.备份第一次观测到该地图点的关键帧的ID：mBackupRefKFId。

1.2.4 预保存关键帧KeyFrame::PreSave

void KeyFrame::PreSave(set &spKF, set &spMP, set &spCam)
{
     1.预保存地图点的ID
    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);
    }
    
     2.预保存本质图
    mBackupConnectedKeyFrameIdWeights.clear();
    for (std::map::const_iterator it = mConnectedKeyFrameWeights.begin(), end = mConnectedKeyFrameWeights.end(); it != end; ++it)
    {
        if (spKF.find(it->first) != spKF.end())
            mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second;
    }

     3.预保存父母节点的ID
    mBackupParentId = -1;
    if (mpParent && spKF.find(mpParent) != spKF.end())
        mBackupParentId = mpParent->mnId;

     4.预保存孩子节点的ID
    mvBackupChildrensId.clear();
    mvBackupChildrensId.reserve(mspChildrens.size());
    for (KeyFrame *pKFi : mspChildrens)
    {
        if (spKF.find(pKFi) != spKF.end())
            mvBackupChildrensId.push_back(pKFi->mnId);
    }

     5.预保存回环节点的ID
    mvBackupLoopEdgesId.clear();
    mvBackupLoopEdgesId.reserve(mspLoopEdges.size());
    for (KeyFrame *pKFi : mspLoopEdges)
    {
        if (spKF.find(pKFi) != spKF.end())
            mvBackupLoopEdgesId.push_back(pKFi->mnId);
    }

     6.mspMergeEdges
    mvBackupMergeEdgesId.clear();
    mvBackupMergeEdgesId.reserve(mspMergeEdges.size());
    for (KeyFrame *pKFi : mspMergeEdges)
    {
        if (spKF.find(pKFi) != spKF.end())
            mvBackupMergeEdgesId.push_back(pKFi->mnId);
    }

     7.预保存相机信息
    mnBackupIdCamera = -1;
    if (mpCamera && spCam.find(mpCamera) != spCam.end())
        mnBackupIdCamera = mpCamera->GetId();

    mnBackupIdCamera2 = -1;
    if (mpCamera2 && spCam.find(mpCamera2) != spCam.end())
        mnBackupIdCamera2 = mpCamera2->GetId();

     8.预保存IMU信息
    mBackupPrevKFId = -1;
    if (mPrevKF && spKF.find(mPrevKF) != spKF.end())
        mBackupPrevKFId = mPrevKF->mnId;

    mBackupNextKFId = -1;
    if (mNextKF && spKF.find(mNextKF) != spKF.end())
        mBackupNextKFId = mNextKF->mnId;

    if (mpImuPreintegrated)
        mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated);
}