后端优化 | VINS-Mono 论文公式推导与代码解析分讲
这是我们 SLAM 组同事高洪臣写的文章，感谢分享❤️❤️❤️～
3. 后端优化(紧耦合)
VIO 紧耦合方案的主要思路就是通过将基于视觉构造的残差项和基于IMU构造的残差项放在一起构造成一个联合优化的问题，整个优化问题的最优解即可认为是比较准确的状态估计。
为了限制优化变量的数目，VINS-Mono 采用了滑动窗口的形式，滑动窗口 中的 全状态量：
- 滑动窗口内 n+1 个所有相机的状态(包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias)
- Camera 到 IMU 的外参
- m+1 个 3D 点的逆深度
优化过程中的误差状态量
进而得到系统优化的代价函数（Minimize residuals from all sensors）
其中三个残差项依次是
- 边缘化的先验信息
- IMU 测量残差
- 视觉的观测残差
三种残差都是用 马氏距离（与量纲无关） 来表示的。
Motion-only visual-inertial bundle adjustment: Optimize position, velocity, rotation in a smaller windows, assuming all other quantities are fixed
3.1 IMU 测量残差
（1）IMU 测量残差
上面的IMU预积分（估计值 - 测量值），得到 IMU 测量残差
/**
* [evaluate 计算IMU测量模型的残差]
* @param Pi，Qi，Vi，Bai，Bgi [前一次预积分结果]
* @param Pj，Qj，Vj，Baj，Bgj [后一次预积分结果]
*/
Eigen::Matrix<double, 15, 1> evaluate(
const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
// IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
// IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求'diff'得到residual
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}（2）协方差矩阵
此处用到的协方差矩阵为前面 IMU 预积分计算出的协方差矩阵。
残差的后处理对应代码：
// 在优化迭代的过程中, 预积分值是不变的, 输入的状态值会被不断的更新, 然后不断的调用evaluate()计算更新后的IMU残差
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
Eigen::Matrix<double, 15, 15> sqrt_info =
Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual; // 为了保证 IMU 和 视觉參差项在尺度上保持一致，一般会采用与量纲无关的马氏距离这里残差 residual 乘以 sqrt_info，这是因为真正的优化项其实是 Mahalanobis 距离:d= r^{T} P^{-1} r , 其中 P_{} 是协方差。Mahalanobis距离其实相当于一个残差加权，协方差大的加权小，协方差小的加权大，着重优化那些比较确定的残差。
而 ceres 只接受最小二乘优化，也就是 min e^{T}e_{} ,所以把 P^{-1} 做 LLT分解，即 LL^{T}=P^{-1} ，则 d= r^{T}\left( LL^{T} \right)r=\left( L^{T} r\right)^{T}\left( L^{T}r \right) ，令 r ′=( L^{T}r )，作为新的优化误差，所以 sqrt_info 等于 L^{T}
（3）雅克比矩阵
高斯迭代优化过程中会用到IMU测量残差对状态量的雅克比矩阵，但此处我们是 对误差状态量求偏导，下面对四部分误差状态量求取雅克比矩阵。
雅克比矩阵计算的对应代码在class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>中的Evaluate()函数中。
3.2 视觉(td) 测量残差
视觉测量残差 即 特征点的重投影误差，视觉残差和雅克比矩阵计算的对应代码在 ProjectionFactor::Evaluate 函数中。
（1）切平面重投影误差（Spherical camera model）
// 将第i frame下的3D点转到第j frame坐标系下
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; // pt in ith camera frame, 归一化平面
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; // pt in ith body frame
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; // pt in world frame
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); // pt in jth body frame
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); // pt in jth camera frame（2）像素重投影误差（Pinhole camera model）
Eigen::Map<Eigen::Vector2d> residual(residuals);
#ifdef UNIT_SPHERE_ERROR
// 把归一化平面上的重投影误差投影到Unit sphere上的好处就是可以支持所有类型的相机 why
// 求取切平面上的误差
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
// 求取归一化平面上的误差
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
residual = sqrt_info * residual; // 转成 与量纲无关的马氏距离（3）协方差矩阵
固定的协方差矩阵，归一化平面的标准差为 \frac{1.5}{f} ，即像素标准差为 1.5。
ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();（4）雅克比矩阵
下面关于误差状态量对相机测量残差求偏导，得到高斯迭代优化过程中的雅克比矩阵。
（5）Vision measurement residual for temporal calibration
视觉残差和雅克比矩阵计算的对应代码在 ProjectionTdFactor::Evaluate 函数中。
// TR / ROW * row_i 是相机 rolling 到这一行时所用的时间
Eigen::Vector3d pts_i_td, pts_j_td;
pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;
Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map<Eigen::Vector2d> residual(residuals);
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
#endif
residual = sqrt_info * residual;- 添加对 imu-camera 时间戳不完全同步和 Rolling Shutter 相机的支持：通过前端光流计算得到每个角点在归一化的速度，根据 imu-camera 时间戳的时间同步误差和Rolling Shutter相机做一次rolling的时间，对角点的归一化坐标进行调整
3.3 Temporal Calibration
Timestamps
Time Synchronization
Temporal Calibration
- calibrate the fixed latency t_{d} occurred during time stamping
- change the IMU pre-integration interval to the interval between two image timestamps
- linear incorporation of IMU measurements to obtain the IMU reading at image time stamping
- estimates states(position, orientation, etc.) at image time stamping
3.4 边缘化(Marginalization)
SLAM is tracking a noraml distribution through a large state space
滑窗（Sliding Window）限制了关键帧的数量，防止pose和feature的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长。
Marginalization
然而，将pose移出windows时，有些约束会被丢弃掉，这样势必会导致求解的精度下降，而且当MAV进行一些退化运动(如: 匀速运动)时，没有历史信息做约束的话是无法求解的。所以，在移出位姿或特征的时候，需要将相关联的约束转变成一个约束项作为prior放到优化问题中，这就是marginalization要做的事情。
边缘化的过程就是将滑窗内的某些较旧或者不满足要求的视觉帧剔除的过程，所以边缘化也被描述为 将联合概率分布分解为边缘概率分布和条件概率分布的过程(就是利用shur补减少优化参数的过程)。
直接进行边缘化而不加入先验条件的后果：
- 无故地移除这些 pose 和 feature 会丢弃帧间约束，会降低了优化器的精度，所以在移除pose 和 feature 的时候需要将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中。
- 在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失，尤其是系统在进行退化运动时(如无人机的悬停和恒速运动)。一般来说 只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的可观性。
VINS-Mono 中为了处理一些悬停的case，引入了一个 two-way marginalization：
- MARGIN_OLD：如果次新帧是关键帧，则丢弃滑动窗口内最老的图像帧，同时对与该图像帧关联的约束项进行边缘化处理。这里需要注意的是，如果该关键帧是观察到某个地图点的第一帧，则需要把该地图点的深度转移到后面的图像帧中去。
- MARGIN_NEW：如果次新帧不是关键帧，则丢弃当前帧的前一帧。因为判定当前帧不是关键帧的条件就是当前帧与前一帧视差很小，也就是说当前帧和前一帧很相似，这种情况下直接丢弃前一帧，然后用当前帧代替前一帧。为什么这里可以不对前一帧进行边缘化，而是直接丢弃，原因就是当前帧和前一帧很相似，因此当前帧与地图点之间的约束和前一帧与地图点之间的约束是很接近的，直接丢弃并不会造成整个约束关系丢失信息。这里需要注意的是，要把当前帧和前一帧之间的 IMU 预积分转换为当前帧和前二帧之间的 IMU 预积分。
在悬停等运动较小的情况下，会频繁的 MARGIN_NEW ，这样也就保留了那些比较旧但是视差比较大的 pose 。这种情况如果一直 MARGIN_OLD 的话，视觉约束不够强，状态估计会受 IMU 积分误差影响，具有较大的累积误差。
Schur Complement
- Marginalization via Schur complement on information matrix
First Estimate Jacobin
更多 SLAM 内容：
点赞 和分享 是对我们的最大鼓励呀～