vins-fusion gps融合相关总结

1 . 简介：

VINS Fusion在VINS Mono的基础上，添加了GPS等可以获取全局观测信息的传感器，使得VINS可以利用全局信息消除累计误差，进而减小闭环依赖。

相比于局部传感器，全局传感器(如GPS，气压计和磁力计等)可以提供全局观测。这些传感器使用全局统一坐标系，并且输出的观测数据的方差不随时间累计而增加。但这些传感器也存在一些问题，导致无法直接用于精确定位和建图。以GPS为例，GPS数据通常不平滑，存在噪声，且输出速率低。

因此，一个简单而直观的想法是将局部传感器和全局传感器结合起来，以达到局部精确全局零漂的效果。这也就是VINS Fusion这篇论文的核心内容。

https://zhi-ang.github.io/2019/09/11/vins_fusion/

2 . 优化流程：

首先呢，判断是否有gps数据，整体的算法就是在ceres架构下的优化算法。
所以总体的步骤就是ceres优化的步骤，首先添加优化参数块（AddParameterBlock函数），参数为globalPoseMap中的6维位姿（代码中旋转用四元数表示，所以共7维）。
之后添加CostFunction，通过构建factor重载operator方法实现（具体实现需要学习ceres库）。该部分有两个factor，一个是位姿图优化，另一个则是利用gps进行位置约束。
将factor添加后，进行ceres求解，更新此时gps和vio间的坐标系转换参数，之后再利用updateGlobalPath函数更新位姿。

https://blog.csdn.net/weixin_41843971/article/details/86748719

3 . 残差项：

接着开始添加残差项，总共有两个残差项分别是：vio factor以及gps factor

– vio factor：

残差项的costfunction创建由 relativeRTError来提供。观测值由vio的结果提供。此时计算的是以i时刻作为参考，从i到j这两个时刻的位移值以及四元数的旋转值作为观测值传递进入代价函数中。 此时iPj代表了i到j的位移，iQj代表了i到j的四元数变换。添加残差项的时候，需要添加当前i时刻的位姿以及j时刻的位姿。即用观测值来估计i时刻的位姿以及j时刻的位姿。

– gps factor：

残差项的costfunction创建由 TError来提供。观测值由Gps数据的结果提供。添加残差项的时候，只需要添加当前i时刻的位姿。

TError 及 RelativeRTError

直观上理解：
{0, 1，2，3，4, 5，6…}
要估计出这些时刻，每个时刻的位姿。
我有的是两个方面的观测值，一方面是GPS得到的每个时刻的位置（x,y,z）（并且GPS信号可以提供在该时刻得到这个位置的精度posAccuracy），没有累计误差，精度较低。另一方面是VIO得到的每个时刻的位置（x,y,z）以及对应的姿态四元数（w,x,y,z）,存在累计误差，短时间内精度较高。为了得到更好的一个融合结果，因此我们采用这个策略：观测值中，初始位置由GPS提供，并且vio观测值信任的是i到j时刻的位移以及姿态变化量。 并不信任vio得到的一个绝对位移量以及绝对的旋转姿态量。只信任短期的i到j的变化量，用这个变化量作为整个代价函数的观测值，进行求解。
因此两个残差项TError及RelativeRTError分别对应的就是GPS位置信号以及vio短时间内估计的i到j时刻的位姿变化量对最终结果的影响。迭代求解的过程中均采用了AutoDiffCostFunction自动求解微分来进行迭代。
（1）TError
TError（x,y,z,accuracy），最后一项是定位精度，可以由GPS系统提供。残差除了直接观测值与真值相减以外，还除了这个accuracy作为分母。意味着精度越高，accuracy越小，对结果的影响就越大。
（2）RelativeRTError
RelativeRTError（dx,dy,dz,dqw,dqx,dqy,dqz，t_var,q_var），最后两项是位移以及旋转之间的权重分配比例，并且为了使得与TError对应。在程序中，应该是根据经验把最后两项设置成一个常值，分别对应0.1以及0.01。残差的得到就根据实际值与观测值之间的偏差直接得出。

原文链接：https://blog.csdn.net/huanghaihui_123/article/details/87183055

Local Factor: 局部观测约束,VIO相对位姿变换, 计算的是相邻两帧之间位姿的残差