vslam融合策略

我们在前端输出了多条轨迹。
轨迹1：视觉+ imu+里程计融合出来的轨迹(vio) $T_1$
轨迹2：imu+里程计轨迹（imu用dcmq求解的姿态）$T_2$
在vio初始化没有稳定之前，使用轨迹2，稳定后拼接上轨迹1，如果vio挂掉，则拼接上轨迹2，vio稳定后再拼接轨迹1。故这样实则是让轨迹2补充vio不好的时候。值得注意的是拼接的方式，拼接vio是计算出当前轨迹和vio轨迹在yaw和xyz上面的delta值，在vio轨迹乘以这个delta值即可。我们没有用增量的方式去拼接，原因增量的拼接方式有时会出现突然歪掉的情况，猜想是vio不够光滑导致。
以上拼接的轨迹我们成为轨迹3。$T_3$ world_Ts
无论vio是否稳定，我们都有稳定的轨迹3输出。
与rtk和marker融合，输出轨迹4. $T_4$ fusion_world_Ts
融合策略使用滑窗内11帧进行融合，轨迹4的初值用轨迹3的增量更新，

    if(frame_count <= 0)
    {
        fusion_world_Ts[frame_count] = world_Ts[frame_count];
    }
    else
    {
        Matrix4d TPre2Cur= Utility::toEigenInversePose(world_Ts[frame_count-1]) * world_Ts[frame_count];
        fusion_world_Ts[frame_count] = fusion_world_Ts[frame_count-1] * TPre2Cur;
    }

建立残差方程有：

1. 轨迹3的相邻帧之间的增量是正确的（我们选了5帧之间都可以）
2. 每帧的rtk观测（只有xy）
3. 每帧marker观测
   里面还要很多trick, 如果只有rtk我们就只更新xy,不更新yaw。如果有marker我们需要去估计marker的置信度。（用与marker相对距离，机器的旋转速度，计算出来的roll, pitch来评估marker是否可用，如果有rtk用x y来评估marker），总之融合marker需要很小心，我们对marker的挑选非常严格。
   以上就得到了轨迹4.
   如果rtk信号一直有，机器在走直线时，比如走1m,rtk拟合直线，轨迹4拟合直线，计算它们之间的夹角，即轨迹4需要修正的航向。这样就得到了轨迹5.
   我们输出给机器的就是轨迹5.
   如果一直有rtk，轨迹5就是很不错的轨迹了。但是rtk不是所有时刻都有的。
   建图：
   后端建图：
   在建图过程中，我们传给后端的每帧位姿是轨迹3，如果该帧被rtk更新则带上轨迹4，有marker观测则带上marker计算的位姿。
   在回到基站后，使用以上的信息做全局优化，观测是1.轨迹3相邻帧之间增量是比较准确的 2.rtk信息 3. marker信息。
   全局优化后把轨迹更新。同时利用这个轨迹我们通过BA把点云更新。
   回环
   如果长时间没有rtk则我们需要依靠回环来修正位姿。10分钟没有rtk或者marker，就进入回环检测，注意此时我们传给后端的位姿是轨迹5。
   得到的回环修正量修正前端输出的轨迹5。
   因此在我们的设计中，建图和回环是完全独立的，建图是为了得到图库的位姿尽可能的准确。回环是为了利用图库的帧修正当前帧。

相对于vins简单的回环，回环部分我们基本重写了算法。
4. 2d-3d求相对位姿，3d变成了回环帧的点云，因为我们之前发现当前帧的点云，由于点云优化次数不够，点云质量不高，导致pnp算错。
5. 用pnp计算出来的值作为初值，优化求解3d-2d，迭代优化4次。这时候如果内点还比较多，说明位姿已经计算比较准确。
6. 如果内点少于40，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解，再次迭代优化求解4次。最后内点大于4次则认为回环成功了。
7. 由于我们并没有做局部的ba，所以我们利用短时间内连续多次回环，会计算出很多帧修正值，我们像ranasc一样，找到正确的修正值。（比如6组修正值，有3个很接近，我们则认为这3个平均值是正确的修正值）

相对于原版vins，我们使用superpoint特征本来就会增多很多匹配点，加上以上策略，回环的精度要远高于以前。