文章导读
ORB_SLAM3 相比于 ORB_SLAM2 ，新增了一个关键功能——地图保存。这一功能主要通过保存子地图 Atlas 来实现。这种设计允许 ORB_SLAM3 不仅创建和更新地图，还能保存和重新加载这些地图，使其可以在后续的定位和建图任务中使用。本文通过分享 ORB_SLAM3 中实现地图保存和加载功能的技术细节，帮助读者理解如何将已有地图保存下来，当下一次任务时，如何加载这个地图作为先验信息，从而提高系统的效率。
文章结构概览
- boost 库序列化原理
- ORB_SLAM3 地图序列化
- 整体框架
- ORB_SLAM3 中 7 类数据的序列化方式
- 反序列化的方式
- 在 ORB_SLAM2 上添加地图保存和加载功能（实践验证）
- 单例模式序列化的问题
boost 库序列化原理
有关 Boost 库序列化的使用这里就不再赘述，大家可以通过 4.4 C++ Boost 数据集序列化库 - lyshark - 博客园 或者其他资料学习。下面针对 Boost 的递归序列化的特性，结合 ORB_SLAM3 为案例来讲解：
Boost 库在序列化保存时会递归地对对象进行序列化，这就意味着如果一个对象的成员变量也是一个对象，那么 Boost 序列化库会查找并调用这个成员变量对象的序列化函数。
在 ORB_SLAM3 中，Atlas 类有一个成员变量 mvpBackupMaps ，它是一个 std::vector<Map*> 类型的对象。当 Atlas 对象被序列化时，Boost 序列化库会对 mvpBackupMaps 进行序列化。由于 mvpBackupMaps 是一个 std::vector<Map*>，Boost 序列化库会对 std::vector 中的每一个 Map* 进行序列化。对于每一个 Map*，Boost 序列化库会查找并调用 Map 类的序列化函数。在 Map 类的序列化函数中，又有一些成员变量需要被序列化，比如 mvpBackupKeyFrames 和 mvpBackupMapPoints 等。如果这些成员变量的类型也定义了序列化函数，那么 Boost 序列化库会继续递归地对这些成员变量进行序列化。
这个过程会一直持续下去，直到遇到基本类型的变量 (如 int、｀float、‘double 等) 或者没有定义序列化函数的自定义类型变量。对于基本类型的变量，Boost 序列化库会直接将它们的值写入到序列化的输出中。对于没有定义序列化函数的自定义类型变量，Boost 序列化库会报错。
Boost 序列化库的工作方式就像是一种深度优先搜索 (DFS)，它会递归地对对象的所有成员变量进行序列化，直到所有的变量都被序列化。这种方式可以确保对象的所有信息都被正确地保存和恢复，无论这个对象有多复杂。
ORB_SLAM3 地图序列化
整体框架
在了解了 boost 库对于序列化的实现以后，就可以深入 ORB_SLAM3 来了解它对于系统中的数据保存方式了。
在深入了解实现的细节前，首先先了解整体的框架，下图为 ORB_SLAM3 序列化保存与加载的整体框架（如有遗漏请评论私信纠正，十分感谢！）。
可以看到对于整个序列化保存，是先 PreSave 再进行 boost 库保存。其中 PreSave 正如其名，作用就是对数据的预处理（下面详细介绍），作用是将不合适 boost 保存的数据转换为合适的数据，然后再通过 boost 库保存。
而对于加载部分，当然是与保存的步骤相反，先进行 boost 库的加载，然后再通过 PostLoad 将数据转换为原始的在系统中的数据形式。
PreSave 和 PostLoad 的详细解释
预保存 PreSave 和 后加载 PostLoad（因为很多数据都是要保存的关键帧或者地图点的对象地址，而从文件恢复出来的地址会变化，所以要用 backup 数据把这些数据先保存为 ID 索引的形式，后面恢复完所有的地图点和关键帧了，有对应的对象地址了，再通过 Postload 后加载，通过保存的 ID 索引将地址恢复出来）
ORB_SLAM3 中 7 类数据的序列化方式
下面将 ORB_SLAM3 中要序列化的数据分为六大类型来讲解。
1. 基本数据类型的序列化（例如：long unsigned int mnId）
基本数据类型可以直接使用 ar & 进行序列化和反序列化。
ar & mnId; // 序列化ID
原因：基本数据类型在内存中的表示是标准的，所以可以直接读取和写入这些值。
2. 常量数据的序列化（例如：const long unsigned int mnFrameId）
常量数据需要使用 const_cast 来序列化。
ar & const_cast<long unsigned int&>(mnFrameId); // 序列化帧ID
原因：常量成员变量在对象构造后不能更改，但在反序列化过程中，你可能需要更改这些值以重建对象的状态。const_cast 允许你临时去除常量性，以便能够反序列化这些成员。
3. 自定义数据格式的序列化（例如：cv::Mat mDescriptor）
自定义数据格式，如 OpenCV 的矩阵，需要特殊处理。
//此函数定义在SerializationUtils.h文件中
// 序列化cv::Mat对象的模板函数
template<class Archive>
void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version)
{
int cols, rows, type; // 定义矩阵的列、行和类型
bool continuous; // 定义矩阵是否连续
// 如果是保存操作，则从矩阵中获取这些属性
if (Archive::is_saving::value) {
cols = mat.cols;
rows = mat.rows;
type = mat.type();
continuous = mat.isContinuous();
}
// 序列化列、行、类型和连续性
ar & cols & rows & type & continuous;
// 如果是加载操作，则使用这些属性创建矩阵
if (Archive::is_loading::value)
mat.create(rows, cols, type);
// 如果矩阵是连续的，则一次性序列化整个数据
if (continuous) {
const unsigned int data_size = rows * cols * mat.elemSize();
ar & boost::serialization::make_array(mat.ptr(), data_size);
} else {
// 如果矩阵不连续，则逐行序列化
const unsigned int row_size = cols * mat.elemSize();
for (int i = 0; i < rows; i++) {
ar & boost::serialization::make_array(mat.ptr(i), row_size);
}
}
}
// 序列化const cv::Mat对象的模板函数
template<class Archive>
void serializeMatrix(Archive& ar, const cv::Mat& mat, const unsigned int version)
{
cv::Mat matAux = mat; // 创建一个非const副本
serializeMatrix(ar, matAux, version); // 调用非const版本的序列化函数
// 如果是加载操作，则将加载的数据复制到原始const对象
if (Archive::is_loading::value)
{
cv::Mat* ptr;
ptr = (cv::Mat*)( &mat ); // 强制转换为非const指针
*ptr = matAux; // 将加载的数据复制到原始对象
}
}
原因：矩阵可能有不同的大小、类型和存储方式，所以需要自定义序列化方法来逐一处理这些属性。
4. 容器 + 自定义数据格式的序列化（例如：std::vector<cv::KeyPoint> mvKeys）
容器中包含自定义数据格式，如关键点向量，需要特殊处理。
//此函数定义在SerializationUtils.h文件中
// 序列化std::vector<cv::KeyPoint>对象的模板函数
template<class Archive>
void serializeVectorKeyPoints(Archive& ar, const std::vector<cv::KeyPoint>& vKP, const unsigned int version)
{
int NumEl; // 定义关键点的数量
// 如果是保存操作，则从向量中获取关键点的数量
if (Archive::is_saving::value) {
NumEl = vKP.size();
}
ar & NumEl; // 序列化关键点的数量
std::vector<cv::KeyPoint> vKPaux = vKP; // 创建一个非const副本
if (Archive::is_loading::value)
vKPaux.reserve(NumEl); // 如果是加载操作，则预留空间
// 遍历关键点
for(int i=0; i < NumEl; ++i)
{
cv::KeyPoint KPi; // 定义单个关键点
// 如果是加载操作，则创建一个新的关键点
if (Archive::is_loading::value)
KPi = cv::KeyPoint();
// 如果是保存操作，则从副本中获取关键点
if (Archive::is_saving::value)
KPi = vKPaux[i];
// 序列化关键点的各个属性
ar & KPi.angle;
ar & KPi.response;
ar & KPi.size;
ar & KPi.pt.x;
ar & KPi.pt.y;
ar & KPi.class_id;
ar & KPi.octave;
// 如果是加载操作，则将新关键点添加到副本中
if (Archive::is_loading::value)
vKPaux.push_back(KPi);
}
// 如果是加载操作，则将加载的数据复制到原始const对象
if (Archive::is_loading::value)
{
std::vector<cv::KeyPoint> *ptr;
ptr = (std::vector<cv::KeyPoint>*)( &vKP ); // 强制转换为非const指针
*ptr = vKPaux; // 将加载的数据复制到原始对象
}
}
原因：关键点包括多个属性，所以需要自定义序列化方法来逐一处理这些属性。
5. 使用 boost::serialization::make_array 序列化向量/矩阵（例如：Eigen::Vector3f mWorldPos）
连续的内存块可以使用 boost::serialization::make_array 进行序列化。
ar & boost::serialization::make_array(mWorldPos.data(), mWorldPos.size()); // 序列化世界位置
原因：连续的内存块可以一次性序列化整个块，比逐个序列化元素更高效。
6. 自定义 DBoW 数据格式的序列化（例如：DBoW2::BowVector mBowVec）
DBoW 数据格式需要在 DBoW 库源码中添加 serialize 函数。但是在 KeyFrame 中依旧是使用 ar & mBowVec,boost 库看到这个格式后会跳转到第三方库中看有没有对序列化进行定义。
//KeyFrame.h中：
ar & mBowVec; // 序列化Bow向量
//---------------------------------------------------------------------
//Thirdparty/DBoW2/DBoW2/BowVector.h中:
class BowVector:
public std::map<WordId, WordValue>
{
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const int version)
{
ar & boost::serialization::base_object<std::map<WordId, WordValue> >(*this);
}
public:
...
原因：DBoW 数据格式包括复杂的结构和属性，所以需要在 DBoW 库中添加特定的序列化方法。
7.带有关键帧或者地图点对象地址的数据要 PreSave（例如：std::vector<MapPoint*> mvpMapPoints）
这类数据是要保存的关键帧或者地图点的对象地址，而从文件恢复出来的对象地址会改变，所以要用 backup 数据把这些数据先保存为 ID 索引的形式 (也就是 PreSave 要做的事情)，后面恢复完所有的地图点和关键帧了，有对应的对象地址了，再通过 postload 后加载，通过保存的 ID 索引将地址恢复出来。
**流程：先 presave 成能直接序列化的基本数据类型（基本都是些地图点和关键帧 Id），然后通过 ar & xxx 序列化保存。
void KeyFrame::PreSave()
{
//保存帧内所有地图点
mvBackupMapPointsId.clear();
mvBackupMapPointsId.reserve(N);
for(int i = 0 ; i < N ; ++i)
{
if(mvpMapPoints[i])
mvBackupMapPointsId.push_back(mvpMapPoints[i]->mnId);
else
mvBackupMapPointsId.push_back(-1);
}
//保存连接的关键帧ID和权重
mBackupConnectedKeyFrameIdWeights.clear();
for(auto it = mConnectedKeyFrameWeights.begin(),end = mConnectedKeyFrameWeights.end();it != end ; ++it)
{
mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second;
}
//保存父关键帧ID
mBackupParentId = -1;
if (mpParent)
mBackupParentId = mpParent->mnId;
//保存子关键帧ID
mvBackupChildrensId.clear();
mvBackupChildrensId.reserve(mspChildrens.size());
for(KeyFrame* pKFi : mspChildrens)
{
mvBackupChildrensId.push_back(pKFi->mnId);
}
// //保存Frame的静态变量nNextid
// mFramenNextId = Frame::nNextId;
...
}
反序列化的方式
前六种情况的反序列化加载
在反序列化加载过程中，前六种情况无需人为操作，boost 库会自动根据 serialize 函数进行加载。但对于类对象的反序列化，情况会略有不同。
首先，如果你要恢复如 std::vector<KeyFrame*> mvpBackupKeyFrames 这样的类对象数据，必须为 boost 库提供一个默认的构造函数。这样做的原因是，boost 库需要使用这个默认构造函数来实例化类的对象。
例如，对于 mvpBackupKeyFrames 这个容器中的每一个 KeyFrame，boost 库会通过特定的关键帧默认构造函数来进行实例化。一旦实例化完毕，boost 库就会根据 KeyFrame.h 里的 serialize 函数来逐一恢复每个 KeyFrame 对象里保存了的数据。
KeyFrame::KeyFrame()
: mnFrameId(0), mTimeStamp(0), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
mfGridElementWidthInv(0), mfGridElementHeightInv(0),
mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), mnBALocalForMerge(0),
mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnMergeQuery(0), mnMergeWords(0), mnBAGlobalForKF(0),
fx(0), fy(0), cx(0), cy(0), invfx(0), invfy(0), mnPlaceRecognitionQuery(0), mnPlaceRecognitionWords(0), mPlaceRecognitionScore(0),
mbf(0), mb(0), mThDepth(0), N(0), mvKeys(static_cast<vector<cv::KeyPoint>>(NULL)), mvKeysUn(static_cast<vector<cv::KeyPoint>>(NULL)),
mvuRight(static_cast<vector<float>>(NULL)), mvDepth(static_cast<vector<float>>(NULL)), mnScaleLevels(0), mfScaleFactor(0),
mfLogScaleFactor(0), mvScaleFactors(0), mvLevelSigma2(0), mvInvLevelSigma2(0), mnMinX(0), mnMinY(0), mnMaxX(0),
mnMaxY(0), mPrevKF(static_cast<KeyFrame *>(NULL)), mNextKF(static_cast<KeyFrame *>(NULL)), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
mbToBeErased(false), mbBad(false), mHalfBaseline(0), mbCurrentPlaceRecognition(false), mnMergeCorrectedForKF(0),
NLeft(0), NRight(0), mnNumberOfOpt(0), mbHasVelocity(false)
{
}
对 PreSave 数据的反序列化加载
第七种情况处理的是具有预保存（presave）格式的数据，例如 mvBackupMapPointsId。相对于其他情况，这一情况的处理更为复杂，需要分为两个主要阶段来执行。
- 反序列化加载阶段：
- 目的：将之前以特定格式保存的数据重新转化为可用的数据结构。
- 过程：首先对数据进行反序列化加载。
- 后加载（PostLoad）处理阶段：
- 目的：确保数据结构的正确恢复，特别是关键帧或地图点的 ID 恢复为对应的对象地址。
- 过程：
- 关联创建：通过 Map 中的转换，将地图中所有的关键帧和地图点 ID 与反序列化加载后得到的关键帧和地图点相关联。这一步确保了数据结构的完整性。
- 数据打包：将上述关联打包为
map<long unsigned int, KeyFrame*>& mpMapPointId, map<long unsigned int, MapPoint*>& mpKeyFrameId。
void Map::PostLoad()
{
std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin()));
std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), std::inserter(mspKeyFrames, mspKeyFrames.begin()));
//将地图点ID和地图点地址关联打包起来
map<long unsigned int, MapPoint *> mpMapPointId;
for (MapPoint *pMPi : mspMapPoints)
{
if (!pMPi || pMPi->isBad())
continue;
mpMapPointId[pMPi->mnId] = pMPi;
}
//将关键帧ID和关键帧地址关联打包起来
map<long unsigned int, KeyFrame *> mpKeyFrameId;
for (KeyFrame *pKFi : mspKeyFrames)
{
if (!pKFi || pKFi->isBad())
continue;
mpKeyFrameId[pKFi->mnId] = pKFi;
}
...
}
- 地址恢复：最后，每个对象中的数据再从这个打包关联的容器中恢复出对应的对象地址。
void KeyFrame::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid)
{
//设置位姿
SetPose(Tcw);
//恢复观测地图点
mvpMapPoints.clear();
mvpMapPoints.reserve(N);
for(int i = 0; i < N; ++i)
{
if(mvBackupMapPointsId[i] != -1)
mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]];
else
mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
}
//恢复共视帧以及权重
mConnectedKeyFrameWeights.clear();
for(auto it = mBackupConnectedKeyFrameIdWeights.begin(), end = mBackupConnectedKeyFrameIdWeights.end(); it != end; ++it)
{
mConnectedKeyFrameWeights[mpKFid[it->first]] = it->second;
}
//恢复父关键帧
if(mBackupParentId >= 0)
mpParent = mpKFid[mBackupParentId];
else
mpParent = static_cast<KeyFrame *>(NULL);
//恢复子关键帧
mspChildrens.clear();
for(long unsigned int pKFIdi : mvBackupChildrensId)
{
mspChildrens.insert(mpKFid[pKFIdi]);
}
//清理备份容器
mBackupConnectedKeyFrameIdWeights.clear();
mvBackupChildrensId.clear();
mvBackupMapPointsId.clear();
}
在 ORB_SLAM2 上添加地图保存和加载功能（实践验证）
对 ORB_SLAM2 增加初步的序列化保存加载功能（并未将数据加载到优化部分）。
保存的变量
| 类别 | 变量名 |
|---|---|
| MapPoints： | mspMapPoints.size() |
MapPoint | |
mnId | |
世界坐标 | |
| **KeyFrames： | |
| ** | mspKeyFrames.size() |
KeyFrame | |
mnId | |
mTimeStamp | |
Tcw | |
四元数, t | |
N | |
mvKeys[i] | |
x, y | |
Size | |
Angle | |
Response | |
Octave | |
描述子 | |
MapPoint索引 |
效果
下图为初步的保存和加载功能，可以看到加载出了上一次看到的地图点以及上一次的关键帧。
单例模式序列化的问题
-
当使用
shared_ptr管理类地址时，要使用*将智能指针恢复为对象再放序列化，不然 boost 会去智能指针里面找 serialize 函数bool SaveNum(std::shared_ptr<B> classB) { std::ofstream ofs(" Map_Serialization"); boost::archive::text_oarchive oa(ofs); cout << "serializing map" << endl; oa << *classB; cout << "serialized" << endl; } -
对于自定义的单例，反序列化前要实例化出来，这样 boost 才找的到对象写入，因为自定义单例构造函数私有的原因，boost 无法自动构造对象。（在自定义单例中指定 boost 使用 get_instance 函数构造也不行）
-
对于继承 boost 库 singleton 的单例，因为构造函数是 public 的，所以很容易因为一些行为导致类构造了多个对象，然后在反序列化中就会出现段错误
5769
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
