论文
标题：Visual-Inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration
由于轮式机器人基本上都是在平面上运动的，这就会导致vio系统不可观的自由度增加，比如说尺度。为了解决这一问题，就引用了编码器，编码器的引用会解决这些问题[1]，所以呢，在轮式机器人上编码器和VIO的融合也成为了一个研究的方向。
该篇文章主要是做了三点的工作：（1）IMU与Odometry的预积分，（2）Odometry的引用之后的初始化过程，主要是为了修正重力方向和给相机位姿一个真实的尺度，（3）线上的外参标定。
坐标系的定义
这里用到了IMU、相机和轮速计，所以一共需要4个坐标系，IMU坐标系，相机坐标系，轮速计坐标系以及世界坐标系，论文中使用下图进行表示。除了世界坐标系，相机和轮速计都是通过IMU来进行转化的，所以需要标定IMU和相机，轮速计和IMU之间的外参。
坐标系之间的关系
预积分阶段
该预积分的IMU部分与vins-mono的论文中写的是一样的，区别就是多了一项编码器部分，这里的公式为
预积分的公式
这是在第k和第k+1帧图像之间的积分，
![]()
为IMU坐标系下的位置，速度和角度，与vins无异就不做解释了。第四项
![]()
就是增加的一项表示在IMU坐标系下，Odometry运行的位置，
![]()
表示的是轮速计的速度读数，一般为
![]()
，将它乘以时间就是在odometry坐标系下的位置，通过IMU和Odometry之间的外参，以及IMU得到的旋转角，就可以得到在IMU坐标系下的轮速计所运动的距离。从上述的公式也可以看出，IMU与轮速计之间的频率是一样的。
积分的过程肯定是存在误差和噪声的传播，这里就需要通过协方差来进行传递，就需要计算后一时刻对前一时刻的雅克比A，以及后一时刻对噪声的雅克比B，通过下面的公式进行协方差的传递
协方差的传递
总结：以上的过程可以参考vins-mono的预积分过程，形式差不多就是多了一个参数而已。
初始化阶段
初始化过程主要是为了优化IMU的角速度偏置，重力方向，以及根据轮速计得到尺度信息。不像无人机可以自由的在天空翱翔，轮式机器的运动是有局限性的，这也是他在初始化阶段会由于激励不够导致初始化失败的原因。所以该篇文章在这个部分也对vins-mono进行了一定的修改。
1、对于角速度的偏置计算需要最小化的函数为
预积分已经得到了旋转对角速度偏置的雅克比，误差也已知，这样就可以得到角速度偏置。
2、重力方向的优化
在vins中优化的是相机坐标系下的重力，而该篇文章中优化的在IMU坐标系下的重力，IMU的位置与里程计的位置之间的转化关系为下面公式(1)，其中o为Odometry的坐标系，
![]()
表示的是和主帧相机时间对齐的IMU帧
根据预积分并且将世界坐标系转换为
![]()
坐标系，可以得到
将（1）式带入（2）式，并且对重力加速度进行施密特正交化，最后就可以得到论文中的公式
重力优化公式
其中
![]()
，g是重力的幅度，将其进行施密特正交化，就会得到令两维的方向，优化的时候只要优化这两维的幅度就可以了。另两维的表示为
![]()
，得到
![]()
。这样得到IMU坐标系下的重力就可以得到IMU与世界坐标系之间的关系，就能让IMU与世界坐标对齐了，也让IMU与Odometry之间对齐。
对于相机的位姿，文章中使用的是航迹推算，这里文章中没有细说，我的理解应该是相机的位姿使用的就是里程计的位姿，通过里程计与相机之间的外参就能得到相机的位姿了。
3、非线性优化
需要优化的变量如下所示
优化变量
K表示滑窗内的关键帧个数。
优化的目标为
优化目标函数
![]()
为重投影误差形式和雅克比与vins相同，
![]()
为先验误差也和vins一样，
![]()
为IMU和Odometry的误差，形式如下表示
IMU和编码器误差
这里就是多了第4项，它的误差可以通过（1）式得到，优化是通过ceres的Dogleg进行的。初始的时候外参是固定的，加速度偏置收敛的时候才进行优化。
外参优化
文中写的是线上外参标定，我觉得这应该算是一个外参优化，因为在初始化阶段需要知道外参。
在这一部分提出了一个理论就是如果要进行外参优化，必须等到加速度偏置是否收敛。使用该项准则的原因如下图所示
可以看到绿线，在初始化后进行外参优化会很不稳定。造成这样的原因是认为其他参数没有很好的进行约束，这里就提出了一个加速度偏置。确实在初始化之后，加速度偏置也是没有进行优化的。文中就引出了一个等加速度偏置收敛了再进行外参的优化，如何判断加速度是否收敛，文中用实验结果发现车转弯超过45度之后，角速度的偏置会在后面趋向于一个稳定值。判断依据是两个连续估计的加速度偏置在20s之内小于下面的阈值
加速度偏置幅度的阈值
![]()
为加速度偏置白噪声的标准差，
![]()
表示每两个连续估计加速度之间的平均时间。雅克比的计算可以见论文。
总结
该篇论文大部分的内容还是参考了vins-mono，主要就是增加了一个里程计约束项，但是在初始化阶段相机位姿方面我还是没怎么搞清楚它到底是不是使用的是轮速计的位姿来进行计算的。
转播
