vins-fusion中的GPS和VO融合

https://blog.csdn.net/hltt3838/article/details/109725845
这个帖子还蛮有价值的。
GPS进行更新的时候，需要寻找与VIO时间戳相对应的GPS消息。根据+-10ms的时间偏差找GPS中的位置信息。
坐标系处理就是GPS信息转换到第一个GPS信息为原点的东北天 坐标系里，，即机体系相对BODY系里面的坐标是已知的
VIO结果以到第一帧图像的东北天坐标系为参考，和机体系之间的矩阵是已知的，
这两个矩阵可以计算得到VIO和GPS的矩阵变换，同时将这个矩阵变换不断地进行优化。
公式的原型是Tgps=Tgps<-vio *Tvio
一开始假设Tgps<-vio中旋转矩阵为单位阵，平移向量为0，在实际进行融合的时候，重新更新Tgps<-vio=Tgps*Tvio.inverse()
理论上该变换矩阵是不变的，因为第一帧都已经确定了，但是在实际解算的过程中，Tvio存在误差，所以是在一直变化的