系统初始化:

目的:

计算出绝对尺度s,陀螺仪偏置bg,加速度偏置ba,重力加速度G和每个时刻的速度v.也就是系统固有参数的标定

背景:

单目惯性紧耦合是一个非线性程度很高的系统.纯视觉单目SLAM缺乏实际的尺度的信息,并且无法知道惯性坐标系,它的pose大多是基于第一帧为世界坐标系建立的(如ORB SLAM),同时它无法得到比较准确的速度,这对于控制和规划是很重要的.而虽然IMU与视觉互补,可以提供世界的尺度,估计惯性坐标系,但是这些信息通常会受到噪声和Bias的影响.所以可以把这两者优势互补,首先估计出这些这些量.

实现:

采用loosely-coupled方案,首先用SFMA求解滑窗内所有帧的位姿,和所有路标点的3D位置,然后跟IMU预积分值对齐,求解重力重力方向,尺度因子,陀螺仪bias和每一帧对应的速度.论文中提到,加速度的偏置与重力加速度相耦合,而重力加速度的量级要远大于其加速度的偏置,而且加速度的偏置很小,所以忽略加速度bias的计算.

①纯视觉初始化

②视觉IMU对齐–视觉惯性联合初始化

③与世界坐标系对齐

a.纯视觉初始化

构建一个滑窗,包含一组帧,将当前帧与之前帧进行匹配,当两帧匹配上的特征点数量>30个,并且有足够的视差(旋转>20个像素)使用5-point法进行恢复相对运动,然后三角化得到相应的3d点坐标,将3d点与滑窗内其他帧的2d点进行PNP求解获得滑窗中所有帧的位姿和3d坐标,BA优化滑窗内的所有位姿.即求得了qc0ck.(采用相机第一帧作为基准坐标系)

b.视觉IMU联合初始化流程:

①若旋转外参数qbc未知,则先估计旋转外参数

②利用旋转约束估计陀螺仪bias

③利用平移约束估计重力方向,速度,以及尺度初始值

④对重力向量进行进一步优化  

⑤求解世界坐标系和初始相机坐标系之间的旋转矩阵qwc,并将轨迹对齐到世界坐标系

                                                   

视觉和IMU之间的几何约束为(这是对齐的约束):

                                                                                     

其中s为尺度因子,上面有一横代表非米制单位轨迹.

b②.陀螺仪bias校正

目标函数:visual给出的相邻帧之间的旋转=IMU预积分旋转值

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-q8PxXY0N-1573615403865)(/home/yhj/.config/Typora/typora-user-images/1572096846218.png)]

b③.估计重力方向,速度,以及尺度初始值

待估计变量是:

                                                                     

残差可定义为相邻两帧之间的IMU预积分出的增量与预测值之间的误差:

                                                                    

                                

<论文中给出是求解最小二乘,但程序中采用cholosky分解的方法求解,假设没有噪声影响.>

b④.重力向量优化

上一步计算的重力加速度吸收了加速度计的偏置,所以需要对它进行优化.在③中求出的g没有加入模长的限制(绝对值为固定的9.81),可能是随机的,而实际情况中重力的大小是已知的.所以这里进行优化时重力的自由度为2(方向).采用球面坐标系进行参数化:
                                            

                                                                 ;

<我的理解: w1和w2为待优化变量,是③中求解出的g在两个方向上不断进行校正,b1和b2为归一化的单位向量,每次迭代求解后,只保留方向进行下一次优化,保证了它的大小为9.81,它的优化公式和③一样>

c.将相机坐标系对齐世界坐标系

将所有c0坐标系下的变量旋转到w下,将相机平移和特征点尺度恢复到米制单位.

(通过世界坐标系下的重力加速度与相机坐标系下的重力加速度之间的关系求出w和c之间的旋转矩阵)