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