程序员经验分享-hwhale

GTSAM中imu预积分及其因子图优化过程

2023-05-16

前言

使用IMU和llidar或者相机进行多传感器融合的slam方案中，主要分为紧耦合和松耦合方案。目前，主流的方案都是紧耦合的。而紧耦合方案中主要分为基于滤波（比如，ESKF）和基于优化的方案。
本文则介绍基于优化方案中IMU的预积分理论以及其在GTSAM中的实现。

IMU预积分理论：

简介

IMU的使用一般是对其测得的加速度，角速度进行积分，从而推算出机器人的位姿。但是，由于积分关系，IMU的积分所得的位姿飘移会随着积分时间的增大而增大。另一方面，当优化方法优化了历史时刻的位姿之后，之后时刻的IMU积分值需要重新进行积分，计算量大。因此，IMU预积分就被提出，以解决以上问题。IMU预积分简单来说就是描述了lidar帧（或者相机帧）之间（时间间隔比较短，比如lidar帧间间隔通常为100ms），IMU数据的观测量。其只跟上一帧时刻的IMU状态量相关，因此，在进行优化过程中，计算量较小。作为观测量，IMU预积分自然是作为一个约束用于slam位姿优化。

公式推导

IMU模型：
在这里插入图片描述
其中， w b w^b wb是IMU坐标系中角速度的真实值。 a w a^w aw是世界坐标系中加速度真实值。 b g b^g bg, b a b^a ba为IMU偏置，用随机游走建模。 n g n^g ng, n a n^a na为测量值的噪声。 a ~ b \widetilde{a}^b a b, w ~ b \widetilde{w}^b w b为IMU测量值。
根据积分关系，可以得到位置，速度和姿态的迭代公式。
在这里插入图片描述
又因为
在这里插入图片描述
那么，PVQ积分公式中的积分项则变为相对于第i时刻的姿态，而不是相对于世界坐标系的姿态：
在这里插入图片描述
进而定义预积分量：
在这里插入图片描述
移项整理，构建残差：
在这里插入图片描述
此时，获得了优化过程中需要的残差，残差对相应的状态量求导可得雅可比矩阵。但是，缺少了优化过程需要的协方差矩阵。因此，需要推导出imu噪声协方差和预积分协方差的关系。
根据预积分的定义，使用中值积分方法对其进行离散化，可得：
在这里插入图片描述
根据上式可以发现，预积分量和imu测量值之间是非线性关系，直接通过该方程求得协方差传播不现实，需要进行线性化。
根据EKF的线性化过程，对上述方程进行一阶泰勒展开，可得：
在这里插入图片描述
其中，F,G分别是方程 f ( x k − 1 , u k − 1 ) f(x_{k-1},u_{k-1}) f(xk−1​,uk−1​)在 x ^ k − 1 \hat{x}_{k-1} x^k−1​,噪声项为0处的雅可比矩阵。
由于误差量的协方差矩阵和原随机变量的协方差矩阵相同。因此，预积分量的协方差矩阵可以通过F,G雅可比矩阵进行传播。

GTSAM中IMU预积分实现及其因子图构建

简介

gtsam是通过因子图这个模型实现优化的。因此，在使用gtsam解决一个优化问题时，就需要构建相应的因子图。
在因子图中，图的节点就是待优化的变量，因子就是相应的观测约束。而节点的值（初始值）需要一个类Value来保存。
gtsam优化过程简单描述就是优化方法（比如，GN，ISAM）调用因子图以及相应图节点的值，计算因子（观测约束）的残差，因子（观测约束）对相关图节点（相关量）的雅可比，完成迭代优化。
因此，gtsam中因子的类（比如，ImuFactor）会有相应的求取残差以及雅可比的函数（比如，linearize()函数，这个函数会被重载）。

IMU预积分类

gtsam中的一种IMU预积分实现类是PreintegratedImuMeasurements。其中，

void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
    const Vector3& measuredOmega, double dt) {
  Matrix9 A;
  Matrix93 B, C;
  update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}

这个成员函数，接受IMU的加速度，角速度测量值以及imu测量值之间的时间间隔。然后，计算相应的预积分量和噪声传播的雅克比矩阵。

NavState PreintegrationBase::predict(const NavState& state_i,
    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
    OptionalJacobian<9, 6> H2) const {
  Matrix96 D_biasCorrected_bias;
  Vector9 biasCorrected = biasCorrectedDelta(bias_i,
                                             H2 ? &D_biasCorrected_bias : nullptr);

  Matrix9 D_delta_state, D_delta_biasCorrected;
  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
                                  p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr,
                                  H2 ? &D_delta_biasCorrected : nullptr);

  Matrix9 D_predict_state, D_predict_delta;
  NavState state_j = state_i.retract(xi,
                                     H1 ? &D_predict_state : nullptr,
                                     H2 || H2 ? &D_predict_delta : nullptr);
  if (H1)
    *H1 = D_predict_state + D_predict_delta * D_delta_state;
  if (H2)
    *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
  return state_j;
}

这个成员函数根据预积分量以及输入的PVQ,Bias进行当前时刻imu积分值计算。计算得到的结果可以作为相应节点的初始值。

gtsam因子图构建

NonlinearFactorGraph* graph = new NonlinearFactorGraph();
graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);

先实例化因子图对象，而后加入先验节点。

std::shared_ptr<PreintegrationType> preintegrated =
    std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);

创建imu预积分对象。

// Adding the IMU preintegration.
preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);

根据imu观测值的输入，进行imu预积分计算。直到获得位姿观测值。

auto preint_imu =
  dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
                   X(correction_count), V(correction_count),
                   B(correction_count - 1), preint_imu);
graph->add(imu_factor);
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(
  B(correction_count - 1), B(correction_count), zero_bias,
  bias_noise_model));

当获得位姿观测值（比如，GPS数据，lidar匹配的位姿等）时，就利用imu预积分对象构建imu因子。

auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
GPSFactor gps_factor(X(correction_count),
                     Point3(gps(0),   // N,
                            gps(1),   // E,
                            gps(2)),  // D,
                     correction_noise);
graph->add(gps_factor);

同时，利用位姿观测值构建相应的因子。
至此，构建的因子图，如下所示：
在这里插入图片描述
其中，X节点是位姿，V节点是速度，B节点是bias。
然后，创建优化方法类对象，使用构建的因子图和初始值进行迭代优化。

LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
result = optimizer.optimize();

GTSAM因子图优化源码实现：

gtsam中NonlinearConjugateGradientOptimizer这个优化器的optimize()源码如下所示：

const Values& NonlinearConjugateGradientOptimizer::optimize() {
  System system(graph_);
  Values newValues;
  int iterations;
  boost::tie(newValues, iterations) =
      nonlinearConjugateGradient(system, state_->values, params_, false);
  state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations));
  return state_->values;
}

其中，nonlinearConjugateGradient函数就是调用因子图中的因子类对象，计算残差和相应的雅可比矩阵。然后，使用该优化器的优化算法进行节点变量优化。具体说明雅可比矩阵的实现过程。

static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg,
    const Values &values) {
  // Linearize graph
  GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
  return linear->gradientAtZero();
}

其中，主要是因子图对象调用linearize()函数。linearize()函数部分源码如下所示：

GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const
{
  gttic(NonlinearFactorGraph_linearize);

  // create an empty linear FG
  GaussianFactorGraph::shared_ptr linearFG = boost::make_shared<GaussianFactorGraph>();

#ifdef GTSAM_USE_TBB

  linearFG->resize(size());
  TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP

  // First linearize all sendable factors
  tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
    _LinearizeOneFactor(*this, linearizationPoint, *linearFG));

  // Linearize all non-sendable factors
  for(size_t i = 0; i < size(); i++) {
    auto& factor = (*this)[i];
    if(factor && !(factor->sendable())) {
      (*linearFG)[i] = factor->linearize(linearizationPoint);
    }
  }
  return linearFG;
}

其中，有使用tbb多线程，访问因子图中的因子，调用因子对象中的linearize()函数。而对于ImuFactor这个因子对象，由于其是NoiseModelFactor的子类。因此，调用了NoiseModelFactor的linearize()函数，其源码如下所示：

boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
    const Values& x) const {

  // Only linearize if the factor is active
  if (!active(x))
    return boost::shared_ptr<JacobianFactor>();

  // Call evaluate error to get Jacobians and RHS vector b
  std::vector<Matrix> A(size());
  Vector b = -unwhitenedError(x, A);
  check(noiseModel_, b.size());

  // Whiten the corresponding system now
  if (noiseModel_)
    noiseModel_->WhitenSystem(A, b);

  // Fill in terms, needed to create JacobianFactor below
  std::vector<std::pair<Key, Matrix> > terms(size());
  for (size_t j = 0; j < size(); ++j) {
    terms[j].first = keys()[j];
    terms[j].second.swap(A[j]);
  }

  // TODO pass unwhitened + noise model to Gaussian factor
  using noiseModel::Constrained;
  if (noiseModel_ && noiseModel_->isConstrained())
    return GaussianFactor::shared_ptr(
        new JacobianFactor(terms, b,
            boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
  else
    return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}

而其中的unwhitenedError函数，则是计算残差和雅可比矩阵的函数，该函数最终调用imu预积分类PreintegratedImuMeasurements中的残差计算，雅可比矩阵求导函数computeErrorAndJacobians。
在这里插入图片描述至此，摸索清楚了从优化器到残差，雅可比矩阵计算的整个计算流程。
才疏学浅，如有错误，欢迎批评指正！

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

GTSAM

IMU

预积分及其因子图优化过程

GTSAM中imu预积分及其因子图优化过程 的相关文章

  • IMU方向位姿估计

    系列文章目录 提示 xff1a 这里可以添加系列文章的所有文章的目录 xff0c 目录需要自己手动添加 TODO 写完再整理 文章目录 系列文章目录前言一 方法一 xff1a IMU方向位姿可以直接从IMU本身提供的专有算法中获得 xff0

  • IMU与GPS传感器ESKF融合定位

    IMU与GPS传感器ESKF融合定位 文章目录 IMU与GPS传感器ESKF融合定位1 代码整体框架说明2 主要函数介绍2 1 LocalizationWrapper构造函数2 2 滤波算法进行预测2 3 通过GPS位置测量数据更新系统的状

  • STM32—驱动GY85-IMU模块

    GY85是一个惯性测量模块 xff0c 内部集成了三轴加速度计 三轴陀螺仪 电子罗盘 气压传感器等芯片 xff0c 用于测量和报告设备速度 方向 重力 xff0c 模块可以将加速度计 陀螺仪 电子罗盘等传感器的数据进行综合 xff0c 在上

  • Mavros读取PixHawk硬件的IMU数据

    Ubuntu18 04 读取PixHawk硬件的IMU数据 实现方式 使用mavros话题读取到Pixhawk飞控的IMU数据 实现步骤 安装ros 检查是否安装cmake xff08 未安装根据提示安装 xff09 cmake span

  • realsense D435i双目IMU 数据集

    realsense D435i 双目IMU数据集 使用双目 43 IMU的数据双目内参双目IMU外参 使用双目 43 IMU的数据 双目内参 model type PINHOLE camera name camera image width

  • VINS slam , imu fusion

    VINS 基本介绍 VINS Mono 和 VINS Mobile 是香港科技大学沈劭劼老师开源的单目视觉惯导 SLAM 方案 2017年发表于 IEEE Transactions on Robotics 另外 xff0c VINS 的最新

  • ROS2中IMU话题的发布及可视化

    环境 xff1a Ubuntu 20 04 xff0c ROS2 Foxy 传感器 xff1a 维特智能BWT901CL 代码是从维特智能的示例代码修改的 xff0c 实现基本的加速度 角速度和角度读取 xff0c 发布IMU消息 这个传感

  • 在ROS下Intel RealSense D435i 驱动的安装，避免踩坑，避免缺少imu话题等各种问题（适用于D400系列、SR300和T265跟踪模块等）

    版权声明 本文为博主原创文章 未经博主允许不得转载 https blog csdn net AnChenliang 1002 article details 109454465 目录 背景 方法1 使用apt安装 不建议使用此方法 了解一下

  • 再谈IMU数据处理（滤波器）

    本文开始前 xff0c 先回答一个问题 上一篇文章最后提到了卡尔曼滤波器用来做一维数据的数字滤波处理 xff0c 最终的实验结果说 xff1a 该模型下的卡尔曼滤波处理与二阶IIR低通滤波处理效果几乎一致 有网友指出是错误的 xff0c 卡

  • vins中imu融合_VINS代码解读

    VINS estimator 摘抄 我们初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统 xff0c 首先单目是无法获得空间中的绝对尺度 xff0c 而IMU又必然存在偏置 xff0c 在后面进行求解的时候还需要用到重力加速度 包

  • ZED 2i 双目-IMU标定

    目录 xff1a 前言IMU标定1 编译标定工具2 准备数据集3 标定 Camera IMU标定1 安装依赖2 编译Kaibr3 制作标定板下载标定板生成标定板target yaml文件 4 数据采集5 相机标定标定中遇到的问题问题1 xf

  • IMU让无人机控制变得更轻松

    多翼无人机广泛应用于监视和侦察 航空摄影和测量 搜索和救援任务 通信中继和环境监测 目前无人机的手动控制大部分基于视觉反馈 xff0c 所以操作环境中的障碍物会造成干扰 因此 xff0c 需要其他感官反馈 xff0c 例如触觉 xff0c

  • 最小的IMU模组——DETA10系列

    性能全面升级 飞迪航空 xff08 FDISYSTEMS xff09 DETA10系列产品目前出货已达十万量级 xff0c 广泛应用于机器人 可穿戴设备 人工智能教育套件 自动驾驶小车 智慧农业 扫地机器人 稳定平台 无人系统等相关领域 D

  • IMU 测量模型和运动学模型

    一 概念 高斯白噪声 测量噪声是AD转换器件引起的外部噪声 xff0c 波动激烈的测量白噪声 随机游走 这里指零偏Bias 随机游走噪声 xff0c 是传感器内部机械 温度等各种物理因素产生的传感器内部误差的综合参数 xff0c 是变化缓慢

  • vins-fusion代码解读[五] imu在vins里的理解

    SLAM新手 xff0c 欢迎讨论 IMU作用 vins中 xff0c IMU只读取IMU六轴的信息 xff0c 3轴线加速度 xff08 加速度计 xff09 和3轴角速度 xff08 陀螺仪 xff09 通过对陀螺仪的一次积分 xff0

  • 一文了解IMU原理、误差模型、标定、惯性传感器选型以及IMU产品调研(含IMU、AHRS、VRU和INS区别)

    在此记录一下测试IMU过程中的其它文章 xff0c 便于以后查看 xff1a IMU的误差标定以及姿态解算ROS下通过USB端口读取摄像头数据 包括笔记本自带摄像头 激光 摄像头 IMU等传感器数据同步方法 message filters

  • 武汉大学研究生组合导航课程合集【2022年春】

    第四公式中kk是权重 zk hx 为innovation新息 即真实的观测 估计的观测 前者包含观测误差 gps的电离层 多径 后者包含估计误差 kk近似1 则无限相信新观测 kk 0 相信估计

  • IMU+摄像头实现无标记运动捕捉

    惯性传感和计算机视觉的进步为在临床和自然环境中获得精准数据带来了新可能 然而在临床应用时需要仔细地将传感器与身体对齐 这减慢了数据收集过程 随着无标记运动捕捉的发展 研究者们提出了一个新的深度学习模型 利用来自视觉 惯性传感器及其噪声数据来

  • An Introduction for IMU 2 - IMU数据融合与姿态解算

    在上一篇博客中 我们已经介绍了IMU的内部工作原理 以及如何通过Arduino读取MPU6050的数据 虽然可以从DMP直接读取姿态角 但其数据返回的频率相对较低 同时由于DMP库不是开源的 其内部的工作原理 输出姿态角的准确性都不清楚 而

  • 四元素与旋转矩阵

    如何描述三维空间中刚体的旋转 是个有趣的问题 具体地说 就是刚体上的任意一个点P x y z 围绕过原点的轴 i j k 旋转 求旋转后的点P x y z 旋转矩阵 旋转矩阵乘以点P的齐次坐标 得到旋转后的点P 因此旋转矩阵可以描述旋转 x

随机推荐

热门标签