Vins-mono 源码笔记 (8) 后端优化各种残差Factor总结

前言

非线性最小二乘法优化问题求解可以总结：
根据当前残差优化状态X即求解下式:

其中J为jacobian矩阵 , rB 为当前残差, P 为残差各分量协方差矩阵, 求逆后变为了信息矩阵, 表示权重.
这个式子的参考下面:

整个优化问题 可以表述为:

对每一个残差给一个扰动, 扰动后的值为:

对扰动求导即得:

因此对于一个残差factor， 我们需要获得 其关于优化状态的jacobian矩阵 , 当前残差rB, 残差协方差矩阵P。

1. IMU Factor

IMU factor的定义是:

// ceres 中每个Factor 都是  CostFunction 类的子类       7,9 分别表示 姿态, 速度偏置的维度 
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
{
 ....... 
}

它是 SizedCostFunction 类的子类, 同时也是 CostFunction 的子类.
其中, 15表示残差的维度, 7,9,7,9 表示该残差关联的4个状态变量的维度.

类中对

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const

进行了重载, 其中 residuals 为当前预积分残差, jacobians为残差关于状态的jacobian矩阵.

下面是Evaluate()的分析.

Evaluate()

优化时,每一次迭代都会对状态量进行调整，每调整一次， 都需要在这个函数里完成 jacobian 以及残差的计算.

1. 计算预积分残差 - 残差最后为 residual = LT*r , LT为预积分协方差矩阵的逆LLT分解的矩阵

// 计算预积分残差   
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                    Pj, Qj, Vj, Baj, Bgj);
// 首先对预积分协方差的逆进行LLT分解    matrixL()表示获取L       这里获取LT  
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;      // LT*r

其中 pre_integration->evaluate()函数的定义在 integration_base.h ，通过该函数完成基于当前状态量的残差求解。

// 计算IMU观测残差
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;
    // 获取P V Q 关于 ba   bg的jacobian     
    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;
    // 求偏置改变后的预积分量   
    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残差   
    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;
}

该函数计算的就是这个残差
由于在优化过程当中, 偏置的值会发生改变, 而预积分量是在执行优化之前就已经求出来的, 因为预积分量会随着偏置的变化而变化, 所以 如果直接用以前的预积分量计算残差,肯定会不准, 因此 首先需要根据bias来更新预积分, 利用更新后的预积分去计算残差.

注意这里通过泰勒展开更新预积分量只是为了求解残差 而并不会真正更新原来的预积分值 .

2. 计算IMU残差的jacobian - jacobian = LT*J, LT为预积分协方差矩阵的逆LLT分解的矩阵

这样的话, ceres中采用的:
$$J^{T}Jx=J^{T}r$$

也就是:
$$J^{T}L{L}^{T}Jx=J^{T}L{L}^{T}r$$

其中 $L{L}^T={\sigma }^{-1}$ 为协方差矩阵的逆

2. 视觉残差

主要分析考虑时间戳对齐的视觉残差jacobian.

3. 先验残差