相机提供丰富但低频、可能模糊的几何信息IMU 高频但会积分漂移。视觉惯性 SLAM 的关键不是把两条轨迹平均而是在同一状态和概率模型中联合估计。假设两个相邻关键帧i和 j之间相隔 0.5 秒IMU 频率为 200 Hz那么这两个关键帧之间大约有 100 组 IMU 测量。最直接的做法是从关键帧i 的状态开始逐个积分这 100 组 IMU 测量得到关键帧 j 的预测状态优化器修改关键帧 i 的状态后再重新积分一遍。问题在于非线性优化会反复更新关键帧的位姿、速度和零偏。如果每次更新都重新处理全部原始 IMU 数据计算量会很大。预积分的核心思想是把关键帧 i 到关键帧 j之间的全部 IMU 测量提前压缩成一个相对运动约束。这个约束主要包含分别表示相对旋转增量相对速度增量相对位置增量。它们还需要附带对 IMU 零偏的雅可比预积分测量的协方差总积分时间用于必要时重新积分的原始 IMU 测量。在 ORB-SLAM3 中这些内容主要由下面这个类保存ORB_SLAM3::IMU::Preintegrated一. 坐标系与旋转符号1.世界坐标系与IMU坐标系这一约定非常重要。ORB-SLAM3 在惯性预测和惯性残差中使用的核心旋转量就是 Rwb。例如在Tracking::PredictStateIMU()中Eigen::Matrix3f Rwb1 mLastFrame.GetImuRotation(); Eigen::Matrix3f Rwb2 IMU::NormalizeRotation( Rwb1 * mCurrentFrame.mpImuPreintegratedFrame -GetUpdatedDeltaRotation());它实现的是因此预积分旋转 ΔRij 是一个在关键帧 i 局部坐标意义下定义的相对旋转。2.叉乘矩阵ORB-SLAM3 中对应的函数是Eigen::Matrix3f skew(const Eigen::Vector3f v);位于ImuTypes.cc。后面的源码中经常看到const Eigen::Matrix3f Wacc Sophus::SO3f::hat(acc);这就是二. IMU 测量模型IMU 一般包含三轴陀螺仪三轴加速度计。陀螺仪测角速度加速度计测比力。这里最容易混淆的是加速度计直接输出的并不是世界坐标系中的线加速度而是去除重力后的比力在 IMU 坐标系中的表示。1.陀螺仪测量模型设则所以真实角速度为实际系统既不知道瞬时测量噪声 ng也不知道真实陀螺仪零偏 bg。ORB-SLAM3 使用当前零偏估计值对测量进行去偏:噪声的影响不直接从测量中减掉而是进入协方差传播。零偏估计值会在运行过程中不断优化更新初始设置为0可见ImuTypes.hpublic: // 默认构造零偏初始化为 0 Bias():bax(0),bay(0),baz(0),bwx(0),bwy(0),bwz(0){} // 按分量指定初始零偏 Bias(const float b_acc_x, const float b_acc_y, const float b_acc_z, const float b_ang_vel_x, const float b_ang_vel_y, const float b_ang_vel_z): bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z){} // 从另一个 Bias 对象拷贝数值 void CopyFrom(Bias b); // 支持用 直接打印零偏值便于调试 friend std::ostream operator (std::ostream out, const Bias b);也就是说系统刚启动时还不知道真实零偏于是先假设并在这个参考零偏下开始预积分。当视觉—惯性初始化以及后续优化得到更好的零偏估计后系统调用pKF-SetNewBias(b);或者pFrame-SetNewBias(b);把优化后的零偏写回关键帧或普通帧。ORB-SLAM3 中的对应实现单次 IMU 测量由IMU::Point保存class Point { public: Point(const float acc_x, const float acc_y, const float acc_z, const float ang_vel_x, const float ang_vel_y, const float ang_vel_z, const double timestamp); Eigen::Vector3f a; Eigen::Vector3f w; double t; };其中a加速度计测量w陀螺仪角速度测量t时间戳。在IntegratedRotation构造函数中源码执行const float x (angVel(0) - imuBias.bwx) * time; const float y (angVel(1) - imuBias.bwy) * time; const float z (angVel(2) - imuBias.bwz) * time; const Eigen::Vector3f v(x, y, z);对应注意 ORB-SLAM3 的Bias成员命名bwx,bwy,bwz陀螺仪零偏bax,bay,baz加速度计零偏。2.加速度计测量模型为什么是 aw-gw因为加速度计测的是比力将世界坐标系中的比力旋转到 IMU 坐标系再加上零偏和噪声就得到传感器输出。现在从测量模型反解世界坐标系线加速度。首先减去零偏和噪声左右同时左乘 Rwb因此由于ba和na同样受到瞬时影响在ORB-SLAM3的代码中实际计算中使用于是ORB-SLAM3 中的对应实现在Preintegrated::IntegrateNewMeasurement()中Eigen::Vector3f acc( acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz);对应如何将加速度转换为速度呢因本文面向对象为0基础学习者如果您已知晓上述问题请直接下滑到下方红字位置第一步将数学推导与代码实际参数统一第二 加速度减去零偏第k时刻加速度计给出的加速度测量值为amk由于加速度测量值中包含了零偏误差所以在源码中执行了“减去加速度计零偏”操作即Eigen::Vector3f acc( acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz);数学形式是因此注意此时的acc有三个重要特点它只是第 k 时刻的一次测量它仍然表达在第 k 时刻的 IMU 坐标系中它是去偏后的比力还没有加入重力。所以现在还不能直接把它加到世界速度上。第三步加速度乘以时间得到单次速度增量在最简单的一维运动中有因此第 k 个加速度测量产生的速度变化应该是但是这里出现了一个问题位于第 k 时刻的 IMU 坐标系中。不同时间的 IMU 坐标系方向可能不同。例如第一个时刻IMU 的 x 轴朝前第二个时刻IMU 已经旋转了 90°此时第二个测量的 x 轴可能已经朝左。所以不能直接执行因为这两个向量可能位于两个不同方向的坐标系中。必须先把所有测量旋转到同一个坐标系中然后才能相加。ORB-SLAM3 选择的公共坐标系是第四步用dR把当前加速度转到起点坐标系定义它把第 k 时刻 IMU 坐标系中的向量旋转到积分起点 i 的 IMU 坐标系中。因此在源码中定义这里acc当前 k 时刻 IMU 坐标系中的去偏加速度dR * acc转换到起点 i 坐标系中的去偏加速度。于是第 k 个 IMU 测量产生的局部速度增量为上述公式在源码中的对应为dR * acc * dt公式中的表示“一次 IMU 测量产生的小速度增量”而不是整个i-j区间的预积分速度。第五步把每一次小速度增量累加到dV源码中写为dV dV dR * acc * dt;实际的数学公式是这是一条递推公式。ORB-SLAM3 会对每一组 IMU 测量重复执行这条代码。下面将使用三组 IMU 测量把循环彻底展开假设从i时刻到j时刻之间只有三组IMU测量ii1i2也就是说ji3.在预积分刚开始时dV Eigen::Vector3f::Zero(); dR Eigen::Matrix3f::Identity();数学上表示为第i时刻测量的加速度为减去零偏由于当前时刻为起点所以代码执行dV dV dR * acc * dt;所以当进入i1时刻时此时IMU已经发生了旋转相对旋转可以表示为i1时刻的去偏测量为第二次执行dV dV dR * acc * dt;得到代入第一次的结果为同理当i2时刻此时得到的结果为因为ji3所以最终可以写为源码中的dV对应的是它表示从 i 到 j 之间由加速度计比力产生的速度变化并且这个速度变化表达在起点 i 的 IMU 坐标系中。它还不是世界坐标系中的速度变化。因此不能直接写成第六步把dV从起点IMU坐标系转到世界坐标系起点姿态为源码中定义为Rwb1因此就是世界坐标系中的“由加速度计比力产生的累计速度变化”。源码对应为Rwb1 * pImuPreintegrated-GetUpdatedDeltaVelocity()暂时忽略零偏更新修正可以把它理解为Rwb1 * dV即但是上述过程我们只考虑了IMU中加速度计提供的加速度信息但是实际上运动过程中还需要考虑重力g的影响。我们目前只考虑了aw但是实际上加速度计包括的是aw-gw所以我们之前通过dV累加得到的只是“去掉重力后的比力”带来的速度变化。完整的加速度应该表示为因此完整速度变化必须包含两部分重力产生的速度变化为在ORB-SLAM3源码中对应的是Gz * t12其中第七步最终获得的速度从起点速度 vi 出发终点速度 初始速度 整个区间的速度变化整个区间的速度变化又包含重力产生的速度变化2. IMU 比力产生的速度变化所以上述公式用通俗的语言表示可以为当前时刻的速度初始速度重力加速度× i-j时刻的时间积分起点i的IMU到世界坐标系的旋转×从i-j时刻的速度增量。源码中表示为Eigen::Vector3f Vwb2 Vwb1 Gz * t12 Rwb1 * pImuPreintegrated-GetUpdatedDeltaVelocity();3.从IMU测量值到连续时间内的运动方程在推导预积分之前我们先解决一个最基本的问题已知 IMU 输出的角速度和加速度怎样计算物体的姿态、速度和位置假设IMU在t时刻的瞬时状态估计可以表示为现在可以写出 IMU 的连续时间运动方程。姿态的变化由角速度决定。旋转矩阵的连续时间微分方程为速度对时间的导数就是世界坐标系中的线加速度因此将前面推导出的加速度公式代入所以位置对时间的导数就是速度陀螺仪零偏和加速度计零偏通常被建模成随机游走因此完整的连续时间运动模型为上面描述的是连续时间中的运动但计算机只能按照一小段一小段的时间间隔进行计算因此接下来需要把连续时间方程变成离散形式。4.从连续时间方程得到普通IMU离散积分假设 IMU 在时刻 tk 和 tk1 之间的时间间隔为Δtk为了简化推导假设在这个很短的时间段内角速度近似不变加速度计测量值近似不变IMU 零偏近似不变。ORB-SLAM3 在Tracking::PreintegrateIMU()中会使用相邻 IMU 测量的均值构造一个时间区间的代表测量。因此下面的可以理解为第 k 个积分区间内使用的角速度和加速度代表值。先看姿态。连续时间姿态方程为在时间区间 Δtk 内假设角速度不变于是可以将这个时间段内累计的旋转角写成通过 SO(3) 指数映射可以将旋转向量转换成旋转矩阵因此从 tk 到 tk1 的姿态更新为ORB-SLAM3 中对应的单步旋转计算为IntegratedRotation dRi(angVel, b, dt);IntegratedRotation内部首先计算const Eigen::Vector3f v( (angVel(0) - imuBias.bwx) * time, (angVel(1) - imuBias.bwy) * time, (angVel(2) - imuBias.bwz) * time);对应然后通过 Rodrigues 公式计算源码中的结果保存在dRi.deltaR再看速度。连续时间速度方程是根据积分的基本关系将速度微分方程代入把积分拆成两个部分重力在世界坐标系中近似为常量所以在一个很短的积分区间内近似使用区间起点姿态同时假设该区间内加速度计测量近似不变所以最终得到这条公式可以理解成其中位于 IMU 坐标系而将它旋转到了世界坐标系。最后看位置。位置对时间的导数是速度因此如果该时间段内世界坐标系加速度近似不变那么速度可以写成将速度表达式代入位置积分第一部分为第二部分为因此将代入将括号展开到这里我们得到了普通 IMU 离散积分的三条公式但是还有一个问题每一次积分都依赖世界坐标系中的当前姿态、速度和位置。如果优化器修改了关键帧起点的姿态、速度或位置那么从这个关键帧之后的全部 IMU 积分都可能需要重新计算。预积分的目的就是把高频 IMU 积分和关键帧起点的绝对状态分离开。5.如何把普通积分转换为IMU预积分呢假设关键帧 i 是积分起点现在要把 IMU 数据积分到时刻 k。关键帧 i 在世界坐标系中的状态为时刻 k 的状态为从 i 到 k 的总时间记为我们首先定义旋转预积分量6.三个预积分量的逐步递推ORB-SLAM3 中首先计算单次旋转增量IntegratedRotation dRi(angVel, b, dt);这里然后累积dR NormalizeRotation(dR * dRi.deltaR);对应其中更新前的 dR 是 ΔRk更新后的dR是 ΔRk1。ORB-SLAM3 中对应代码为dV dV dR * acc * dt;其中ORB-SLAM3 中对应代码为dP dP dV * dt 0.5f * dR * acc * dt * dt;源码必须先更新位置再更新速度dP dP dV * dt 0.5f * dR * acc * dt * dt; dV dV dR * acc * dt;dR NormalizeRotation(dR * dRi.deltaR);预积分的意义现在可以更准确地表述为在固定的参考零偏下把关键帧之间的高频 IMU 测量累积成位于积分起点局部坐标系中的相对旋转、相对速度和相对位置增量。这样IMU 的高频积分不再直接依赖起点在世界坐标系中的绝对姿态、速度和位置。IMU预积分过程中有几个自由度呢