VINS-Mono笔记03_预积分

最新推荐文章于 2024-04-22 18:52:37 发布

ncepu_Chen

最新推荐文章于 2024-04-22 18:52:37 发布

阅读量1.8k

点赞数 7

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

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

版权

SLAM 专栏收录该内容

13 篇文章 713 订阅

订阅专栏

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

ncepu_Chen

关注

7
点赞
踩
20

收藏

觉得还不错? 一键收藏
9
评论
VINS-Mono笔记03_预积分

VINS-Mono笔记03_预积分预积分理论为什么需要预积分连续时间下的预积分离散时间下的预积分预积分量的协方差预积分代码实现预积分类`IntegrationBase`预积分类的成员函数构造函数`IntegrationBase::IntegrationBase()`传入IMU数据帧时调用`IntegrationBase::push_back()`进行预积分零偏改变时调用`IntegrationBase::repropagate()`重新积分优化过程中调用`IntegrationBase::evaluate.
复制链接

扫一扫