vins-fusion代码解读[二] 惯性视觉里程结果与GPS松耦合

感谢“slam萌新”，本篇博客部分参考：
https://blog.csdn.net/weixin_41843971/article/details/86748719
欢迎讨论。

惯性视觉里程结果与GPS松耦合：

github开源的是KITTI的数据集的测试代码。跟着程序走一遍。

KITTIGPSTest.cpp

主程序入口:
vins_estimator包下面的KITTIGPSTest.cpp,主要作用：
（1）开启estimator类，进行vio里程估计
（2）从文件中读取视频图片列表，读入estimator类中
（3）从文件中读取GPS数据列表，直接通过ROS发布出去

具体的，从文件中获取图像和GPS数据当前的时间戳信息。以第一帧图像和第一个GPS时间早的作为基准时间，之后，左右双目的图像通过estimator.inputImage（）读入里程计中，里程计Estimator类内部会定时发送VIO计算的结果。同时main函数中会在图片读入里程计的时刻发布同一时刻的GPS信息。（每一帧图片都有对应一条GPS信息，时间戳设置为一致的）

GPS融合主要发生在global_fusion包中。

global_fusion

程序同步开启了global_fusion节点。

rosrun global_fusion global_fusion_node

程序入口globalOptNode.cpp
程序定义了一个GlobalOptimization类globalEstimator来进行融合处理。
程序有三个回调函数：
（1）publish_car_model():根据最终vio与GPS融合的定位结果，发布在特定位置的一个炫酷的小车模型。
（2）GPS_callback()：通过globalEstimator.inputGPS（），放入全局融合器中。
（3）vio_callback()：通过globalEstimator.inputOdom（），放入全局融合器中。并且从全局融合器globalEstimator中取出最终位姿融合的结果，调用publish_car_model()发布出来。

最重要类的核心为 GlobalOptimization类 和类内的optimize（）函数。

GlobalOptimization类

类成员：
(1)map类型
localPoseMap中保存着vio的位姿
GPSPositionMap中保存着gps数据
globalPoseMap中保存着优化后的全局位姿

以上类成员中map的格式:

map<double,vector<double>> =<t,vector<x,y,z,qw,qx,qy,qz>>

(2)bool类型
initGPS:第一个GPS信号触发
newGPS:有新的GPS信号后触发

(3)GeographicLib::LocalCartesian 类型
geoconverter GPS经纬度信号转化为X,Y,Z用到的第三方库
当该类已进行初始化，就同步开启了新线程optimize（），对两个结果不断进行优化。

optimize（）

（1）当有新的GPS信号到来时候，进行GPS与视觉惯性的融合
（2）建立ceres的problem

• lossfunction 设置为Huberloss
• addParameterBlock添加优化的变量 ,优化的变量是q_array以及t_array。即globalPoseMap保存下来的每帧图像的位姿信息。其中参数变量的多少是由localPoseMap来决定的。即VIO有多少个数据，全局也就有多少个。迭代器指向的first为时间，second为7变量的位姿。其中在添加q_array由于维度只有三维，因此增加了local_parameterization来进行约束。
• 接着开始添加残差项，总共有两个残差项分别是：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时刻的位姿。
• 求解非线性优化方程
• 求解出来后，把t_array和q_array（即两个优化的变量）赋值给globalposeMap。并且根据最新解算出来的结果（即i=length-1时刻最新的结果），跟新GPS到vio这两个独立体系之间坐标转换关系。

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。残差的得到就根据实际值与观测值之间的偏差直接得出。