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的基础上增加1if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // 初始化关键帧ID为当前最大ID的下一个// 将当前地图储存起来，其实就是把mIsInUse标记为falsempCurrentMap->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模式下前后帧超过1s3. 上一帧为最近丢失且重定位失败时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=truembSetInit=false;  // 好像没什么用mnInitialFrameId = mCurrentFrame.mnId+1;mState = NO_IMAGES_YET;// Restart the variable with information about the last KFmbVelocity = false;//mnLastRelocFrameId = mnLastInitFrameId; // The last relocation KF_id is the current id, because it is the new starting point for new mapVerbose::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 KFif(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);}elseCreateMapInAtlas();  // 当前地图中关键帧数目超过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 如果第二阶段跟踪失败，跟踪状态为LOSTif(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();// 新增加了个returnreturn;}

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

结束语

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