VINS-Mono笔记03_预积分

最新推荐文章于 2023-10-24 08:27:47 发布

ncepu_Chen

最新推荐文章于 2023-10-24 08:27:47 发布

阅读量2.1k

点赞数 7

分类专栏： SLAM 文章标签：计算机视觉算法人工智能

本文链接：https://blog.csdn.net/ncepu_chen/article/details/122458244

版权

SLAM 专栏收录该内容

13 篇文章 742 订阅

订阅专栏

VINS-Mono笔记03_预积分

预积分理论
预积分代码实现

预积分理论

定义数学表示如下:

$(\cdot)^w$ 表示世界坐标系(world frame)下的值, $(\cdot)^b$ 表示机器人坐标系(body frame)下的值, $(\cdot)^c$ 表示相机坐标系下的值.
$\mathbf{q}^w_b$ , $\mathbf{p}^w_b$ 表示从机器人坐标系到世界坐标系的的旋转.
$b_k$ 表示第 $k$ 帧图像对应的机器人坐标, $c_k$ 表示第 $k$ 帧图像对应的相机坐标.
$\otimes$ 表示四元数乘法.
$\mathbf{g}^w = [0,0,g]^T$ 表示世界坐标系下的重力加速度值.
$\hat{(\cdot)}$ 表示物理量的测量值,即考虑噪声和零偏在内的值.

为什么需要预积分

IMU频率和图像频率不同,一般IMU频率高于图像频率,因此需要将两图像帧之间的所有IMU数据积分起来,得到两图像帧间的IMU测量值.

请添加图片描述

IMU可以输出3轴角速度和3轴加速度,其数学方程如下:
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \begin{split} …$

$\mathbf{a}_t$ 和 $\boldsymbol{\omega}_t$ 表示真实值.
$\hat{\mathbf{a}_t}$ 和 $\hat{\boldsymbol{\omega}}_t$ 表示陀螺仪输出的测量值.
$\mathbf{b}_{a_t}$ 和 $\mathbf{b}_{w_t}$ 表示零偏.
$\mathbf{n}_a$ 和 $\mathbf{n}_w$ 表示噪声, $\mathbf{n}_a \sim \mathcal{N}(\mathbf{0},\boldsymbol{\sigma}_a^{2})$ , $\mathbf{n}_w \sim \mathcal{N}(\mathbf{0},\boldsymbol{\sigma}_w^{2})$ .
IMU在静止不动时，输出垂直向下的重力加速度值 $\mathbf{g}^w$ .

连续时间下对 $b_k$ 帧到 $b_{k+1}$ 帧间的IMU测量值做积分,得到的位置 $\mathbf{p}^w_{b_{k+1}}$ 、速度 $\mathbf{v}^w_{b_{k+1}}$ 和姿态 $\mathbf{q }^{w}_{b_{k+1}}$ 的表达式如下.

$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \begin{split} …$

其中 $\boldsymbol{\Omega}(\boldsymbol{\omega})$ 和 $\lfloor \boldsymbol{\omega} \rfloor_{\times}$ 的定义如下:
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \begin{split} …$
上式中位置 $\mathbf{p}^w_{b_{k+1}}$ 和速度 $\mathbf{v}^w_{b_{k+1}}$ 的积分容易理解:可以类比中学物理路程和速度针对加速度的积分;姿态 $\mathbf{q }^{w}_{b_{k+1}}$ 的积分表达式的来源如下:
$\frac{d\mathbf{q}}{dt} = \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega})\mathbf{q}$
即被积表达式本身是位置四元数 $\mathbf{q}$ 对时间 $t$ 的微分.

连续时间下的预积分

在上述连续时间位置 $\mathbf{p}^w_{b_{k+1}}$ 和速度 $\mathbf{v}^w_{b_{k+1}}$ 积分表达式中都用到了世界坐标系下的姿态 $\mathbf{R}^w_t$ (亦即 $\mathbf{q }^{w}_t$ ),这依赖于 $b_k$ 帧的姿态 $\mathbf{R}^w_{b_k}$ ,一旦 $b_k$ 帧的姿态 $\mathbf{R}^w_{b_k}$ 改变,就要重新积分,增大运算量.

请添加图片描述

为防止重复积分的问题,将参考系转换至 $b_k$ 帧,即在上式左右两端同时乘以 $\mathbf{R}^{b_k}_w$ (亦即 $\mathbf{q}^{b_k}_w$ ),得到 $b_{k+1}$ 帧相对于 $b_k$ 帧的位置 $\mathbf{p}^{b_k}_{b_{k+1}}$ 、速度 $\mathbf{v}^w_{b_{k+1}}$ 和姿态 $\mathbf{q }^{w}_{b_{k+1}}$ 的表达式如下:
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \begin{split}\…$
其中积分式 $\boldsymbol{\alpha}^{b_k}_{b_{k+1}}$ 、 $\boldsymbol{\beta}^{b_k}_{b_{k+1}}$ 和 $\boldsymbol{\gamma}^{b_k}_{b_{k+1}}$ 被称为预积分量:
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \label{eq:inte…$
这样积分部分就只与IMU输出以及当前时刻相对于 $b_k$ 帧的姿态 $\mathbf{R}^{b_k}_{t}$ 有关.

离散时间下的预积分

离散时间下,预积分量的估计值表达式如下:
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \label{eq:prop…$
实际编程中使用中值积分方式,也就是将上式中IMU测量值 $\hat{\mathbf{a}}_i$ 、 $\hat{\boldsymbol{\omega}}_i$ 用连续两帧的均值 $\frac{\hat{\mathbf{a}}_i+\hat{\mathbf{a}}_{i+1}}{2}$ 、 $\frac{\hat{\mathbf{\omega}}_i+\hat{\mathbf{\omega}}_{i+1}}{2}$ 代替.

在实际编程中 $\hat{\boldsymbol{\alpha}}^{b_k}_{i}$ 和 $\hat{\boldsymbol{\beta}}^{b_k}_{i}$ 被初始化为零矩阵, $\hat{\boldsymbol{\gamma}}^{b_k}_{i}$ 被初始化为单位矩阵.

预积分量的协方差

使用误差卡尔曼滤波,可以得到连续时间下预积分量标准差的导数表达式如下:
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \begin{aligned…$
将其离散化,再加上亿点点数学推导,可以得到:

请添加图片描述

数学不好太要命了,像我这样的人可能一辈子都理解不了这个推导过程了.有人专门写了一篇论文来推导VINS-Mono论文里的数学表达式,见Formula Derivation and Analysis of the VINS-Mono.

在一些情况下,陀螺仪零偏 $\mathbf{b}_{a_t}$ 和 $\mathbf{b}_{w_t}$ 也是未知的,在这种情况下,VINS-Mono将陀螺仪零偏也作为待优化变量处理,这样就又带来了重复积分的问题:理论上每经过一次优化,零偏 $\mathbf{b}_{a_t}$ 和 $\mathbf{b}_{w_t}$ 值发生改变后,就要重新积分计算预积分量 $\boldsymbol{\alpha}^{b_k}_{b_{k+1}}$ 、 $\boldsymbol{\beta}^{b_k}_{b_{k+1}}$ 和 $\boldsymbol{\gamma}^{b_k}_{b_{k+1}}$ :

请添加图片描述

为解决这一问题,对预积分量 $\boldsymbol{\alpha}^{b_k}_{b_{k+1}}$ 、 $\boldsymbol{\beta}^{b_k}_{b_{k+1}}$ 和 $\boldsymbol{\gamma}^{b_k}_{b_{k+1}}$ 对于零偏 $\mathbf{b}_{a_t}$ 和 $\mathbf{b}_{w_t}$ 做一阶泰勒展开,在零偏值变化不大时使用一阶微分估计变化量,避免重新做积分运算.同样地,因为我不会数学,其数学推导略.
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ \label{eq:corr…$

预积分代码实现

预积分类`IntegrationBase`

预积分类实现在vins_estimator包下src/factor/integration_base.h文件内的IntegrationBase类中.主要成员变量如下:

请添加图片描述

成员变量	数据类型	意义
`linearized_acc`	`const Eigen::Vector3d`	$b_0$ 时刻(参与预积分的首个IMU帧)加速度测量值 $\hat{\mathbf{a}}_0$
`linearized_gyr`	`const Eigen::Vector3d`	$b_0$ 时刻(参与预积分的首个IMU帧)角速度测量值 $\hat{\boldsymbol{\omega}}_0$
`acc_0`	`Eigen::Vector3d`	$b_k$ 时刻加速度测量值 $\hat{\mathbf{a}}_k$
`acc_1`	`Eigen::Vector3d`	$b_{k+1}$ 时刻加速度测量值 $\hat{\mathbf{a}}_{k+1}$
`gyr_0`	`Eigen::Vector3d`	$b_k$ 时刻角速度测量值 $\hat{\boldsymbol{\omega}}_k$
`gyr_1`	`Eigen::Vector3d`	$b_{k+1}$ 时刻角速度测量值 $\hat{\boldsymbol{\omega}}_{k+1}$
`linearized_ba`	`Eigen::Vector3d`	加速度零偏 $\mathbf{b}_{a_i}$
`linearized_bg`	`Eigen::Vector3d`	陀螺角速度零偏 $\mathbf{b}_{a_i}$
`delta_p`	`Eigen::Vector3d`	预积分量 $\boldsymbol{\alpha}^{b_k}_{i+1}$
`delta_v`	`Eigen::Vector3d`	预积分量 $\boldsymbol{\beta}^{b_k}_{i+1}$
`delta_q`	`Eigen::Quaterniond`	预积分量 $\boldsymbol{\gamma}^{b_k}_{i+1}$
`dt_buf`	`std::vector<double>`	存储所有帧间的 $d_t$
`acc_buf`	`std::vector<Eigen::Vector3d>`	存储所有帧的加速度测量值 $\hat{\mathbf{a}}_i$
`gyr_buf`	`std::vector<Eigen::Vector3d>`	存储所有帧的角速度测量值 $\hat{\boldsymbol{\omega}}_i$
`covariance`	`Eigen::Matrix<double, 15, 15>`	见下图
`jacobian`	`Eigen::Matrix<double, 15, 15>`	见下图
`noise`	`Eigen::Matrix<double, 18, 18>`	见下图

请添加图片描述

预积分类的成员函数

构造函数`IntegrationBase::IntegrationBase()`

$\hat{\boldsymbol{\alpha}}^{b_k}_{i}$ 和 $\hat{\boldsymbol{\beta}}^{b_k}_{i}$ 被初始化为零矩阵, $\hat{\boldsymbol{\gamma}}^{b_k}_{i}$ 被初始化为单位矩阵;jacobian被初始化为单位矩阵,covariance被初始化为零矩阵,noise被初始化为单位矩阵乘以对应传感器的噪声方差(不确定度).

IntegrationBase(const Eigen::Vector3d &_acc_0,
                const Eigen::Vector3d &_gyr_0,
                const Eigen::Vector3d &_linearized_ba,
                const Eigen::Vector3d &_linearized_bg) : 
acc_0{_acc_0},
gyr_0{_gyr_0},
linearized_acc{_acc_0},
linearized_gyr{_gyr_0},
linearized_ba{_linearized_ba},
linearized_bg{_linearized_bg},
jacobian{Eigen::Matrix<double, 15, 15>::Identity()},
covariance{Eigen::Matrix<double, 15, 15>::Zero()},
sum_dt{0.0},
delta_p{Eigen::Vector3d::Zero()},
delta_q{Eigen::Quaterniond::Identity()},
delta_v{Eigen::Vector3d::Zero()} {
    noise = Eigen::Matrix<double, 18, 18>::Zero();
    noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
}

传入IMU数据帧时调用`IntegrationBase::push_back()`进行预积分

每传入一IMU帧,调用一次push_back()函数,其内部调用midPointIntegration()函数进行中值预积分:

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr) {
  // 1.记录对应IMU帧测量值
  dt_buf.push_back(dt);
  acc_buf.push_back(acc);
  gyr_buf.push_back(gyr);
  // 2. IMU传播
  propagate(dt, acc, gyr);
}

void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1) {
  // 1.记录本帧IMU帧测量值
  dt = _dt;
  acc_1 = _acc_1;
  gyr_1 = _gyr_1;
  Vector3d result_delta_p;
  Quaterniond result_delta_q;
  Vector3d result_delta_v;
  Vector3d result_linearized_ba;
  Vector3d result_linearized_bg;
  // 2.中值预积分
  midPointIntegration(_dt,
                      acc_0,
                      gyr_0,
                      _acc_1,
                      _gyr_1,
                      delta_p,
                      delta_q,
                      delta_v,
                      linearized_ba,
                      linearized_bg,
                      result_delta_p,
                      result_delta_q,
                      result_delta_v,
                      result_linearized_ba,
                      result_linearized_bg,
                      1);
  // 3. 更新成员变量
  delta_p = result_delta_p;
  delta_q = result_delta_q;
  delta_v = result_delta_v;
  linearized_ba = result_linearized_ba;
  linearized_bg = result_linearized_bg;
  delta_q.normalize();
  sum_dt += dt;
  acc_0 = acc_1;
  gyr_0 = gyr_1;
}

void midPointIntegration(double _dt,
                         const Eigen::Vector3d &_acc_0,
                         const Eigen::Vector3d &_gyr_0,
                         const Eigen::Vector3d &_acc_1,
                         const Eigen::Vector3d &_gyr_1,
                         const Eigen::Vector3d &delta_p,
                         const Eigen::Quaterniond &delta_q,
                         const Eigen::Vector3d &delta_v,
                         const Eigen::Vector3d &linearized_ba,
                         const Eigen::Vector3d &linearized_bg,
                         Eigen::Vector3d &result_delta_p,
                         Eigen::Quaterniond &result_delta_q,
                         Eigen::Vector3d &result_delta_v,
                         Eigen::Vector3d &result_linearized_ba,
                         Eigen::Vector3d &result_linearized_bg,
                         bool update_jacobian) {
  // 1.计算预积分量
  Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
  Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
  result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
  Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
  Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
  result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
  result_delta_v = delta_v + un_acc * _dt;
  result_linearized_ba = linearized_ba;
  result_linearized_bg = linearized_bg;

  // 2.计算协方差
  if (update_jacobian) {
    Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
    Vector3d a_0_x = _acc_0 - linearized_ba;
    Vector3d a_1_x = _acc_1 - linearized_ba;
    Matrix3d R_w_x, R_a_0_x, R_a_1_x;
    R_w_x << 0, -w_x(2), w_x(1), w_x(2), 0, -w_x(0), -w_x(1), w_x(0), 0;
    R_a_0_x << 0, -a_0_x(2), a_0_x(1), a_0_x(2), 0, -a_0_x(0), -a_0_x(1), a_0_x(0), 0;
    R_a_1_x << 0, -a_1_x(2), a_1_x(1), a_1_x(2), 0, -a_1_x(0), -a_1_x(1), a_1_x(0), 0;
    // 2.1. 计算矩阵F 
    MatrixXd F = MatrixXd::Zero(15, 15);
    F.block<3, 3>(0, 0) = Matrix3d::Identity();
    F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt
        + -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
    F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * _dt;
    F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
    F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
    F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
    F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3, 3) * _dt;
    F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt
        + -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
    F.block<3, 3>(6, 6) = Matrix3d::Identity();
    F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
    F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
    F.block<3, 3>(9, 9) = Matrix3d::Identity();
    F.block<3, 3>(12, 12) = Matrix3d::Identity();
    // 2.2. 计算矩阵V
    MatrixXd V = MatrixXd::Zero(15, 18);
    V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
    V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
    V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
    V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
    V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3, 3) * _dt;
    V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3, 3) * _dt;
    V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
    V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
    V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
    V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
    V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * _dt;
    V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * _dt;
	// 2.3. 更新jacobian和covariance
    jacobian = F * jacobian;
    covariance = F * covariance * F.transpose() + V * noise * V.transpose();
  }
}

请添加图片描述

零偏改变时调用`IntegrationBase::repropagate()`重新积分

零偏改变较大时调用repropagate()进行重新积分,其内部对xxx_buf中的每一帧数据调用propagate().

void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) {
  // 1. 重新初始化成员变量
  sum_dt = 0.0;
  acc_0 = linearized_acc;
  gyr_0 = linearized_gyr;
  delta_p.setZero();
  delta_q.setIdentity();
  delta_v.setZero();
  linearized_ba = _linearized_ba;
  linearized_bg = _linearized_bg;
  jacobian.setIdentity();
  covariance.setZero();
  // 2. 逐帧积分
  for (int i = 0; i < static_cast<int>(dt_buf.size()); i++) {
    propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
  }
}

优化过程中调用`IntegrationBase::evaluate()`计算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;

  //其中各增量关于 bias 的 Jacobian 可从公式(16)的大 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);

  //当每次迭代时,我们得到一个新的 bias,又得根据公式(5)重新对第 k 帧和第 k+1 帧之间的 IMU 预积分,非常耗时。这里假设预积分的变化量与 bias 是线性关系,可以写成:
  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;

  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;
}