RGBD惯性SLAM
介绍
本篇介绍一种基于视觉光流法的RGBD惯性SLAM算法，该算法的前身是DEMO算法。
相对于原始版本：
更新了相对较老的IplImage 相关接口
加入了GTSAM后端；滑动窗口后端；3D-3Dbatch优化；点到面batch后端。分别对应不同的branch。
后端表现都很一般。待进一步优化，只推荐使用visualOdometry.cpp 对应的前端。其他推荐的借鉴代码见后介绍，主要供自己后续使用。
代码地址：https://gitee.com/nie_xun/ademo/tree/master
推荐测试：
- slidW 分支：bundleAdjust_new.cpp使用slidingwindow的方法。使用方法同v5.1。
- v4.1： launch/realsense_test1.launch 点到面batch优化，稠密建图，增加平面点筛选显示，后端效果一般。
- v5.1： launch/realsense_depth_align.launch或realsense_depth_raw.launch。
在visualOdometry.cpp增加了3D-3D的batch优化，增加关键帧简单显示，非稠密建图，实时性高。
align 使用与rgb图像对齐的深度图；raw使用的是原始深度图，未与深度图对齐。有些数据包所录的深度图是raw的。 - master 分支：接近原始的，修改了少量代码，能够成功运行，环境ubuntu1804&ROS melodic。
Branches
- master 接近原始版本，修改了部分编译问题，加入了IMU filter等,
imudataPrefilter.cpp - dataset 最接近原始的版本，修改了部分编译问题，能够运行原作者的datasets
- devel 点到面batch优化，
bundleAdjust.cpp - slidW BA_new使用 sliding window & marglization,
bundleAdjust_new.cpp - useGTSAM BA 使用 GTSAM,
bundleAdjust.cpp - vo_refine 没有使用后端，但在前端代码中加入了3D-3D batch优化,
visualOdometry.cpp
代码结构
Congfig foler:
.yaml文件包含了相机参数以及IMU频率，轨迹存放位置等等配置。
.config 主要用来配置log级别等系统性配置。
src folder:
imudataPrefilter.cpp : IMU互补滤波器，详见参考文献2: /imu_to_init -> visualOdometry.cpp
featureTracking.cpp : /image_points -> visualOdometry.cpp
基于cvGoodFeaturesToTrack接口提取角点；
基于cvCalcOpticalFlowPyrLK得到匹配的角点特征点，在匹配的特征点基础上进行新提取并发布。
visualOdometry.cpp : /cam_to_init -> transformMaintenance.cpp
/cam_to_init -> stackDepthPoint.cpp
ICP 3D-3D : /cam2_to_init -> transformMaintenance.cpp
融合IMU积分结果；
通过体积/面积的方法来计算点到面的深度；
基于匹配好的特征点建立观测约束（根据是否匹配深度建立不同的约束方程）最后通过高斯牛顿进行位姿解算。
processFlatpoints.cpp : 通过计算点簇的协方差来提取平面点 : /flat_cloud -> bundleAdjust.cpp
-> registerPointCloud.cpp
transformMaintenance.cpp : ->/camera2_refine
通过插值法融合前端高频位姿输出与后端低频位姿输出，最后得到优化后的高频位置输出,
registerPointCloud.cpp : map点云到原始坐标系下并发布点云地图：->/surround_cloud
bundleAdjust.cpp : 后端优化代码，不同分支根据方法不同而不同-> /bef_ba_to_init
bundleAdjust_new.cpp : marglize & sliding window BA : ->/cam2_to_init
trajLoad.cpp: 发布并生成轨迹文件，订阅/cam_to_init & /cam2_to_init
simulator.cpp: 发布仿真图像特征点数据，用于测试 bundleAdjust_new.cpp
stackDepthPoint.cpp : stack feature point : /depth_points_stacked -> bundleAdjust.cpp
使用示例图
压力测试，让数据集倍速播放，查看输出速率，基于v5代码:
https://gitee.com/nie_xun/ademo/blob/master/doc/outdoor_map.mp4
建图视频:
https://gitee.com/nie_xun/ademo/blob/master/doc/stress.webm
参考
1 原始代码地址-https://github.com/wpfhtl/demo_rgbd
2 IMU互补滤波器-https://blog.csdn.net/weixin_41469272/article/details/113599414