Vins-fusion GPS融合部分测试（自己的数据ZED+RTK)

经过前一段时间的积累，目前暂时成功实现了用自己的数据测试实现Vins-fusion+GPS融合，其实放在数据采集处理上的时间比较多，踩了很多坑，效果在一些部分还不是很好，后期持续优化吧

其中黑色轨迹为VIO的输出轨迹，没加回环节点的，同时毛刺还比较多，不平滑，应该是我的双目相机标定的不好，但真是目前我标定得到的最佳数据了，唉～～～
紫色轨迹，被绿色基本覆盖的是RTK数据，中间存在丢帧及浮点解，因为围绕大楼，RTK浮点解数据还是偏多的
绿色的就是融合后的轨迹啦，其实已经大大优化了VIO的定位情况，但在部分地方出现很明显的折点情况，造成的原因无非RTK不好或VIO数据不好了，应该还有数据上的同步问题
×符号标注的地方是WGPS_T_WVIO更新点

融合轨迹：

evo_traj tum vio_global.txt -p

evo评测下吧～
由于没有同时采集真值，所以暂且和RTK数据比较下吧

evo_ape tum GPS.txt vio_global.txt -va --plot

APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

       max	3.483001
      mean	0.582088
    median	0.433857
       min	0.060866
      rmse	0.789349
       sse	138.321790
       std	0.533146

暂时只是先实现的基本的融合目标，优化之路道阻且长呀～

---

后续更新：
经过后期优化相机标定及算法bug，并上车测试后，目前定位精度可达到：

相机标定结果目前仍然较粗糙，需要专业的标定器材及专业的方式继续优化…