[论文阅读]SRVIO:Super Robust Visual Inertial Odometry for dynamic environments...

文章全名：SRVIO: Super Robust Visual Inertial Odometry for dynamic environments and challenging Loop-closure conditions
提出了一个基于“Vins-Mono”并结合了“DNN网络”的针对“动态场景”的“实时”SLAM系统。

文章的主要贡献：
1、最重要的贡献不是方法上的创新，而应该是给了一个好用的实时的完整的SLAM系统
2、每个部分都给了DNN网络的结合方法。Preprocessing结合了语义网络HRNet筛选特征点、自己训练的去IMU噪声网络处理IMU数据流；回环用了SuperPoints和SuperGlue。

SRVIO系统框架

主要分为预处理preprocessing、优化optimization和回环检测loop closure三部分。

A.预处理preprocessing

预处理的输入：照片流+IMU数据流
预处理的输出：
组成部分：视觉预处理+惯导预处理

视觉预处理

输入：照片流，输出：每帧的特征点矩阵和该帧的特征点质量权重（表示这个帧的好坏）
1、首先照片经过①，得到特征点位置和描述子
2、然后照片通过②经由HRNet处理，得到特征点属于moveable还是unmoveable
3、由于神经网络结果的语义信息的局限性，到☂这里用到一个三步的处理方法。
先用unmoveable的结果通过RANSAC计算位姿；（unmoveable结果给的位姿）
再用位姿通过Fundamental矩阵检验所有匹配的离群情况；（点的实际动、静情况）
最后对所有的inliner再RANSAC计算一次得到最终位姿；（取所有静态点再次计算位姿）
$$\begin{array}{rl}& [P^{C_i,S_i},F_{initial}^{i,i-1}]=RANSAC(P_{initial}^{C_i},P^{C_i-1})\\ & P^{C_i,S_2}=\{P_{l,initial}^{C_i}:|P_{l,initial}^{C_i}.F_{initial}^{i,i-1}.P_l^{C_i-1}|<ϵ\}\\ & [P^{C_i},F_{accurate}^{i,i-1}]=RANSAC(P^{C_i{,}_2},P^{C_i-1})\end{array}$$
4、最后在④计算一个权重表示照片帧的质量，通过静态点占总匹配的比例计算，在优化阶段使用
$${\Psi }_{c_i}=\frac{|P^{C_i}|}{F_{max}}$$

惯性预处理

输入：IMU数据流，输出：通过去噪声卷积神经网络DCNN的IMU数据，认为是没有噪声的（合理吗？凭什么）。共有两个卷积神经网络，一个是用于陀螺仪（角速度）去噪声，一个用于加速度去噪声

陀螺仪卷积网络：输入：从i-N到i-1的N个IMU数据序列，输出：经过修正的第i个imu角速度测量数据；
加速度卷积网络：输入：从i-N到i-1的N个IMU数据序列，输出：经过修正的第i个imu加速度测量数据；
网络结构：卷积和全连接两个部分
卷积部分用有标签的IMU数据训练用于预测误差，训练好固定住（使用到GELU策略（GELU activation）和余弦学习率（cosine schedulers））
全连接部分用带噪声的IMU数据训练，用于预测IMU数据质量

由于高频下IMU数据没有真实的值，则将网络给出的结果通过积分到有真实值的地方来定义训练误差：
$$\begin{array}{rl}& \delta {\hat{R}}_{i,i+j}=\sum _{k=i}^{i+j-1}exp(\widehat{w_k})\\ & \delta {\hat{P}}_{i,i+j}=\int {\int }_{t\in [i,i+j]}exp(\widehat{w_t})\widehat{a_t}d{t}^2\\ & {\mathcal{L}}_j^{\omega }=\sum _i\rho \left(log\left(\delta R_{i,i+j}\delta {\hat{R}}_{i,i+j}^T\right)\right)\\ & {\mathcal{L}}_j^a=\sum _i\rho \left(\delta P_{i,i+j}-\delta {\hat{P}}_{i,i+j}\right)\\ & {\mathcal{L}}^{\omega }={\mathcal{L}}_{16}^{\omega }+{\mathcal{L}}_{32}^{\omega }\\ & {\mathcal{L}}^a={\mathcal{L}}_{16}^a={\mathcal{L}}_{32}^a\end{array}$$
其中$\delta {\hat{R}}_{i,i+j}$和$\delta {\hat{P}}_{i,i+j}$表示真实的旋转和平移变换，网络给出的$\delta R_{i,i+j}$和$\delta P_{i,i+j}$与真实值作差并在一个时间跨度为j的窗口内求和，得到两个误差项${\mathcal{L}}_j^{\omega }$和${\mathcal{L}}_j^a$，16步和32步两种情况下的误差项求和得到最终的误差项。

1、经过①网络处理得到IMU去噪声序列$\widetilde{w_i}$和$\widetilde{a_i}$，用于计算后续的IMU数据权重的数据质量${\zeta }_i^{\omega }$和${\zeta }_i^a$
2、原始的IMU数据经过一个修正矩阵${\hat{C}}_{(.)}$后和$\widetilde{w_i}$以及$\widetilde{a_i}$相加得到矫正的角速度和加速度。PS：我猜测${\hat{C}}_{(.)}$是IMU坐标系转换到相机坐标系，网络输出的矫正序列是相对相机坐标系而言的。
$$\begin{array}{rl}& {\hat{C}}_{(.)}=\widehat{S_{(.)}}\widehat{M_{(.)}}\\ & \widehat{{\omega }_i}=\widehat{C_{\omega }}{w}_i^{IMU}+\widetilde{w_i}\\ & \widehat{a_i}=\widehat{C_a}{a}_i^{IMU}+\widetilde{a_i}\\ & {\zeta }_i={\zeta }_i^{\omega }+{\zeta }_i^a\end{array}$$
矫正后的数据在②进行预积分
3、最后在☂计算IMU数据权重
$${\Psi }_{b_i}=\frac{{\sum }_{k\in M_{l,j}}{\zeta }_k}{m}$$

B.优化Optimization

这部分就是在误差这里，用网络简化了IMU预积分过程+质量参数影响误差
IMU测量误差
IMU预积分
$$\begin{array}{rl}& {\alpha }_{b_{k+1}}^{b_k}=\int {\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t)d{t}^2\\ & {\beta }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t)dt\\ & {\gamma }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega (\hat{\omega })\gamma {}_t^{b_k}dt\end{array}$$
其中${\hat{a}}_t$和$\hat{\omega }$是加速度和角速度，$\Omega$表示一种矩阵变换。上式的第三行是四元数的积分计算，通过$\Omega$将两个四元数运算转换成矩阵运算
$$\begin{array}{rl}& \Omega (\omega )=\left[\begin{array}{cc}-{\lfloor \omega \rfloor }_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right]\\ & -{\lfloor \omega \rfloor }_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]\end{array}$$
此时我们得到IMU误差如下，基本表现就是“误差”=“两个时刻真实值表示的变换（减号左边部分）” - “IMU积分结果（减号右边部分）”：
$$\begin{array}{rl}& r_{\mathcal{B}}({\hat{z}}_{b_{k+1}}^{b_k},\chi )=\left[\begin{array}{c}\delta {\alpha }_{b_{k+1}}^{b_k}\\ \delta {\beta }_{b_{k+1}}^{b_k}\\ \delta {\theta }_{b_{k+1}}^{b_k}\end{array}\right]\\ & =\left[\begin{array}{c}{R}_{\omega }^{b_k}(p_{b_{k+1}}^{\omega }-p_{b_k}^{\omega }+\frac{1}{2}{g}^{\omega }\Delta t_k^2-v_{b_k}^{\omega })-{\hat{\alpha }}_{b_{k+1}}^k\\ R_{\omega }^{b_k}(v_{b_{k+1}}^{\omega }+g^{\omega }\Delta t_k-v_{b_k}^{\omega })-{\hat{\beta }}_{b_{k+1}}^{b_k}\\ 2[q_{b_k}^{\omega -1}\otimes q_{b_{k+1}}^{\omega }\otimes ({\hat{\gamma }}_{b_{k+1}}^{b_k}{)}^{-1}{]}_{xyz}\end{array}\right]\end{array}$$
由于去噪声网络的存在，这里的积分结果应该极大地简化了

视觉测量误差
如下式给出
$$\begin{array}{rl}& {\mathcal{P}}_l^{c_j}={\pi }_c^{-1}(\left[\begin{array}{c}{u}_l^{c_j}\\ v_l^{c_j}\end{array}\right])\\ & {\mathcal{P}}_l^{c_i}=R_b^c(R_{\omega }^{b_j}(R_{b_i}^{\omega }(R_c^b\frac{1}{{\lambda }_l}{\pi }_c^{-1}(\left[\begin{array}{c}{u}_l^{c_i}\\ v_l^{c_i}\end{array}\right])+p_c^b)+p_{b_i}^{\omega }-p_{b_j}^{\omega })-p_c^b)\\ & r_c({\hat{z}}_l^{c_j},\chi )=\left[\begin{array}{c}{b}_1{b}_2\end{array}\right]\cdot ({\mathcal{P}}_l^{c_j}-\frac{{\mathcal{P}}_l^{c_i}}{||{\mathcal{P}}_l^{c_i}||})\end{array}$$
第一个式子将第j张图片的点反投影到归一化相机坐标系；
第二个式子将第i张图片的点反投影到相机坐标系（到${\lambda }_l$这），转换到IMU坐标系（到$R_c^b$这），转换到世界坐标系（到$R_{b_i}^{\omega }$这），转换到第j帧的IMU坐标系(到$R_{\omega }^{b_j}$这)，转换到第j帧的相机坐标系(到$R_b^c$这)
第三个式子将在同一个世界坐标系下的两个坐标求差，$b_1$和$b_2$表示两个坐标所占的权重，作者说得高大上是投影到一个平面上，$b_1$和$b_2$是平面的基
第三个式子感觉有点问题，${\mathcal{P}}_l^{c_j}$在归一化相机坐标系下，${\mathcal{P}}_l^{c_i}$在未归一化相机坐标系下，作者将
${\mathcal{P}}_l^{c_i}$除自己的模应该是想让他们都在归一化后的坐标系下，这里的操作应该为除${\mathcal{P}}_l^{c_i}$的z坐标

结合误差
一个状态窗口内的数据有
$$\begin{array}{rl}& \chi =[x_0,x_1,...x_n,x_c^b,{\lambda }_1,...,{\lambda }_m],\\ & x_k=[p_{b_k}^{\omega },v_{b_k}^{\omega },q_{b_k}^{\omega }],\in [0,n],\\ & x_c^b=[p_c^b,q_c^b]\end{array}$$
$x_k$表示IMU的状态：位置、速度、朝向；$x_c^b$表示IMU和相机间的位姿变换；${\lambda }_l$表示第l个特征点在窗口内第一次被观测时的深度倒数
最终误差给出如下形式，误差=先验信息（不太懂怎么来的）+IMU误差（${\Psi }_{c_k}$修正）+相机误差（${\Psi }_{b_k}$）修正：
$$\mathcal{R}=||r_p-H_p\chi |{|}^2+{\Psi }_{c_k}\sum _{k\in \mathcal{B}}||r_{\mathcal{B}}({\hat{z}}_{b_{k+1}}^{b_k},\chi )|{|}_{P_{b_{k+1}}^{b_k}}^2+{\Psi }_{b_k}\sum _{l,j\in \mathcal{C}}\rho (||r_{\mathcal{C}}({\hat{z}}_l^{{\mathcal{C}}_j},\chi )|{|}_{P_l^{{\mathcal{C}}_j}}^2)$$

C.回环检测Loop closure

其实没什么改变，就是把特征点换成了SuperPoint，把回环候选帧的特征点匹配换成了SuperGlue。先通过knn检测闭环，检测到之后用SuperGlue寻找特征点，并且没有用基础矩阵检查静止的会运动物体，因为他们在后续的时候会消失。

总结：
1、对动态语义的进一步处理
2、误差去噪声网络的应用，极大地简化了IMU融合
3、SuperGlue锦上添花