1. 数据文件读取, 发布话题, 计算速度 ,加速度
2. 订阅话题, 送入ESKF, 得到速度, 加速度等

1. 把 pangolin可视化加入, 以便更好地比较两条轨迹
2. 添加step调试的功能, 把需要比较的所有参数都可视化出来

1. POSE和融合后的轨迹(time, p, R)
2. POSE和融合后的状态(time, p, v, R, bg, ba, g)
3. POSE和融合后的角速度, 线速度, 线加速度
4. evo 计算轨迹的误差
5. plot 绘制两个轨迹和状态

用自己的设备oak采集一些数据 去跑双目或双目惯性SLAM, 有时间的话最好自己试着写一下. 用数据来测试一下 该算法 的可行性和效果