如何评价ORB-SLAM3？

Posted 2023-05-04

我觉得 ORB-SLAM3 系统是基于之前的 ORB-SLAM2、ORB-SLAM-VI 进行扩展。作者组的工作一脉相承，围绕着 ORB feature-based SLAM 做了非常多有重大意义的工作。本文其中在一些重要改进模块，如 IMU 初始化、multi-map system 等，是作者组里前几年的工作。我认为这是一篇更加偏向于系统性质的文章，把这么多工作串了起来，并且作者非常慷慨的把它开源了出来，非常赞！

参考技术A

优势: 在静态环境下定位准确，稳定， 单目和双目版本都可以达到实时(高于10frames/s)。代码可读性强，易扩展， 网上也有实现和imu融合的版本。

劣势：建的地图点云稀疏。 运行速度方面，因为提特征点的时间有瓶颈最快的运行速度应该不超过30frames/s, 我在本机 (i7-6600U) 测的速度基本都在20frames/s左右，因此对于高帧率的相机需要降帧率才能用。对动态物体很敏感，再有动态物体时非常容易tracking lost。

总的来说ORB-SLAM还是在智能驾驶领域用得最广泛的SLAM算法，因为它在work的时候可以做得很好，急需解决的问题是对特征点提取的加速，以及处理的环境中的动态物体。

参考技术B

ORB-SLAM3是一个基于单目、双目、RGB-D相机的实时视觉SLAM系统，是ORB-SLAM系列的最新版本。以下是对ORB-SLAM3的评价：

精度高：ORB-SLAM3采用了多种技术来提高定位和建图的精度，例如在ORB特征匹配、全局优化和回环检测等方面进行了优化，使得系统的精度得到了提高。

实时性好：ORB-SLAM3采用了多线程和GPU加速等技术，使得系统可以在实时性要求较高的场景下运行，例如在移动机器人或自动驾驶等领域。

稳定性强：ORB-SLAM3在不同场景下的稳定性得到了保证，例如在光照变化、运动模糊和动态物体等复杂场景下，系统的表现依然稳定。

易于使用：ORB-SLAM3提供了友好的用户界面和API，使得用户可以方便地使用系统进行SLAM任务。

开放源代码：ORB-SLAM3是一个开源系统，用户可以方便地修改和扩展系统的功能，例如增加新的相机模型或传感器等。
综上所述，ORB-SLAM3是一个功能强大、精度高、实时性好、稳定性强、易于使用和开放源代码的视觉SLAM系统，具有很高的实际应用价值。

参考技术C 首先，我们要了解，机器人领域的视觉（Machine Vision）跟计算机领域（Computer Vision）的视觉有一些不同：机器视觉的目的是给机器人提供操作物体的信息。所以，机器视觉的研究大概有这几块： 物体识别（Object Recognition）：在图像中检测到物体类型等，这跟 CV 的研究有很大一部分交叉； 位姿估计（Pose Estimation）：计算出物体在摄像机坐标系下的位置和姿态，对于机器人而言，需要抓取东西，不仅要知道这是什么，也需要知道它具体在哪里； 相机标定（Camera Calibration）：因为上面做的只是计算了物体在相机坐标系下的坐标，我们还需要确定相机跟机器人的相对位置和姿态，这样才可以将物体位姿转换到机器人位姿。当然，我这里主要是在物体抓取领域的机器视觉；SLAM 等其他领域的就先不讲了。 由于视觉是机器人感知的一块很重要内容，所以研究也非常多了，我就我了解的一些，按照由简入繁的顺序介绍吧： 0. 相机标定 这其实属于比较成熟的领域。由于我们所有物体识别都只是计算物体在相机坐标系下的位姿，但是，机器人操作物体需要知道物体在机器人坐标系下的位姿。所以，我们先需要对相机的位姿进行标定。 内参标定就不说了，参照张正友的论文，或者各种标定工具箱； 外参标定的话，根据相机安装位置，有两种方式：Eye to Hand：相机与机器人极坐标系固连，不随机械臂运动而运动Eye in Hand：相机固连在机械臂上，随机械臂运动而运动 两种方式的求解思路都类似，首先是眼在手外（Eye to Hand） 只需在机械臂末端固定一个棋盘格，在相机视野内运动几个姿态。由于相机可以计算出棋盘格相对于相机坐标系的位姿 、机器人运动学正解可以计算出机器人底座到末端抓手之间的位姿变化 、而末端爪手与棋盘格的位姿相对固定不变。 这样，我们就可以得到一个坐标系环而对于眼在手上（Eye in Hand）的情况，也类似，在地上随便放一个棋盘格（与机器人基座固连），然后让机械臂带着相机走几个位姿，然后也可以形成一个 的坐标环平面物体检测 这是目前工业流水线上最常见的场景。目前来看，这一领域对视觉的要求是：快速、精确、稳定。所以，一般是采用最简单的边缘提取+边缘匹配/形状匹配的方法；而且，为了提高稳定性、一般会通过主要打光源、采用反差大的背景等手段，减少系统变量。目前，很多智能相机（如 cognex）都直接内嵌了这些功能；而且，物体一般都是放置在一个平面上，相机只需计算物体的 三自由度位姿即可。 另外，这种应用场景一般都是用于处理一种特定工件，相当于只有位姿估计，而没有物体识别。 当然，工业上追求稳定性无可厚非，但是随着生产自动化的要求越来越高，以及服务类机器人的兴起。对更复杂物体的完整位姿 估计也就成了机器视觉的研究热点。 2. 有纹理的物体 机器人视觉领域是最早开始研究有纹理的物体的，如饮料瓶、零食盒等表面带有丰富纹理的都属于这一类。 当然，这些物体也还是可以用类似边缘提取+模板匹配的方法。但是，实际机器人操作过程中，环境会更加复杂：光照条件不确定（光照）、物体距离相机距离不确定（尺度）、相机看物体的角度不确定（旋转、仿射）、甚至是被其他物体遮挡（遮挡）。幸好有一位叫做 Lowe 的大神，提出了一个叫做 SIFT （Scale-invariant feature transform）的超强局部特征点： Lowe, David G. "Distinctive image features from scale-invariant keypoints."International journal of computer vision 60.2 (2004): 91-110. 具体原理可以看上面这篇被引用 4万+ 的论文或各种博客，简单地说，这个方法提取的特征点只跟物体表面的某部分纹理有关，与光照变化、尺度变化、仿射变换、整个物体无关。 因此，利用 SIFT 特征点，可以直接在相机图像中寻找到与数据库中相同的特征点，这样，就可以确定相机中的物体是什么东西（物体识别）。对于不会变形的物体，特征点在物体坐标系下的位置是固定的。所以，我们在获取若干点对之后，就可以直接求解出相机中物体与数据库中物体之间的单应性矩阵。 如果我们用深度相机（如Kinect）或者双目视觉方法，确定出每个特征点的 3D 位置。那么，直接求解这个 PnP 问题，就可以计算出物体在当前相机坐标系下的位姿。↑ 这里就放一个实验室之前毕业师兄的成果 当然，实际操作过程中还是有很多细节工作才可以让它真正可用的，如：先利用点云分割和欧氏距离去除背景的影响、选用特征比较稳定的物体（有时候 SIFT 也会变化）、利用贝叶斯方法加速匹配等。 而且，除了 SIFT 之外，后来又出了一大堆类似的特征点，如 SURF、ORB 等。 3. 无纹理的物体 好了，有问题的物体容易解决，那么生活中或者工业里还有很多物体是没有纹理的：我们最容易想到的就是：是否有一种特征点，可以描述物体形状，同时具有跟 SIFT 相似的不变性？ 不幸的是，据我了解，目前没有这种特征点。 所以，之前一大类方法还是采用基于模板匹配的办法，但是，对匹配的特征进行了专门选择（不只是边缘等简单特征）。 这里，我介绍一个我们实验室之前使用和重现过的算法 LineMod： Hinterstoisser, Stefan, et al. "Multimodal templates for real-time detection of texture-less objects in heavily cluttered scenes." Computer Vision (ICCV), 2011 IEEE International Conference on. IEEE, 2011. 简单而言，这篇论文同时利用了彩色图像的图像梯度和深度图像的表面法向作为特征，与数据库中的模板进行匹配。 由于数据库中的模板是从一个物体的多个视角拍摄后生成的，所以这样匹配得到的物体位姿只能算是初步估计，并不精确。 但是，只要有了这个初步估计的物体位姿，我们就可以直接采用 ICP 算法（Iterative closest point）匹配物体模型与 3D 点云，从而得到物体在相机坐标系下的精确位姿。当然，这个算法在具体实施过程中还是有很多细节的：如何建立模板、颜色梯度的表示等。另外，这种方法无法应对物体被遮挡的情况。（当然，通过降低匹配阈值，可以应对部分遮挡，但是会造成误识别）。 针对部分遮挡的情况，我们实验室的张博士去年对 LineMod 进行了改进，但由于论文尚未发表，所以就先不过多涉及了。 4. 深度学习 由于深度学习在计算机视觉领域得到了非常好的效果，我们做机器人的自然也会尝试把 DL 用到机器人的物体识别中。 首先，对于物体识别，这个就可以照搬 DL 的研究成果了，各种 CNN 拿过来用就好了。有没有将深度学习融入机器人领域的尝试？有哪些难点？ - 知乎 这个回答中，我提到 2016 年的『亚马逊抓取大赛』中，很多队伍都采用了 DL 作为物体识别算法。 然而， 在这个比赛中，虽然很多人采用 DL 进行物体识别，但在物体位姿估计方面都还是使用比较简单、或者传统的算法。似乎并未广泛采用 DL。 如 @周博磊 所说，一般是采用 semantic segmentation network 在彩色图像上进行物体分割，之后，将分割出的部分点云与物体 3D 模型进行 ICP 匹配。 当然，直接用神经网络做位姿估计的工作也是有的，如这篇： Doumanoglou, Andreas, et al. "Recovering 6d object pose and predicting next-best-view in the crowd." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2016. 它的方法大概是这样：对于一个物体，取很多小块 RGB-D 数据（只关心一个patch，用局部特征可以应对遮挡）；每小块有一个坐标（相对于物体坐标系）；然后，首先用一个自编码器对数据进行降维；之后，用将降维后的特征用于训练Hough Forest。 5. 与任务/运动规划结合 这部分也是比较有意思的研究内容，由于机器视觉的目的是给机器人操作物体提供信息，所以，并不限于相机中的物体识别与定位，往往需要跟机器人的其他模块相结合。我们让机器人从冰箱中拿一瓶『雪碧』，但是这个 『雪碧』 被『美年达』挡住了。 我们人类的做法是这样的：先把 『美年达』 移开，再去取 『雪碧』 。 所以，对于机器人来说，它需要先通过视觉确定雪碧在『美年达』后面，同时，还需要确定『美年达』这个东西是可以移开的，而不是冰箱门之类固定不可拿开的物体。 当然，将视觉跟机器人结合后，会引出其他很多好玩的新东西。由于不是我自己的研究方向，所以也就不再班门弄斧了。机器人家上有关于这个很详细的图文讲解，你可以看下，希望对你有用 参考技术D 首先，我们要了解，机器人领域的视觉（Machine Vision）跟计算机领域（Computer Vision）的视觉有一些不同：机器视觉的目的是给机器人提供操作物体的信息。所以，机器视觉的研究大概有这几块： 物体识别（Object Recognition）：在图像中检测到物体类型等，这跟 CV 的研究有很大一部分交叉； 位姿估计（Pose Estimation）：计算出物体在摄像机坐标系下的位置和姿态，对于机器人而言，需要抓取东西，不仅要知道这是什么，也需要知道它具体在哪里； 相机标定（Camera Calibration）：因为上面做的只是计算了物体在相机坐标系下的坐标，我们还需要确定相机跟机器人的相对位置和姿态，这样才可以将物体位姿转换到机器人位姿。当然，我这里主要是在物体抓取领域的机器视觉；SLAM 等其他领域的就先不讲了。 由于视觉是机器人感知的一块很重要内容，所以研究也非常多了，我就我了解的一些，按照由简入繁的顺序介绍吧： 0. 相机标定 这其实属于比较成熟的领域。由于我们所有物体识别都只是计算物体在相机坐标系下的位姿，但是，机器人操作物体需要知道物体在机器人坐标系下的位姿。所以，我们先需要对相机的位姿进行标定。 内参标定就不说了，参照张正友的论文，或者各种标定工具箱； 外参标定的话，根据相机安装位置，有两种方式：Eye to Hand：相机与机器人极坐标系固连，不随机械臂运动而运动Eye in Hand：相机固连在机械臂上，随机械臂运动而运动 两种方式的求解思路都类似，首先是眼在手外（Eye to Hand） 只需在机械臂末端固定一个棋盘格，在相机视野内运动几个姿态。由于相机可以计算出棋盘格相对于相机坐标系的位姿 、机器人运动学正解可以计算出机器人底座到末端抓手之间的位姿变化 、而末端爪手与棋盘格的位姿相对固定不变。 这样，我们就可以得到一个坐标系环而对于眼在手上（Eye in Hand）的情况，也类似，在地上随便放一个棋盘格（与机器人基座固连），然后让机械臂带着相机走几个位姿，然后也可以形成一个 的坐标环平面物体检测 这是目前工业流水线上最常见的场景。目前来看，这一领域对视觉的要求是：快速、精确、稳定。所以，一般是采用最简单的边缘提取+边缘匹配/形状匹配的方法；而且，为了提高稳定性、一般会通过主要打光源、采用反差大的背景等手段，减少系统变量。目前，很多智能相机（如 cognex）都直接内嵌了这些功能；而且，物体一般都是放置在一个平面上，相机只需计算物体的 三自由度位姿即可。 另外，这种应用场景一般都是用于处理一种特定工件，相当于只有位姿估计，而没有物体识别。 当然，工业上追求稳定性无可厚非，但是随着生产自动化的要求越来越高，以及服务类机器人的兴起。对更复杂物体的完整位姿 估计也就成了机器视觉的研究热点。 2. 有纹理的物体 机器人视觉领域是最早开始研究有纹理的物体的，如饮料瓶、零食盒等表面带有丰富纹理的都属于这一类。 当然，这些物体也还是可以用类似边缘提取+模板匹配的方法。但是，实际机器人操作过程中，环境会更加复杂：光照条件不确定（光照）、物体距离相机距离不确定（尺度）、相机看物体的角度不确定（旋转、仿射）、甚至是被其他物体遮挡（遮挡）。幸好有一位叫做 Lowe 的大神，提出了一个叫做 SIFT （Scale-invariant feature transform）的超强局部特征点： Lowe, David G. "Distinctive image features from scale-invariant keypoints."International journal of computer vision 60.2 (2004): 91-110. 具体原理可以看上面这篇被引用 4万+ 的论文或各种博客，简单地说，这个方法提取的特征点只跟物体表面的某部分纹理有关，与光照变化、尺度变化、仿射变换、整个物体无关。 因此，利用 SIFT 特征点，可以直接在相机图像中寻找到与数据库中相同的特征点，这样，就可以确定相机中的物体是什么东西（物体识别）。对于不会变形的物体，特征点在物体坐标系下的位置是固定的。所以，我们在获取若干点对之后，就可以直接求解出相机中物体与数据库中物体之间的单应性矩阵。 如果我们用深度相机（如Kinect）或者双目视觉方法，确定出每个特征点的 3D 位置。那么，直接求解这个 PnP 问题，就可以计算出物体在当前相机坐标系下的位姿。↑ 这里就放一个实验室之前毕业师兄的成果 当然，实际操作过程中还是有很多细节工作才可以让它真正可用的，如：先利用点云分割和欧氏距离去除背景的影响、选用特征比较稳定的物体（有时候 SIFT 也会变化）、利用贝叶斯方法加速匹配等。 而且，除了 SIFT 之外，后来又出了一大堆类似的特征点，如 SURF、ORB 等。 3. 无纹理的物体 好了，有问题的物体容易解决，那么生活中或者工业里还有很多物体是没有纹理的：我们最容易想到的就是：是否有一种特征点，可以描述物体形状，同时具有跟 SIFT 相似的不变性？ 不幸的是，据我了解，目前没有这种特征点。 所以，之前一大类方法还是采用基于模板匹配的办法，但是，对匹配的特征进行了专门选择（不只是边缘等简单特征）。 这里，我介绍一个我们实验室之前使用和重现过的算法 LineMod： Hinterstoisser, Stefan, et al. "Multimodal templates for real-time detection of texture-less objects in heavily cluttered scenes." Computer Vision (ICCV), 2011 IEEE International Conference on. IEEE, 2011. 简单而言，这篇论文同时利用了彩色图像的图像梯度和深度图像的表面法向作为特征，与数据库中的模板进行匹配。 由于数据库中的模板是从一个物体的多个视角拍摄后生成的，所以这样匹配得到的物体位姿只能算是初步估计，并不精确。 但是，只要有了这个初步估计的物体位姿，我们就可以直接采用 ICP 算法（Iterative closest point）匹配物体模型与 3D 点云，从而得到物体在相机坐标系下的精确位姿。当然，这个算法在具体实施过程中还是有很多细节的：如何建立模板、颜色梯度的表示等。另外，这种方法无法应对物体被遮挡的情况。（当然，通过降低匹配阈值，可以应对部分遮挡，但是会造成误识别）。 针对部分遮挡的情况，我们实验室的张博士去年对 LineMod 进行了改进，但由于论文尚未发表，所以就先不过多涉及了。 4. 深度学习 由于深度学习在计算机视觉领域得到了非常好的效果，我们做机器人的自然也会尝试把 DL 用到机器人的物体识别中。 首先，对于物体识别，这个就可以照搬 DL 的研究成果了，各种 CNN 拿过来用就好了。有没有将深度学习融入机器人领域的尝试？有哪些难点？ - 知乎 这个回答中，我提到 2016 年的『亚马逊抓取大赛』中，很多队伍都采用了 DL 作为物体识别算法。 然而， 在这个比赛中，虽然很多人采用 DL 进行物体识别，但在物体位姿估计方面都还是使用比较简单、或者传统的算法。似乎并未广泛采用 DL。 如 @周博磊 所说，一般是采用 semantic segmentation network 在彩色图像上进行物体分割，之后，将分割出的部分点云与物体 3D 模型进行 ICP 匹配。 当然，直接用神经网络做位姿估计的工作也是有的，如这篇： Doumanoglou, Andreas, et al. "Recovering 6d object pose and predicting next-best-view in the crowd." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2016. 它的方法大概是这样：对于一个物体，取很多小块 RGB-D 数据（只关心一个patch，用局部特征可以应对遮挡）；每小块有一个坐标（相对于物体坐标系）；然后，首先用一个自编码器对数据进行降维；之后，用将降维后的特征用于训练Hough Forest。 5. 与任务/运动规划结合 这部分也是比较有意思的研究内容，由于机器视觉的目的是给机器人操作物体提供信息，所以，并不限于相机中的物体识别与定位，往往需要跟机器人的其他模块相结合。我们让机器人从冰箱中拿一瓶『雪碧』，但是这个 『雪碧』 被『美年达』挡住了。 我们人类的做法是这样的：先把 『美年达』 移开，再去取 『雪碧』 。 所以，对于机器人来说，它需要先通过视觉确定雪碧在『美年达』后面，同时，还需要确定『美年达』这个东西是可以移开的，而不是冰箱门之类固定不可拿开的物体。 当然，将视觉跟机器人结合后，会引出其他很多好玩的新东西。由于不是我自己的研究方向，所以也就不再班门弄斧了。机器人家上有关于这个很详细的图文讲解，你可以看下，希望对你有用

ORB-SLAM2 地图加载

一、前面说了ORB-SLAM地图的保存部分，继续说地图如何加载，因为加载部分相比保存要稍微复杂一些，所以要多说一点。

二、ORB-SLAM2地图加载构成

　　首先同样是在头文件中声明加载函数，包含地图点和关键帧类的加载。　　

void Load( const string &filename, SystemSetting* mySystemSetting );
MapPoint* LoadMapPoint( ifstream &f );
KeyFrame* LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting );

　　下面先是加载主函数Load的构成，关于SystemSetting类后面再说：

void Map::Load ( const string &filename, SystemSetting* mySystemSetting )
 {
     cerr << "Map reading from:"<<filename<<endl;
     ifstream f;
     f.open( filename.c_str() );
 
     //按照保存的顺序，先读取MapPoints的数目；
     unsigned long int nMapPoints, max_id = 0;
     f.read((char*)&nMapPoints, sizeof(nMapPoints));
 
     //依次读取每一个MapPoints，并将其加入到地图中
     cerr<<"The number of MapPoints:"<<nMapPoints<<endl;
     for ( unsigned int i = 0; i < nMapPoints; i ++ )
     {
         MapPoint* mp = LoadMapPoint(f);
         if ( mp->mnId >= max_id ) max_id = mp->mnId;
         AddMapPoint(mp);
     }
 
     //获取所有的MapPoints；
     std::vector<MapPoint*> vmp = GetAllMapPoints();
 
     //读取关键帧的数目；
     unsigned long int nKeyFrames;
     f.read((char*)&nKeyFrames, sizeof(nKeyFrames));
     cerr<<"The number of KeyFrames:"<<nKeyFrames<<endl;
 
     //依次读取每一关键帧，并加入到地图；
     vector<KeyFrame*>kf_by_order;
     for( unsigned int i = 0; i < nKeyFrames; i ++ )
     {
         KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);
         AddKeyFrame(kf);
         kf_by_order.push_back(kf);
     }
 
     cerr<<"KeyFrame Load OVER!"<<endl;
     //读取生长树；
     map<unsigned long int, KeyFrame*> kf_by_id;
     for ( auto kf: mspKeyFrames )
         kf_by_id[kf->mnId] = kf;
     cerr<<"Start Load The Parent!"<<endl;
     for( auto kf: kf_by_order )
     {
         //读取当前关键帧的父节点ID；
         unsigned long int parent_id;
         f.read((char*)&parent_id, sizeof(parent_id));
 
         //给当前关键帧添加父节点关键帧；
         if ( parent_id != ULONG_MAX )
             kf->ChangeParent(kf_by_id[parent_id]);
 
         //读取当前关键帧的关联关系；
         //先读取当前关键帧的关联关键帧的数目；
         unsigned long int nb_con;
         f.read((char*)&nb_con, sizeof(nb_con));
         //然后读取每一个关联关键帧的ID和weight，并把该关联关键帧加入关系图中；
         for ( unsigned long int i = 0; i < nb_con; i ++ )
         {
             unsigned long int id;
             int weight;
             f.read((char*)&id, sizeof(id));
             f.read((char*)&weight, sizeof(weight));
             kf->AddConnection(kf_by_id[id],weight);
         }
    }
    cerr<<"Parent Load OVER!"<<endl;
    for ( auto mp: vmp )
    {
        if(mp)
        {
             mp->ComputeDistinctiveDescriptors();
             mp->UpdateNormalAndDepth();
         }
    }
     f.close();
     cerr<<"Load IS OVER!"<<endl;
     return;
 }

 　　其过程就是根据保存的顺序依次加载地图点的数目、地图点、关键帧的数目、关键帧、生长树和关联关系。

　　下面是LoadMapPoints函数的构成：

 MapPoint* Map::LoadMapPoint( ifstream &f )
 {
         //主要包括MapPoints的位姿和ID；
         cv::Mat Position(3,1,CV_32F);
         long unsigned int id;
         f.read((char*)&id, sizeof(id));
 
         f.read((char*)&Position.at<float>(0), sizeof(float));
         f.read((char*)&Position.at<float>(1), sizeof(float));
         f.read((char*)&Position.at<float>(2), sizeof(float));
 
         //初始化一个MapPoint，并设置其ID和Position；
         MapPoint* mp = new MapPoint(Position, this );
         mp->mnId = id;
         mp->SetWorldPos( Position );
 
         return mp;
 }

 　　从这里开始涉及到了MapPoint类的初始化问题，由于这里只有Position以及当前的Map，所以需要从新定义MapPoint的构造函数，分别加入到MapPoint的头文件和源文件中：

  MapPoint( const cv::Mat& Pos, Map* pMap );

  MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap):
      mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
      mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1), mnFound(1), mbBad(false),
      mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
  {
      Pos.copyTo(mWorldPos);
      mNormalVector = cv::Mat::zeros(3,1,CV_32F);
  
      // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
      unique_lock<mutex> lock(mpMap->mMutexPointCreation);
      mnId=nNextId++;
  }

　　紧接着是LoadKeyFrame函数的构成，这里由于KeyFrame类需要的初始化信息比较多，因此定义了一个InitKeyFrame类，它通过SystemSetting进行初始化，二SystemSetting的主要作用就是读取设置文件（相机内参，ORB特征参数等），后面将给出SystemSetting和InitKeyFrame类的代码：

 KeyFrame* Map::LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting )
 {
     //声明一个初始化关键帧的类initkf；
     InitKeyFrame initkf(*mySystemSetting);
 
     //按照保存次序，依次读取关键帧的ID和时间戳；
     f.read((char*)&initkf.nId, sizeof(initkf.nId));
     f.read((char*)&initkf.TimeStamp, sizeof(double));
 
     //读取关键帧位姿矩阵；
     cv::Mat T = cv::Mat::zeros(4,4,CV_32F);
     std::vector<float> Quat(4);
     //Quat.reserve(4);
     for ( int i = 0; i < 4; i ++ )
         f.read((char*)&Quat[i],sizeof(float));
     cv::Mat R = Converter::toCvMat( Quat );
     for ( int i = 0; i < 3; i ++ )
         f.read((char*)&T.at<float>(i,3),sizeof(float));
     for ( int i = 0; i < 3; i ++ )
         for ( int j = 0; j < 3; j ++ )
             T.at<float>(i,j) = R.at<float>(i,j);
     T.at<float>(3,3) = 1;
 
 //    for ( int i = 0; i < 4; i ++ )
 //    {
 //      for ( int j = 0; j < 4; j ++ )
 //      {
 //              f.read((char*)&T.at<float>(i,j), sizeof(float));
 //              cerr<<"T.at<float>("<<i<<","<<j<<"):"<<T.at<float>(i,j)<<endl;
 //      }
 //    }
 
     //读取当前关键帧特征点的数目；
     f.read((char*)&initkf.N, sizeof(initkf.N));
     initkf.vKps.reserve(initkf.N);
     initkf.Descriptors.create(initkf.N, 32, CV_8UC1);
     vector<float>KeypointDepth;
 
     std::vector<MapPoint*> vpMapPoints;
     vpMapPoints = vector<MapPoint*>(initkf.N,static_cast<MapPoint*>(NULL));
     //依次读取当前关键帧的特征点和描述符；
     std::vector<MapPoint*> vmp = GetAllMapPoints();
     for(int i = 0; i < initkf.N; i ++ )
     {
         cv::KeyPoint kp;
         f.read((char*)&kp.pt.x, sizeof(kp.pt.x));
         f.read((char*)&kp.pt.y, sizeof(kp.pt.y));
         f.read((char*)&kp.size, sizeof(kp.size));
         f.read((char*)&kp.angle,sizeof(kp.angle));
         f.read((char*)&kp.response, sizeof(kp.response));
         f.read((char*)&kp.octave, sizeof(kp.octave));
 
         initkf.vKps.push_back(kp);
 
         //根据需要读取特征点的深度值；
         //float fDepthValue = 0.0;
         //f.read((char*)&fDepthValue, sizeof(float));
         //KeypointDepth.push_back(fDepthValue);
 
         //读取当前特征点的描述符；
         for ( int j = 0; j < 32; j ++ )
                 f.read((char*)&initkf.Descriptors.at<unsigned char>(i,j),sizeof(char));
 
         //读取当前特征点和MapPoints的对应关系；
         unsigned long int mpidx;
         f.read((char*)&mpidx, sizeof(mpidx));
 
         //从vmp这个所有的MapPoints中查找当前关键帧的MapPoint，并插入
         if( mpidx == ULONG_MAX )
                 vpMapPoints[i] = NULL;
         else
                 vpMapPoints[i] = vmp[mpidx];
     }
 
     initkf.vRight = vector<float>(initkf.N,-1);
     initkf.vDepth = vector<float>(initkf.N,-1);
     //initkf.vDepth = KeypointDepth;
     initkf.UndistortKeyPoints();
     initkf.AssignFeaturesToGrid();
 
     //使用initkf初始化一个关键帧，并设置相关参数
     KeyFrame* kf = new KeyFrame( initkf, this, NULL, vpMapPoints );
     kf->mnId = initkf.nId;
     kf->SetPose(T);
     kf->ComputeBoW();

     for ( int i = 0; i < initkf.N; i ++ )
     {
         if ( vpMapPoints[i] )
         {
             vpMapPoints[i]->AddObservation(kf,i);
             if( !vpMapPoints[i]->GetReferenceKeyFrame())
                 vpMapPoints[i]->SetReferenceKeyFrame(kf);
         }
     }
     return kf;
 }

　　从文件中读取的内容同保存的一致，同时由于是通过InitKeyFrame初始化的关键帧类KeyFrame，因此这里同样需要添加构造函数以及初始化方式：

KeyFrame(InitKeyFrame &initkf, Map* pMap, KeyFrameDatabase* pKFDB,vector< MapPoint*>& vpMapPoints);


KeyFrame::KeyFrame(InitKeyFrame &initkf, Map *pMap, KeyFrameDatabase *pKFDB, vector<MapPoint*> &vpMapPoints):
      mnFrameId(0), mTimeStamp(initkf.TimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
      mfGridElementWidthInv(initkf.fGridElementWidthInv), mfGridElementHeightInv(initkf.fGridElementHeightInv),
      mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
      mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
      fx(initkf.fx), fy(initkf.fy), cx(initkf.cx), cy(initkf.cy), invfx(initkf.invfx),
      invfy(initkf.invfy), mbf(initkf.bf), mb(initkf.b), mThDepth(initkf.ThDepth), N(initkf.N),
      mvKeys(initkf.vKps), mvKeysUn(initkf.vKpsUn), mvuRight(initkf.vRight), mvDepth(initkf.vDepth),
      mDescriptors(initkf.Descriptors.clone()), mBowVec(initkf.BowVec), mFeatVec(initkf.FeatVec),
      mnScaleLevels(initkf.nScaleLevels), mfScaleFactor(initkf.fScaleFactor), mfLogScaleFactor(initkf.fLogScaleFactor),
      mvScaleFactors(initkf.vScaleFactors), mvLevelSigma2(initkf.vLevelSigma2),mvInvLevelSigma2(initkf.vInvLevelSigma2),
      mnMinX(initkf.nMinX), mnMinY(initkf.nMinY), mnMaxX(initkf.nMaxX), mnMaxY(initkf.nMaxY), mK(initkf.K),
      mvpMapPoints(vpMapPoints), mpKeyFrameDB(pKFDB), mpORBvocabulary(initkf.pVocabulary),
      mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false),
      mHalfBaseline(initkf.b/2), mpMap(pMap)
  {
    mnId = nNextId ++;
  }

 　　加载了一个关键帧之后还需要计算其BoW向量等操作，同时指定关键帧对地图点的观测。

三、总结

　　加载的过程大概就是这样，时间太晚了，下次在给出InitKeyFrame和SystemSetting两个类的代码，以及其它修改的地方。总结来看，加载时关键帧类和地图点类的初始化是比较烦人的，各种繁琐的参数需要设置，所以很可能存在什么bug也说不定。另外，博客里面的代码部分是参考的网上的代码，部分是自己写的，希望有人看到了有什么错误及时指正。在数据库rgbd_dataset_freiburg1_xyz上的视频测试时是没问题的。