ORB-SLAM3源码的学习：Atlas.cc②: Atlas:: CreateNewMap创建新地图

前言

简单总结一下地图是何时创建的：

构建slam系统时还没有地图就需要创建，当时间戳不对劲时影响数据的同步时需要创建，当跟踪的第一和第二阶段都为失败时都要分别创建，且满足一定要求的地图会保留作为非活跃地图。

1.创建新地图

1.存在当前活跃地图，则先将当前活跃地图标记为非活跃地图。 

2.新建地图，将新地图标记为活跃地图。 

3.将新的活跃地图插入地图集中。 

函数 

注意：将当前地图存储起来，在代码中并未真的进行存储而是把mIsInUse标记为false即将地图设置为非活跃地图，此时的这个非活跃地图仍然在地图集中。

/**
 * @brief 创建新地图，如果当前活跃地图有效，先存储当前地图为不活跃地图，然后新建地图；否则，可以直接新建地图。
 * 
 */
void Atlas::CreateNewMap()
{
    // 锁住地图集
    unique_lock<mutex> lock(mMutexAtlas);//使用互斥锁来保护地图集的线程安全。
    cout << "Creation of new map with id: " << Map::nNextId << endl;//打印新地图创建信息,显示了新地图的ID。
    // 如果当前活跃地图有效，先存储当前地图为不活跃地图后退出
    if (mpCurrentMap)
    {
        // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1
        if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // 初始化关键帧ID为当前最大ID的下一个

        // 将当前地图储存起来，其实就是把mIsInUse标记为false
        mpCurrentMap->SetStoredMap();
        cout << "Stored map with ID: " << mpCurrentMap->GetId() << endl;

        // if(mHasViewer)
        //     mpViewer->AddMapToCreateThumbnail(mpCurrentMap);
    }
    cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;

    mpCurrentMap = new Map(mnLastInitKFidMap);  //新建地图
    mpCurrentMap->SetCurrentMap();              //设置为活跃地图
    mspMaps.insert(mpCurrentMap);               //插入地图集
}

2.创建地图的时机

创建太频繁会影响效率，创建太保守影响效果。 

1.构建SLAM系统时 

在构建地图集时，地图集是空的，这时候需要创建第一个地图。 在system.cc中：

mpAtlas = new Atlas(0);

Atlas类的构造函数则是在Atlas.cc中：

Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid), mHasViewer(false)
{
    mpCurrentMap = static_cast<Map *>(NULL);
    CreateNewMap();
}

2.跟踪线程中时间戳异常时 

ORB-SLAM2 不需要像 ORB-SLAM3 那样专门处理时间戳异常问题，主要是因为它面向的是较为简单的单传感器或双目传感器配置，时间戳同步的问题相对较少。而 ORB-SLAM3 由于支持更复杂的传感器配置（如 IMU、多相机等），因此需要更加精确地处理时间戳同步，以确保不同传感器数据的协调和正确的融合。

异常的两种情况： 

1.时间戳颠倒。当前帧的时间戳比上一帧的时间戳小。 

2.时间戳跳变。当前帧的时间戳与上一帧的时间戳间隔时间比较久（大于1s） 

// Step 2 处理时间戳异常的情况
    if(mState!=NO_IMAGES_YET)
    {
        if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)
        {
            // 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图
            cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
            unique_lock<mutex> lock(mMutexImuQueue);
            // mlQueueImuData.clear();
            // 创建新地图
            CreateMapInAtlas();
            return;
        }
        else if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0)
        {
            // cout << mCurrentFrame.mTimeStamp << ", " << mLastFrame.mTimeStamp << endl;
            // cout << "id last: " << mLastFrame.mnId << "    id curr: " << mCurrentFrame.mnId << endl;
            // 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回
            //根据是否是imu模式,进行imu的补偿
            if(mpAtlas->isInertial())
            {
                // 如果当前地图imu成功初始化
                if(mpAtlas->isImuInitialized())
                {
                    cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
                    // IMU完成第3次初始化（在localmapping线程里）
                    if(!pCurrentMap->GetIniertialBA2())
                    {
                        // 如果当前子图中imu没有经过BA2，重置active地图，也就是之前的数据不要了
                        mpSystem->ResetActiveMap();
                    }
                    else
                    {
                        // 如果当前子图中imu进行了BA2，重新创建新的子图，保存当前地图
                        CreateMapInAtlas();
                    }
                }
                else
                {
                    // 如果当前子图中imu还没有初始化，重置active地图
                    cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
                    mpSystem->ResetActiveMap();
                }
                return;
            }

        }
    }

其中调用了CreateMapInAtlas函数不仅创建了新地图还重置了系统状态以便重新获取新数据。

具体代码分析：

1.重置初始化帧 ID

mnLastInitFrameId = mCurrentFrame.mnId;

 将当前帧的 ID 记录到mnLastInitFrameId中，标记当前帧作为新地图的起始帧。

2. 创建新地图

调用CreateNewMap函数创建新地图，此时该地图为活跃地图，并将其插入到地图集中。

mpAtlas->CreateNewMap();

3.设置惯性传感器标志

if (mSensor == System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD)
    mpAtlas->SetInertialSensor();  // mpAtlas中map的mbIsInertial=true

如果是IMU的模式是需要包含惯性数据的。

4. 重置初始化标志

mbSetInit = false;  // 好像没什么用

避免后续的重复初始化

5. 设置新地图的初始化帧 ID

mnInitialFrameId = mCurrentFrame.mnId + 1;

6.重置系统状态

mState = NO_IMAGES_YET;

7.重置其他相关标志

mbVelocity = false; 

8.打印信息

Verbose::PrintMess("First frame id in map: " + to_string(mnLastInitFrameId + 1), Verbose::VERBOSITY_NORMAL);

 打印日志信息，告知当前新地图中的第一帧 ID。这有助于调试和查看系统的状态。

9.初始化 VO 标志

mbVO = false; // Init value for know if there are enough MapPoints in the last KF

将 mbVO（Visual Odometry）标志设置为 false，表示视觉里程计的状态尚未建立。mbVO 可能用于判断当前的地图中是否有足够的地图点（MapPoints）来支持视觉里程计的估计。

10. 重置单目初始化标志

if (mSensor == System::MONOCULAR || mSensor == System::IMU_MONOCULAR)
{
    mbReadyToInitializate = false;
}

如果当前系统使用的是单目传感器或单目 IMU，mbReadyToInitializate 将被重置为 false。这是因为在单目系统中，初始化过程需要根据多帧图像来恢复 3D 位姿，当前的帧在重新创建地图时可能还没有足够的信息来初始化系统。

11. 重新初始化 IMU 数据

if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpImuPreintegratedFromLastKF)
{
    delete mpImuPreintegratedFromLastKF;
    mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
}

12.重置关键帧指针

if (mpLastKeyFrame)
    mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

if (mpReferenceKF)
    mpReferenceKF = static_cast<KeyFrame*>(NULL);

13. 重置当前和上一帧

mLastFrame = Frame();
mCurrentFrame = Frame();

14.清空匹配点和 IMU 数据队列

mvIniMatches.clear();
mlQueueImuData.clear();

15. 标记地图已创建

mbCreatedMap = true;

完整的代码： 

/*
 在Atlas中保存当前地图，创建新地图，所有跟状态相关的变量全部重置
 1. 前后两帧对应的时间戳反了
 2. imu模式下前后帧超过1s
 3. 上一帧为最近丢失且重定位失败时
 4. 重定位成功，局部地图跟踪失败
*/
void Tracking::CreateMapInAtlas()
{
    mnLastInitFrameId = mCurrentFrame.mnId;
    mpAtlas->CreateNewMap();
    if (mSensor==System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD)
        mpAtlas->SetInertialSensor();  // mpAtlas中map的mbIsInertial=true
    mbSetInit=false;  // 好像没什么用

    mnInitialFrameId = mCurrentFrame.mnId+1;
    mState = NO_IMAGES_YET;

    // Restart the variable with information about the last KF
    mbVelocity = false;
    //mnLastRelocFrameId = mnLastInitFrameId; // The last relocation KF_id is the current id, because it is the new starting point for new map
    Verbose::PrintMess("First frame id in map: " + to_string(mnLastInitFrameId+1), Verbose::VERBOSITY_NORMAL);
    mbVO = false; // Init value for know if there are enough MapPoints in the last KF
    if(mSensor == System::MONOCULAR || mSensor == System::IMU_MONOCULAR)
    {
        mbReadyToInitializate = false;
    }

    if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpImuPreintegratedFromLastKF)
    {
        delete mpImuPreintegratedFromLastKF;
        mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
    }

    if(mpLastKeyFrame)
        mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

    if(mpReferenceKF)
        mpReferenceKF = static_cast<KeyFrame*>(NULL);

    mLastFrame = Frame();
    mCurrentFrame = Frame();
    mvIniMatches.clear();
    mlQueueImuData.clear();

    mbCreatedMap = true;
}

3.跟踪线程中确定跟踪丢失后

根据跟踪的阶段不同，判断条件也不同。如果在跟踪的第一阶段就确定跟踪丢失，则进行如下处理：

1.如果当前活跃地图中关键帧的数量小于10 个，则认为该地图中有效信息太少， 直接重置， 丢弃当前地图。

2.如果当前活跃地图中关键帧的数量超过10 个，则认为该地图仍有一定价值，存储起来作为非活跃地图， 然后新建一个地图。

else if (mState == LOST)  // 上一帧为最近丢失且重定位失败时
                {
                    // Step 6.6 如果是LOST状态
                    // 开启一个新地图
                    Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);

                    if (pCurrentMap->KeyFramesInMap()<10)
                    {
                        // 当前地图中关键帧数目小于10，重置当前地图
                        mpSystem->ResetActiveMap();
                        Verbose::PrintMess("Reseting current map...", Verbose::VERBOSITY_NORMAL);
                    }else
                        CreateMapInAtlas();  // 当前地图中关键帧数目超过10，创建新地图
                    // 干掉上一个关键帧
                    if(mpLastKeyFrame)
                        mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

                    Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);

                    return;
                }

如果在跟踪的第二阶段确定跟踪丢失，则进行如下处理：

1. 如果当前是纯视觉模式且地图中关键帧的数量超过10个或者在惯性模式下已经完成IMU 第一阶段初始化，则认为该地图仍有一定价值， 存储起来作为非活跃地图，然后新建一个地图。

2. 否则重置， 丢弃当前地图。

// Reset if the camera get lost soon after initialization
        // Step 10 如果第二阶段跟踪失败，跟踪状态为LOST
        if(mState==LOST)
        {
            // 如果地图中关键帧小于10，重置当前地图，退出当前跟踪
            if(pCurrentMap->KeyFramesInMap()<=10)  // 上一个版本这里是5
            {
                mpSystem->ResetActiveMap();
                return;
            }
            if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
                if (!pCurrentMap->isImuInitialized())
                {
                    // 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪
                    Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);
                    mpSystem->ResetActiveMap();
                    return;
                }
            // 如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图
            CreateMapInAtlas();

            // 新增加了个return
            return;
        }

 以上就是需要创建地图的几种情况。

结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。