VINS-Fusion-RGBD在双轮差数轮小车上配置并进行稠密建图

VINS-Mono由于存在运动初始化过程，对于地面小车来说运动初始化过程没有无人机那么方便，而且大多实用小车需要一运行程序就要执行任务，所以需要选择不需要初始化的版本。

VINS-Fusion的双目、双目+imu版本不需要初始化，但双目+imu轨迹容易飞飘，不能进行稳定建图。而双目版本只能建稀疏路标点地图，无法用于实际导航。

VINS-Fusion-RGBD版本可以进行半稠密建图，不需要初始化，但鲁棒性和精度还不是很好，对于精度高，大尺度场景需要进一步改进。

这里提出了一种VINS-Fusion-RGBD算法适用于小车的改进版：VINS-Fusion-Vehicle，增加了前端接口，把轮式里程计接入VINS-Fusion前端，利用深度摄像头获取深度点云信息，然后利用octomap库管理点云，实时生成栅格地图。经过多次验证，该算法效果良好，在建立大尺度地图（300米的大圈）也可以保持良好的精度和实时性。

1.轮式里程计代替视觉里程计

小车上配备有轮式编码器，利用两个轮式编码器数据，通过双轮差数模型可以获得小车轮式里程计的数据，使用该里程计数据代替VINS-Fusion中通过视觉PnP方法获得的里程计数据。即把其他里程计数据接入了VINS前端。

2.其他获得更为鲁棒性建图效果的做法

利用轮式里程计数据，可以使算法前端精度更高，鲁棒性更好。
禁用imu。
调整回环判定条件，增加抽取的特征点数量。

3.点云地图及栅格地图的获得

点云发布改为只发布当前帧点云数据，利用同时启动的octomap节点，接受VINS后端发布的点云数据，增量拼接到octomap管理的地图中。这样VINS算法可以持续实时运行，发布的点云不会随着运行时间而增大。
octomap管理点云地图并实时生成栅格地图，大规模场景其八叉树点云地图依然很小，几百万个点也才几十兆。栅格地图利用octomap自带的点云分割，建图效果还行，后期再处理一下就可以用来导航避障了。

4.多个参数可配置

增加了回环判定的匹配点数量、前端里程计话题、回环范围限定等参数，可在yaml文件中配置。

目前我已将该算法开源-Git地址传送门:VINS-Fusion-Vehicle。

该算法还有一些问题，欢迎大家一起改进。