1.一种同时优化外参数的视觉惯性里程计的方法，其特征包括以下步骤：第1，方法描述

第1.1，方法框架

首先，通过运动恢复结构算法(SFM)获得两个连续相机帧之间的旋转和平移，该算法仅用于获得相机关键帧之间关系的；然后，采用IMU预积分算法来获得IMU两帧之间的差量，并在之前获得估计变量的基础上估算当前IMU关键帧的测量值；通过运动恢复结构和IMU预积分算法获得的相关关键帧的旋转 和 利用经典算法，使用收敛准则获得外部参数的旋转部分；其次，估计陀螺仪偏差、平移外参数、绝对尺度和重力向量，并重新纠正重力的方向；最后，每一帧的速度可以通过先前的状态估计计算得到；加速度计偏差通过后端VIO紧耦合优化进行补偿；

第1.2，IMU预积分

IMU的测量数据包括三轴角速度 和三轴线性加速度 偏差b和白噪声n影响加速度和角速度的精度；测量值和理想值之间的关系如下：wb和ab分别为 和 的实际值， 和 分别为线性加速度和角速度的偏差；nw和na分别为wwb和ab的高斯白噪声，服从高斯分布； 是世界坐标系 到IMU坐标系 的旋转矩阵，g 是世界坐标系 下的重力向量，所以 表示的是重力向量在IMU坐标系 下的值；

忽略噪声，IMU连续两帧 和 在时间间隔[tk，tk+1]内的位移、速度和旋转表示如下：把公式(1)代入公式(2)，并从 变换到 得到IMU预积分公式如下：公式(3)中， 和 分别为两帧之间的位移、速度、旋转差量；由公式可以看出惯性测量单元的当前关键帧的测量值与之前帧无关，仅由 和 决定；

第2，估计旋转外参数

第2.1，运动恢复结构

视觉初始化使用运动恢复结构(SFM)方法估计滑动窗口内的所有关键帧的位姿；首先，将第一帧相机帧 作为第一个参考关键帧，然后选择一个与当前帧具有更多公共点和足够视差的帧作为第二个参考关键帧；通过五点法或八点法估算当前帧和参考关键帧之间的旋转和平移；然后，利用三角化和PnP算法以及捆集调整(BA)获得滑动窗口内所有关键帧的位姿；在此基础上，得到第k个和第k+1个摄像机关键帧之间的旋转 和缩放平移第2.2，旋转外参数根据坐标系之间的变换，可得到以下关系：

其中 由IMU陀螺仪在IMU坐标系 下积分得到， 通过视觉初始化得到， 是需要估计的旋转外参数矩阵；

把以上旋转矩阵用四元数表示得到以下公式：

将Q1和Q2定义为四元数的左乘和右乘，得到以下公式：变换后得到：

随着关键帧数量的增加，得到以下超线性方程组：其中N为关键帧数量的最大值， 为剔除重投影误差的权重，利用终止准则可以得到旋转外参数；

第3，视觉惯性初始化

第3.1，陀螺仪偏差校正

在相机坐标系 下，设置第一个参考关键帧后，通过SFM算法得到第k帧和参考关键帧之间的旋转 然后 可以由以下公式得到：对于滑动窗口内的两个IMU关键帧 和 和 可由公式(9)得到，相对旋转 由公式(3)获得；因此陀螺仪偏差的目标方程表示如下：其中

因此，最小化公式(10)可以得到陀螺仪偏差bw；相对位移 和相对速度 根据即时计算得出的bw进行更新；

第3.2，平移外参数、绝对尺度和重力向量

通过以上分析，所有的参数都可以从IMU坐标系 转换到相机坐标系 下：其中s是尺度因子，是缩放的位移；绝对尺度和缩放尺度之间的关系如下：所以(3)式可写成如下形式：

通过公式(11)(12)(13)， 和 可以被表示为：假设关键帧的下标i，i+1，i+2分别为1，2，3，并把(15)式的两个式子放在一起得到：其中：

通过公式(16)，利用最小二乘算法可得到平移外参数 重力向量 和绝对尺度s；

第3.3，重力方向矫正

估计出的重力向量受加速度计偏差的影响，假设重力向量的幅值是一个常量，重力向量的方向由于偏差而发生改变；因此，此时矫正重力向量可以设为两个自由度：其中 为重力向量的幅值，w1b1和w2b2是分别来自两个正交方向的扰动；b为扰动的方向，w为对应的b方向的扰动量；

把公式(18)代入(16)得到：

其中：

通过求解公式(20)，重力向量被重新矫正；在此基础上， 和 坐标系下的所有状态变量都可以变换到世界坐标系 下；此外，平移和地图点的坐标可以根据得到的绝对尺度s进行缩放；

第3.4，重力方向矫正

重力和绝对尺度得到后，每一关键帧的速度可以由公式(2)和(15)计算得到；在视觉惯性初始化时没有估计加速度计偏差ba，因为它对初始化的影响并不是很大，并且会在后端的VIO紧耦合优化中被估计出来。