最近在做无人机建图的相关工作，基本的方案是ORB SLAM2+Map2DFusion。
在调试好代码后，我利用大疆精灵4在附近的一个公园进行算法测试，得到的效果图如下：
但在一些细节上会有明显的断裂感，比如这个：
出现这个现象的本质原因是ORB SLAM2进行相机位姿重构时，精度不够高，导致建图拼接的结果产生明显的断裂感。
改善这个问题的办法在于提高SLAM的精度。在Map2DFusion论文中提到将GPS数据融合到SLAM中，我就从这个方向入手，看有没有效果。
我从ORB SLAM2的局部优化线程入手，局部优化的内容是在一个共视图窗口(上图虚线框示意)内，通过将点云重映射到关键帧像素坐标系内，与ORB匹配关键点坐标计算误差，这里称它为重投影误差。然后用Bundle Adjustment最小化这个误差。
我的思路是用过去n个时刻的关键帧的SLAM坐标和GPS坐标计算SLAM坐标系与GPS坐标系之间的相似变换矩阵。当产生第n+1个时刻关键帧的SLAM坐标时，用上图右下框的式子预测GPS坐标，与GPS观测值的坐标做差求误差，将这个误差加到原有的误差函数中，就完成了SLAM对GPS的融合。
但实际程序跑下来，无人机建图的效果并没有肉眼上可分辨的提高。当我加大GPS误差在总误差的权重时，效果反而变差了。
可能我的想法本身有问题，因为我读取到的无人机GPS数据是有时间延迟的，我假设这个时间延迟足够小到可以忽略。或许实际上它并不能忽略。
或许是GPS本身就不适合优化视觉SLAM。因为相对来说GPS是低精度高延迟累积误差小，视觉SLAM是高精度低延迟累积误差大。GPS适合用来减小视觉SLAM的累积误差(或许可以用在回环检测中)。从另一个角度，或许应该用高频的数据去优化低频的数据(这一点类似VIO中用IMU去优化视觉SLAM)。
或许，应该用视觉SLAM去优化GPS定位，而不是用GPS定位去优化视觉SLAM。
以上想法纯属个人思考，没有实验支持，有错之处望谅解。
