VINS理论与代码详解4——初始化

VINS理论与代码详解4——初始化

1.      基于滑动窗口的纯视觉单目初始化

    在介绍纯视觉初始化前我们首先讲一讲为什么要初始化？初始化要做什么？以及初始化的作用？我们初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度（包括大小和方向），对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义（其实这里就可以理解成相机标定一样，没有正确的标定好相机的内参，相机在进行定位的时候必然不准，而且很有可能会挂掉）。所以初始化要做的事其实说起来很简单，就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v，VINS中重点说明了加速度计偏置值一般都会和重力加速度耦合到一起（也就是被重力加速度给吸收掉），重力加速度的量级要远大于其加速度偏置，而且在初始化时间内加速度计偏置比较小，很难真正的计算得到，因此忽略加速度计偏置的影响，在初始化中不再计算。初始化的作用是不言而喻的，直接影响整个紧耦合系统的鲁棒性以及定位精度，并且初始化一般都需要一个比较漫长的时间，VINS大概需要十秒左右，ORB_SLAM2结合IMU的时间设定在15秒完成初始化。话不多说，直接进入正题。

         纯视觉初始化在第V点的A部分，首先构建一个滑动窗口，包含一组数据帧。论文中提及使用的是对极几何模型的5点法求解单目相机的相对变换，包括相对旋转和无尺度信息的位移。其实基本上每个单目模型都是使用对极几何在初始化中求解两帧的相对变换，这里需要注意的是旋转是具有尺度不变性的（其实就是单位旋转，不会有尺度信息，你仔细想想是不是？）。然后三角化得到相应的3d点坐标，有这些3d点和滑动窗口中其他的帧的2d点就可以进行PNP求解获得滑动窗口中的所有的位姿和特征点3d坐标，至此，纯视觉初始化就完成了。是不是很简单？当然啊，毕竟只是简单的视觉初始化，而真正复杂的是视觉惯性联合初始化，也就是我们初始化的重点和难点，所以下面的知识点一定要打起精神学啦！

 

2.      视觉惯性联合初始化

    视觉惯性联合初始化在第V点的B部分，这里作者给定义的名字叫Visual-Inertia Alignment，即视觉惯性联合初始化（而在ORBSLAM2+IMU的论文里，作者定义的名称就叫IMU initialization，即IMU初始化），为什么定义这样一个名词，我觉得有两个意义，第一在进行陀螺仪偏置初始化的时候要同时使用到IMU测量的旋转和视觉测量的旋转，也就是要联合视觉和惯性的数据。第二这里求得的尺度S的值不仅仅是IMU的，还是视觉和IMU整个系统的尺度。在具体的讲解初始化每个过程的时候，有必要来个总体的概括，初始化在物理意义上的定义其实就是固有参数的标定，在数学模型上的定义其实就是公式（6）的矩阵方程求解，而公式（6）其实就是来自于最原始的PVQ积分公式，其中Q旋转对应着陀螺仪，而PV对应着加速度计，如果不明白的话，不要紧，看完下面的整体推导过程相信聪明的你一定会茅塞顿开。

（1）      陀螺仪偏置标定

旋转我们可以通过两种方式求得，一种是陀螺仪测量值，一种就是视觉观测值。按照正常的理解两者的大小一定是相等的（假设没有误差），但实际情况肯定有误差，我们就来看看各自的误差。陀螺仪的误差有两部分测量噪声和陀螺仪偏置，噪声暂时可以忽略（毕竟太小），而视觉的误差就只有观测噪声（也可以忽略不管），因此两者差值的绝对值就是陀螺仪偏置，将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型：

    公式15中第一个式子的第一项和第二项作四元数旋转的广义乘积就可以得到相机从bk到bk+1下的相对旋转（bk+1坐标系下），第三项是陀螺仪从bk+1到bk下的相对旋转（bk坐标系下），两者在做广义乘积，就是首先从bk到bk+1旋转，然后再从bk+1到bk旋转，相当于做差（OA+AO=0），第二个式子就是前面预积分提到的一阶线性近似。然后取最小二乘，当然也可以使用SVD分解等方法求解。注意在求得陀螺仪偏置之后要再次将陀螺仪偏置代入到预积分中再求一次预积分的值，会更加精确。

（1）      速度、重力加速度和尺度标定

作者在这里将这三个状态量统一到一个状态向量中，如公式16所示：

速度的是在bk坐标系下的，重力加速度在初始相机坐标系下，就像前面提到的，求解着几个量是由P、V数学模型求得，在滑动窗口中考虑到两个连续关键帧bk和bk+1，下面进行论文中公式17和19的推导：

    公式推导之后就会得到论文中的公式17、18和19，我们重点关注下为什么要这样推导，以及推导得到的运动方程关系。首先为什么要进行这样的推导，这完全取决于状态向量的定义方式，我们最终要得到的方程形式左边一定是以状态向量的形式来表达的，而且还要满足其他量都是已知的（从IMU预积分和视觉跟踪得到），因此就需要将方程进行如此的变化，才能满足这样的关系。然后是最后的形式我们可以看到状态向量最终的形式维度是（n+1）*3+3+1，两个连续帧产生的运动方程的维度是3+3+3+1（vbkbk，vbk+1bk+1，gc0，s），比较维度就可以看到最终得到的H矩阵一定是一个正定对称矩阵，因此可以采用快速的Cholesky分解。

 

（1）      重力优化

    上面其实已经得到了重力加速度的大小和方向，这里为什么还需要对重力进行优化呢？理由很简单，这里计算的重力吸收了重力加速度计的偏置，虽然不需要计算重力加速度计的偏置，但重力还是需要优化的，说到优化重力加速度，肯定包含两个量，大小和方向，也就是三个维度，但是一般来说大小是确定已知的（这里设为9.8），因此其实我们要做的就是优化方向，是一个两维的向量，下图是优化重力的方法以及b1,b2单位向量的方向确定模型。