关于DSO直接法与IMU预积分联合VIO/SLAM一些思路

本文不适合初学者；干货多没写具体方法，目前还在数论分解和思考中，估计得2个月后完成。

必要性：

1.常规VIO系统如VINS-MONO建立的地图质量太差，稀疏且不便认知

2.假设并入D相机，无论紧耦合还是松耦合，工作量依旧巨大(进行中)

3.自家硬件与光学相机在光学侧的调整能力越来越强，如光圈的控制，曝光时间的控制，全局快门等变化

4.DSO等直接法在纯视觉侧建图优势很大

5.DSO等直接法仍然没有解决视觉导航的各种问题(无纹理等blabla见前文)，需要耦合IMU预积分并入，并辅助以对应手段解决极端场景。

6.如果想用纯视觉DSO得到更鲁棒的位置姿态与建图，只能依赖硬件，如下图(来自港科大的新成果)所示的使用360度鱼眼相机，但这一方面提升了成本，一方面对后端算力平台的要求更高了。

 

DSO的作者很厉害，无论是数论还是代码本身都很强，但缺点就是导致了DSO本身的代码虽然精简，但是并不容易理解，先说一下对DSO关键点的理解：

DSO- D:直接 S:稀疏 O:里程计，看着有点矛盾，但是实现很好。

1. DSO是前后端的联合优化，本身连初始化都是个优化，导致初始化很困难，不带旋转画矩形，再互走几个对角线比较容易完成初始化，上来就不适合机器人场景。。。必须优化。

2. 初始化如果失败(RMSE双倍于前帧)，采用了一个策略模拟27个旋转平移，实际代码里是26个，xyz各有正负零3个状态，还有一个全零状态未在代码体现，共27个。

3. Host frame和Target frame分离设定，每一个激活点对应3个残差进行处理，分别各自对应2个关键帧与点本身的逆深度。在整个滑动窗口SWF的7个关键帧中进行优化。

4.Frame Hession/ Point Hession/ Point Frame Residual三个状态实例和EF后端的类互相持有指针，在整个里程计中不断传递优化及更新返回。(这设计很高效，但是也导致了改动的难度很大。)这一点是DSO能够有更加理想建图效果的核心之一。

5.有3个最关键的Jacobian，分别是图像梯度(或位姿)，逆深度，光度误差(相机内参)，对应到3个残差进入Jacobian矩阵Jk联合优化。

6.Outlier处理使用了Huber norm，使用LM法优化时，逆深度与光度误差都是平滑函数，仅迭代一次，针对图像梯度(黑白)的优化采用了关键权重Wp并持续迭代，导致对图像梯度优化的依赖很高。这3个优化都是针对同一个状态的线性化点，是FEJ的标准体现。

7.建图方面，使用了中心点与7邻域(非标准8邻域，图到处有不画了)的设计，每次逆深度投影都涉及8个点；(1)设定了2000个激活点的阈值(点有4种状态：Active/Marginalization /Candidate/Outlier)，对应32x32的块并分解为dxd(动态自适应)，不断地通过策略调整。(2)Tracking方面用的标准极线搜索，Depth只给初值后续不断迭代优化，2帧间4层LK法进行tracking。(3)持续激活Candidate点至Active，维护了一整个调度空间保持图像中特征点均匀，激活策略很巧妙，这部分代码一般不涉及修改不描述了。这一点是DSO能够有更加理想建图效果的核心之二。

8. Marginalization策略，这个很正规了，保留最新2帧+视差判断法+设定个欧式距离判定条件筛>7的帧，不赘述。

DSO与IMU预积分联合SLAM思路：

我们组一直在VINS-MONO上拓展，首先考虑的就是将DSO整体并入VINS-MONO，但从上述DSO的特点就能看出来DSO初始化与前后端耦合度很高，这个难度是很大的。至少

(1)需要完成直接法+IMU联合初始化，这里有2种做法，一是先DSO初始化/再优化IMU，这个可行性略高；另一个就是联合初始化，完全没思路，先放放。

(2)需要重新构建Hession/信息矩阵，DSO的Frame Hession架构和其他SWF的差不多，单个位姿矩阵8维，6自由度+2个光度参数。相对于VINS的p/q/v/Ba/Bg的15维来说，改造难度不算太大，转为17维即可。

(3)需要重新构建残差矩阵，这个还直接关系到原VINS系统的3约束：先验约束/视觉重投影误差/IMU预积分。这个工作量就比较大了，也是必不可少的工作。DSO的整个残差构建完全不同于常规如ORB SLAM类的视觉重投影误差，同时涉及到了位姿(梯度)，逆深度和光度误差，原来的Tk就已经有了3个权重，工程要大改。相当于变成了5项约束，先验约束也产生了变化。好的事情是VINS的基础PQV由IMU输入，视觉重投影虽然权重也高但是更接近辅助作用(在每2个关键帧之间加以强约束权重)，这里还是一定会有好的构建和约束方法的。逆深度和光度误差迭代仅1次，可以快速固定下来，其实就相当于用图像梯度(位姿)优化去联合前面这2哥们去替代视觉重投影误差约束。想得很理想，具体做好再看效果了。

思路就是如上这样了，如果实操遇到逻辑问题死火的话，那就还是继续攻关D相机与DIO。