基于 GTSAM 的 GINS（gtsam/example/ImuFactorsExample.cpp）-CSDN博客

本文链接：https://blog.csdn.net/m0_49384824/article/details/146459058

一、示例代码

因子图结构如下：

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file ImuFactorsExample
 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
 * navigation code.
 * @author Garrett (ghemann@gmail.com), Luca Carlone
 */
/**
 * @file ImuFactorsExample
 * @brief 使用 GTSAM 的 ImuFactor 和 ImuCombinedFactor 进行导航测试示例（结合 GPS 数据进行测试）
 * @author Garrett (ghemann@gmail.com), Luca Carlone
 */

/**
 * Example of use of the imuFactors (imuFactor and combinedImuFactor) in
 * conjunction with GPS
 *  - imuFactor is used by default. You can test combinedImuFactor by
 *  appending a `-c` flag at the end (see below for example command).
 *  - we read IMU and GPS data from a CSV file, with the following format:
 *  A row starting with "i" is the first initial position formatted with
 *  E, N, U, qx, qY, qZ, qW, velE, velN, velU
 *  A row starting with "0" is an imu measurement
 *  (body frame - Forward, Left, Up)
 *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
 *  A row starting with "1" is a gps correction formatted with
 *  E, N, U, qX, qY, qZ, qW
 * Note that for GPS correction, we're only using the position not the
 * rotation. The rotation is provided in the file for ground truth comparison.
 *
 *  See usage: ./ImuFactorsExample --help
 */
/**
 * 示例说明如何结合使用 IMU 因子（imuFactor 和 combinedImuFactor）与 GPS 测量：
 *  - 默认使用 imuFactor；你可以在命令行末尾添加 “-c” 参数测试 combinedImuFactor（参见下方示例命令）
 *  - 从 CSV 文件中读取 IMU 和 GPS 数据，文件格式如下：
 *      * 以 "i" 开头的一行表示初始位置，格式为：
 *          E, N, U, qx, qY, qZ, qW, velE, velN, velU
 *      * 以 "0" 开头的一行表示 IMU 测量（传感器坐标系下，顺序为：前、左、上）：
 *          linAccX, linAccY, linAccZ, angVelX, angVelY, angVelZ
 *      * 以 "1" 开头的一行表示 GPS 校正，格式为：
 *          E, N, U, qX, qY, qZ, qW
 *    注意：对于 GPS 校正，仅使用位置信息而非旋转；文件中提供旋转仅用于与地面真值进行对比。
 *
 *  用法示例： ./ImuFactorsExample --help
 */

#include <boost/program_options.hpp>

// GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>         // Imu因子：ImuFactor，对应预积分的类 PreintegratedImuMeasurements
#include <gtsam/navigation/CombinedImuFactor.h> // Imu因子：CombinedImuFactor，对应预积分的类 PreintegratedCombinedMeasurements
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// #include <gtsam/slam/dataset.h>

#include <cstring>
#include <fstream>
#include <iostream>

// using namespace gtsam;
// using namespace std;

using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)

namespace po = boost::program_options;

/*
  解析命令行参数
    -h [ --help ]
      显示帮助信息，列出所有可用的命令行选项。

    --data_csv_path arg (=imuAndGPSdata.csv)
      指定包含IMU数据（以及GPS数据）的CSV文件的路径。如果不指定，则默认使用 imuAndGPSdata.csv 文件。

    --output_filename arg (=imuFactorExampleResults.csv)
      指定输出结果文件的路径和文件名。默认输出为 imuFactorExampleResults.csv。

    --use_isam
      如果加上该选项，则程序使用ISAM作为优化器进行求解，否则可能使用其它默认的优化方法。
*/
po::variables_map parseOptions(int argc, char* argv[]) {
  po::options_description desc;
  desc.add_options()("help,h", "produce help message")(
      "data_csv_path", po::value<std::string>()->default_value("/home/wu/fast_slam/liosam_ws/src/gtsam_example_pkg/src/Data/imuAndGPSdata.csv"),
      "path to the CSV file with the IMU data")(
      "output_filename",
      po::value<std::string>()->default_value("/home/wu/fast_slam/liosam_ws/src/gtsam_example_pkg/src/Data/imuFactorExampleResults.csv"),
      "path to the result file to use")("use_isam", po::bool_switch(),
                                        "use ISAM as the optimizer");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    exit(1);
  }

  return vm;
}

// Imu因子：ImuFactor，对应预积分的类 PreintegratedImuMeasurements
boost::shared_ptr<gtsam::PreintegratedImuMeasurements::Params> ImuFactor_imuParams() {
  // We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924;          // 加速度计噪声标准差
  double gyro_noise_sigma = 0.000205689024915;   // 陀螺仪噪声标准差

  // 计算各个协方差矩阵（均为单位矩阵乘以标准差平方）
  gtsam::Matrix33 measured_acc_cov = gtsam::I_3x3 * pow(accel_noise_sigma, 2);
  gtsam::Matrix33 measured_omega_cov = gtsam::I_3x3 * pow(gyro_noise_sigma, 2);
  gtsam::Matrix33 integration_error_cov = gtsam::I_3x3 * 1e-8;  // error committed in integrating position from velocities // 连续时间下描述积分不确定性的协方差矩阵

  // MakeSharedU 表示z轴指向天；MakeSharedD 表示z轴指向地
  // auto p = gtsam::PreintegrationParams::MakeSharedU(0.0); // 与下一行代码等效
  auto p = gtsam::PreintegratedImuMeasurements::Params::MakeSharedD(0.0);
  // PreintegrationBase params:
  p->accelerometerCovariance = measured_acc_cov;     // acc white noise in continuous
  p->integrationCovariance = integration_error_cov;  // integration uncertainty continuous
  // should be using 2nd order integration
  // PreintegratedRotation params:
  p->gyroscopeCovariance = measured_omega_cov;       // gyro white noise in continuous

  return p;
}

// Imu因子：CombinedImuFactor，对应预积分的类 PreintegratedCombinedMeasurements
boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> CombinedImuFactor_imuParams() {
  // We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924;          // 加速度计噪声标准差
  double gyro_noise_sigma = 0.000205689024915;   // 陀螺仪噪声标准差
  double accel_bias_rw_sigma = 0.004905;         // 加速度计偏置随机游走标准差
  double gyro_bias_rw_sigma = 0.000001454441043; // 陀螺仪偏置随机游走标准差

  // 计算各个协方差矩阵（均为单位矩阵乘以标准差平方）
  gtsam::Matrix33 measured_acc_cov = gtsam::I_3x3 * pow(accel_noise_sigma, 2);
  gtsam::Matrix33 measured_omega_cov = gtsam::I_3x3 * pow(gyro_noise_sigma, 2);
  gtsam::Matrix33 integration_error_cov = gtsam::I_3x3 * 1e-8;  // error committed in integrating position from velocities // 连续时间下描述积分不确定性的协方差矩阵
  gtsam::Matrix33 bias_acc_cov = gtsam::I_3x3 * pow(accel_bias_rw_sigma, 2);
  gtsam::Matrix33 bias_omega_cov = gtsam::I_3x3 * pow(gyro_bias_rw_sigma, 2);
  gtsam::Matrix66 bias_acc_omega_init = gtsam::I_6x6 * 1e-5;    // error in the bias used for preintegration // 初始偏置估计的协方差矩阵

  // MakeSharedU 表示z轴指向天；MakeSharedD 表示z轴指向地
  // auto p = gtsam::PreintegrationCombinedParams::MakeSharedU(0.0); // 与下一行代码等效
  auto p = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
  // PreintegrationBase params:
  p->accelerometerCovariance = measured_acc_cov;     // acc white noise in continuous
  p->integrationCovariance = integration_error_cov;  // integration uncertainty continuous
  // should be using 2nd order integration
  // PreintegratedRotation params:
  p->gyroscopeCovariance = measured_omega_cov;       // gyro white noise in continuous
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov;               // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov;           // gyro bias in continuous
  p->biasAccOmegaInt = bias_acc_omega_init;

  return p;
}

int main(int argc, char* argv[]) {
  std::string data_filename, output_filename;

  bool use_isam = false;

  po::variables_map var_map = parseOptions(argc, argv);

  data_filename = var_map["data_csv_path"].as<std::string>();
  output_filename = var_map["output_filename"].as<std::string>();
  std::cout << "data_filename: " << data_filename << "\n\n";
  std::cout << "output_filename: " << output_filename << "\n\n";
  use_isam = var_map["use_isam"].as<bool>();

  // GTSAM：创建空的GTSAM优化器，添加优化器参数（使用ISAM2优化器）
  gtsam::ISAM2* isam2 = 0;
  if (use_isam) {
    printf("Using ISAM2\n");
    gtsam::ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    isam2 = new gtsam::ISAM2(parameters);

  } else {
    printf("Using Levenberg Marquardt Optimizer\n");
  }

  // Set up output file for plotting errors
  // 设置输出文件，用于记录误差等统计数据
  FILE* fp_out = fopen(output_filename.c_str(), "w+");
  fprintf(fp_out,
          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
          "gt_qy,gt_qz,gt_qw\n");

  // Begin parsing the CSV file.  Input the first line for initialization.
  // From there, we'll iterate through the file and we'll preintegrate the IMU
  // or add in the GPS given the input.
  std::ifstream file(data_filename.c_str());
  std::string value;

  // Format is (E,N,U,qX,qY,qZ,qW,velE,velN,velU)
  gtsam::Vector10 initial_state; // 初始状态: E, N, U, qx, qY, qZ, qW, velE, velN, velU 东北天
  getline(file, value, ',');  // 以 "i" 开头的一行
  for (int i = 0; i < 9; i++) {
    getline(file, value, ',');
    initial_state(i) = std::stof(value.c_str());
  }
  getline(file, value, '\n');
  initial_state(9) = std::stof(value.c_str());
  std::cout << "initial state:\n" << initial_state.transpose() << "\n\n"; // 初始状态: E, N, U, qx, qY, qZ, qW, velE, velN, velU 东北天

  // Assemble initial quaternion through GTSAM constructor
  // ::quaternion(w,x,y,z);
  gtsam::Rot3 prior_rotation = gtsam::Rot3::Quaternion(initial_state(6), initial_state(3), initial_state(4), initial_state(5));
  gtsam::Point3 prior_point(initial_state.head<3>());
  gtsam::Pose3 prior_pose(prior_rotation, prior_point);   // 位姿 X
  gtsam::Vector3 prior_velocity(initial_state.tail<3>()); // 速度 V
  gtsam::imuBias::ConstantBias prior_imu_bias;            // 偏置 B，默认全为 0。假设初始偏置为零 // assume zero initial bias 

  // GTSAM：构造初始值对象，插入初始位姿、速度、偏置
  gtsam::Values initial_values;
  int key_GPS = 0; // key，以 GPS 帧为关键帧
  initial_values.insert(X(key_GPS), prior_pose);
  initial_values.insert(V(key_GPS), prior_velocity);
  initial_values.insert(B(key_GPS), prior_imu_bias);

  // Assemble prior noise model and add it the graph.`
  // GTSAM：构造初始值先验噪声模型（位姿、速度、偏置）
  auto pose_noise_model = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished());  // rad,rad,rad,m, m, m
  auto velocity_noise_model = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
  auto bias_noise_model = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3);

  // Add all prior factors (pose, velocity, bias) to the graph.
  // GTSAM：创建空的因子图，并添加所有先验因子
  gtsam::NonlinearFactorGraph* graph = new gtsam::NonlinearFactorGraph();
  graph->addPrior(X(key_GPS), prior_pose, pose_noise_model);
  graph->addPrior(V(key_GPS), prior_velocity, velocity_noise_model);
  graph->addPrior(B(key_GPS), prior_imu_bias, bias_noise_model);

  // GTSAM：创建 IMU 预积分参数
  auto p = ImuFactor_imuParams();         // Imu因子：ImuFactor
  // auto p = CombinedImuFactor_imuParams(); // Imu因子：CombinedImuFactor

  // GTSAM：创建预积分器对象，类型为 PreintegratedImuMeasurements。传入 IMU预积分参数 和 当前偏置的估计值(全为0) 
  std::shared_ptr<gtsam::PreintegrationType> preintegrated = std::make_shared<gtsam::PreintegratedImuMeasurements>(p, prior_imu_bias);

  assert(preintegrated);

  // Store previous state for imu integration and latest predicted outcome.
  // GTSAM：设置前一关键帧的状态和偏置，用于后续 IMU 预积分预测，即调用 predict(前一关键帧状态, 前一关键帧偏置) 函数
  gtsam::NavState prev_state(prior_pose, prior_velocity);
  gtsam::imuBias::ConstantBias prev_bias = prior_imu_bias;
  // GTSAM：初始化当前关键帧状态
  gtsam::NavState prop_state = prev_state;

  // Keep track of total error over the entire run as simple performance metric.
  // 记录每次优化完成后的误差（仅作为性能指标）
  double current_position_error = 0.0, current_orientation_error = 0.0;

  double output_time = 0.0;
  double dt = 0.005;  // The real system has noise, but here, results are nearly
                      // exactly the same, so keeping this for simplicity.




  // All priors have been set up, now iterate through the data file.
  // 依次读取 CSV 文件中的数据
  while (file.good()) {
    // Parse out first value
    getline(file, value, ',');
    int type = std::stoi(value.c_str());

    if (type == 0) {  // IMU 测量（传感器坐标系下，顺序为：前、左、上）：linAccX, linAccY, linAccZ, angVelX, angVelY, angVelZ
      // GTSAM：IMU 数据到达时处理
      gtsam::Vector6 imu;
      for (int i = 0; i < 5; ++i) {
        getline(file, value, ',');
        imu(i) = std::stof(value.c_str());
      }
      getline(file, value, '\n');
      imu(5) = std::stof(value.c_str());

      // Adding the IMU preintegration.
      // GTSAM：IMU 数据到达时处理，将读取的 IMU 测量添加到预积分器对象中：
      preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
      // cout << "imu" << imu.transpose() << "\n";

    } else if (type == 1) {  // GPS 校正，格式为：E, N, U, qX, qY, qZ, qW
      // GTSAM：GPS 数据到达时处理，构建因子图，进行优化
      gtsam::Vector7 gps;
      for (int i = 0; i < 6; ++i) {
        getline(file, value, ',');
        gps(i) = std::stof(value.c_str());
      }
      getline(file, value, '\n');
      gps(6) = std::stof(value.c_str());

      key_GPS++;

      // Adding IMU factor and GPS factor and optimizing.

      // 1. GTSAM：添加 IMU 因子，将前一状态与当前状态连接：
      // 前一状态到当前状态的所有预积分测量值。（通过 dynamic_cast 将 gtsam::PreintegrationType 强制转换为 gtsam::PreintegratedImuMeasurements）
      auto preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*preintegrated);
      gtsam::ImuFactor imu_factor(X(key_GPS - 1), V(key_GPS - 1),
                           X(key_GPS), V(key_GPS),
                           B(key_GPS - 1), preint_imu);
      graph->add(imu_factor);

      // 2. GTSAM：添加偏置随机游走因子，假设偏置没有变化
      gtsam::imuBias::ConstantBias zero_bias(gtsam::Vector3(0, 0, 0), gtsam::Vector3(0, 0, 0));
      // graph->add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(
      //     B(key_GPS - 1), B(key_GPS), zero_bias,
      //     bias_noise_model));
      gtsam::BetweenFactor<gtsam::imuBias::ConstantBias> bias_Factor(B(key_GPS - 1), B(key_GPS), 
                                                                     zero_bias, 
                                                                     bias_noise_model);
      graph->add(bias_Factor);

      // 3. GTSAM：添加 GPS 校正因子：GPSFactor 仅约束位置，不约束旋转
      auto correction_noise = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
      gtsam::GPSFactor gps_factor(X(key_GPS),
                           gtsam::Point3(gps(0),   // E,
                                         gps(1),   // N,
                                         gps(2)),  // U,
                           correction_noise);
      graph->add(gps_factor);

      // Now optimize and compare results.
      // 4. GTSAM：利用Imu预积分器预测当前状态：
      prop_state = preintegrated->predict(prev_state, prev_bias);

      // 5. GTSAM：将预积分预测得到的当前状态，作为因子图中当前状态的初始值
      // 前一时刻状态的初始值使用存储在 isam2 中的上一次优化的结果，不用重新添加
      initial_values.insert(X(key_GPS), prop_state.pose());
      initial_values.insert(V(key_GPS), prop_state.v());
      initial_values.insert(B(key_GPS), prev_bias);

      gtsam::Values result;

      // 根据是否使用 ISAM2 选择不同优化器
      if (use_isam) {
        // 6. GTSAM：ISAM2 增量优化
        // 在一些早期版本的 ISAM2 实现中，开发者发现分两步更新能够更好地控制系统的稳定性和精度。因此，有时用户会习惯性地先将新数据输入，再调用一次空的 update() 来“刷新”整个系统。
        isam2->update(*graph, initial_values); /* 将当前批次的新添加的因子（存储在 graph 中）和新变量的初始猜测（initial_values）添加到 ISAM2 的内部因子图中,执行增量优化
                                                * 新变量的初始猜测（initial_values）：必须包含 newFactors 中所有新变量，不得包含已有变量。*/
        isam2->update();                       // 再次执行增量优化
        result = isam2->calculateEstimate();   // 获取优化结果

        // reset the graph
        // 7. GTSAM：清空因子图和初始估计，为下一次优化做准备
        graph->resize(0);
        initial_values.clear();

      } else {
        gtsam::LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
        result = optimizer.optimize();
      }

      // Overwrite the beginning of the preintegration for the next step.
      // 8. GTSAM：更新上一时刻状态和偏置，将优化后的最新状态作为下一段预积分的初始值
      prev_state = gtsam::NavState(result.at<gtsam::Pose3>(X(key_GPS)), result.at<gtsam::Vector3>(V(key_GPS)));
      prev_bias = result.at<gtsam::imuBias::ConstantBias>(B(key_GPS));

      // Reset the preintegration object.
      // 9. GTSAM：重置预积分器，并设置新的偏置为优化后的最新偏执
      preintegrated->resetIntegrationAndSetBias(prev_bias);



      // Print out the position and orientation error for comparison.
      // 计算当前位置误差：比较预测位姿与 GPS 测量得到的位置信息
      gtsam::Vector3 gtsam_position = prev_state.pose().translation();
      gtsam::Vector3 position_error = gtsam_position - gps.head<3>();
      current_position_error = position_error.norm();

      // 计算当前姿态误差：利用四元数表示并求误差的欧拉角量级
      gtsam::Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
      gtsam::Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
      gtsam::Quaternion quat_error = gtsam_quat * gps_quat.inverse();
      quat_error.normalize();
      gtsam::Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
                                quat_error.z() * 2);
      current_orientation_error = euler_angle_error.norm();

      // display statistics
      // 显示当前位置和姿态误差
      std::cout << "--------------------------------------------------------------------------"<< std::endl;
      // cout << "gtsam_position: " << gtsam_position.transpose() << "    gtsam_quat: " << gtsam_quat.x() << " " << gtsam_quat.y() << " " << gtsam_quat.z()<< " " << gtsam_quat.w() << "\n";
      std::cout << "Position error:" << current_position_error << "\t "
           << "Angular error:" << current_orientation_error << "\n";

      // 将当前时间、状态和 GPS 真值写入输出文件
      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
              output_time, gtsam_position(0), gtsam_position(1),
              gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
              gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
              gps_quat.y(), gps_quat.z(), gps_quat.w());

      output_time += 1.0;

    } else if (type == 555){ // 555为结束标志
      std::cerr << "ERROR parsing file\n";
      break;
    }
  }
  fclose(fp_out);
  std::cout << "Complete, results written to " << output_filename << "\n\n";

  return 0;
}

二、重要代码源码

1. 预积分器对象 (`PreintegratedImuMeasurements`)

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

源码：

/**
 *  构造函数：初始化该类（不包含任何测量数据）
 *  @param p       参数，一般在同一应用中保持固定
 *  @param biasHat 当前加速度和角速度偏置的估计值
 */
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
      PreintegrationType(p, biasHat) {
    preintMeasCov_.setZero();
}

2. 将 IMU 测量值 (`integrateMeasurement`)添加到预积分器对象中

调用该函数，GTSAM 内部会完成链接中的6 预积分流程中的第 1 步到第 4 步

preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);

源码：

/**
 * 将一条单独的 IMU 测量添加到预积分中。
 * 加速度计和陀螺仪的测量均被视为传感器坐标系下的数据，
 * 转换到机体坐标系的操作由 PreintegrationParams 中的 `body_P_sensor` 处理。
 *
 * @param measuredAcc 传感器提供的测量加速度
 * @param measuredOmega 传感器提供的测量角速度
 * @param dt 此次测量与上一次 IMU 测量之间的时间间隔
 */
void integrateMeasurement(const Vector3& measuredAcc,
    const Vector3& measuredOmega, const double dt) override;

3. IMU 因子

      auto preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*preintegrated);
      gtsam::ImuFactor imu_factor(X(key_GPS - 1), V(key_GPS - 1),
                           X(key_GPS), V(key_GPS),
                           B(key_GPS - 1), preint_imu);

源码：

/**
 * ImuFactor 构造函数
 * @param pose_i  前一时刻位姿的变量 Key
 * @param vel_i   前一时刻速度的变量 Key
 * @param pose_j  当前时刻位姿的变量 Key
 * @param vel_j   当前时刻速度的变量 Key
 * @param bias    前一时刻 IMU 偏置的变量 Key
 * @param preintegratedMeasurements 前一状态到当前状态的所有预积分测量值
 */
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PreintegratedImuMeasurements& preintegratedMeasurements);

4. 利用 IMU 预积分器预测当前状态

调用该函数，GTSAM 内部会完成链接中的6 预积分流程中的第 5 步

prop_state = preintegrated->predict(prev_state, prev_bias);

源码：

/// IMU 预积分，根据 i 时刻状态预测时刻 j 的状态
/// @param state_i     i 时刻的状态（包含位姿和速度）
/// @param bias_i      i 时刻的 IMU 偏置
/// @param H1         （可选）关于输入 state_i 的雅可比矩阵，尺寸为 9×9
/// @param H2         （可选）关于输入 bias_i 的雅可比矩阵，尺寸为 9×6
/// @return            预测得到的 j 时刻的状态
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                 OptionalJacobian<9, 9> H1 = boost::none,
                 OptionalJacobian<9, 6> H2 = boost::none) const;

5. ISAM2 类的成员函数

5.1 update()

        isam2->update(*graph, initial_values); 
        isam2->update();

源码：两次均调用了下面的成员函数。注意：下面的成员函数的每一个形参均有默认值。

/**
 * 添加新因子，更新解并根据需要进行重线性化。
 *
 * 可选：此函数可以从系统中移除现有因子，从而实现例如用新因子替换已有因子的行为。
 *
 * 向当前系统中添加新的测量数据，并可选地添加新变量。
 * 此操作执行 ISAM2 算法的完整步骤，根据 wildfire 阈值和重线性化阈值，
 * 对系统进行重线性化并更新解。
 *
 * @param newFactors 要添加到系统中的新因子
 * @param newTheta   新变量的初始猜测。必须包含 newFactors 中所有未在系统中出现的新变量，
 *                   且不能包含不在 newFactors 中的变量；此外，已存在于系统中的变量也不应包含在内。
 * @param removeFactorIndices 要从系统中移除的因子索引
 * @param force_relinearize   如果设为 true，则对所有线性增量足够大的变量（超过 Params::relinearizeThreshold）
 *                            强制进行重线性化，而不考虑重线性化间隔 Params::relinearizeSkip。
 * @param constrainedKeys     可选：一个键到组标签的映射，使得某个变量可以被限制在 BayesTree 的特定分组中。
 * @param noRelinKeys         可选：一组非线性变量的键，ISAM2 将保持这些变量的线性化点不变，
 *                            无论其线性增量大小如何。
 * @param extraReelimKeys     可选：一组非线性变量的键，ISAM2 将无论线性增量大小如何都重新消元，
 *                            允许这些键被重新排序。
 * @return 返回一个 ISAM2Result 结构体，其中包含此次更新的详细信息。
 */
virtual ISAM2Result update(
    const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
    const Values& newTheta = Values(),
    const FactorIndices& removeFactorIndices = FactorIndices(),
    const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
    const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
    const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
    bool force_relinearize = false);

5.2 calculateEstimate()

    result = isam2->calculateEstimate();

源码：

/** 
 * 利用上一次 update() 过程中计算得到的不完整线性增量，计算出一个整体的估计值。
 * 注意：由于该增量未更新到低于 wildfire_threshold，因此是不完整的。
 * 如果只需要计算单个变量的估计，调用 calculateEstimate(const KEY&) 会更快。
 */
Values calculateEstimate() const;

/** 
 * 针对单个变量，利用上一次 update() 过程中计算得到的不完整线性增量计算该变量的估计值。
 * 这种方法比调用无参版本的 calculateEstimate() 更快（后者对所有变量进行计算）。
 * @param key 要计算估计值的变量的键
 * @return 该变量的估计值
 */
template <class VALUE>
VALUE calculateEstimate(Key key) const {
  // 取得上一次 update() 中存储的线性增量（delta），然后利用 Retract 操作，
  // 将当前线性化点 theta_ 中对应变量的估计值向量按照增量 delta 进行修正，
  // 得到该变量的当前估计值。
  const Vector& delta = getDelta()[key];
  return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}

5.3 边缘化

源码：

在路径 gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h 中：

/** 
 * 对 leafKeys 中列出的变量进行边缘化。这些键必须是 BayesTree 中的叶节点。
 * 如果请求边缘化非叶节点，则抛出 MarginalizeNonleafException。
 *
 * 边缘化操作会在系统中留下该边缘的线性近似，
 * 同时所有参与该线性边缘的变量的线性化点都会被固定。
 * 固定变量集合将包含所有与原因子中边缘化变量有关的键，
 * 以及由于填充效应可能额外产生的其他键。
 *
 * 如果提供了 'marginalFactorsIndices' 参数，
 * 在调用 'marginalizeLeaves' 过程中，新添加的边缘因子对应的因子图索引会被追加到其中。
 *
 * 如果提供了 'deletedFactorsIndices' 参数，
 * 在调用 'marginalizeLeaves' 过程中，被移除的因子对应的因子图索引会被追加到其中。
 */
void marginalizeLeaves(
    const FastList<Key>& leafKeys,
    boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
    boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);

/** 返回指定变量的边缘协方差矩阵 */
Matrix marginalCovariance(Key key) const;

在路径 gtsam/nonlinear/Marginals.h 中：

/** 计算单个变量的边缘信息矩阵。  
 *  使用 LLt(const Matrix&) 或 RtR(const Matrix&) 获取平方根信息矩阵。  
 *  LLt 即为 LL^T, L为下三角矩阵。RtR 即为 R^TR, R为上三角矩阵。
 */
Matrix marginalInformation(Key variable) const;

/** 计算单个变量的边缘协方差矩阵。 */
Matrix marginalCovariance(Key variable) const;

经过验证，Marginals.h 中的 marginalCovariance(key).inverse() 和marginalInformation(key) 相等。

  Marginals marginals(graph, result);
  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

  // marginals.marginalCovariance(key).inverse() 和 marginals.marginalInformation(key) 相等
  cout << "x1 covariance^-1:\n" << marginals.marginalCovariance(1).inverse() << endl;
  cout << "x2 covariance^-1:\n" << marginals.marginalCovariance(2).inverse() << endl;
  cout << "x3 covariance^-1:\n" << marginals.marginalCovariance(3).inverse() << endl;

  cout << "x1 information:\n" << marginals.marginalInformation(1) << endl;
  cout << "x2 information:\n" << marginals.marginalInformation(2) << endl;
  cout << "x3 information:\n" << marginals.marginalInformation(3) << endl;

x1 covariance:
  0.00828571429  7.82228372e-14 -2.06607043e-13
 7.82228372e-14   0.00944444444  -0.00305555556
-2.06607043e-13  -0.00305555556   0.00819444445
x2 covariance:
  0.00714285714  3.80266543e-14 -5.38289642e-14
 3.80266543e-14   0.00777777778  -0.00111111111
-5.38289642e-14  -0.00111111111   0.00819444444
x3 covariance:
 0.00828571429 6.60868131e-14 1.73935528e-13
6.60868131e-14  0.00944444444  0.00305555556
1.73935528e-13  0.00305555556   0.0181944444

x1 covariance^-1:
     120.689655 -1.71902606e-11  3.03654593e-09
-1.71902606e-11      120.408163      44.8979592
 3.03654593e-09      44.8979592       138.77551
x2 covariance^-1:
            140 -5.64026054e-10  8.43176059e-10
-5.64026054e-10      131.111111      17.7777778
 8.43176059e-10      17.7777778      124.444444
x3 covariance^-1:
     120.689655 -4.98313237e-10 -1.07008462e-09
-4.98313237e-10      111.965812     -18.8034188
-1.07008462e-09     -18.8034188      58.1196581

x1 information:
     120.689655 -1.71902606e-11  3.03654593e-09
-1.71902606e-11      120.408163      44.8979592
 3.03654593e-09      44.8979592       138.77551
x2 information:
            140 -5.64026054e-10  8.43176059e-10
-5.64026054e-10      131.111111      17.7777778
 8.43176059e-10      17.7777778      124.444444
x3 information:
     120.689655 -4.98313237e-10 -1.07008462e-09
-4.98313237e-10      111.965812     -18.8034188
-1.07008462e-09     -18.8034188      58.1196581

这里的信息矩阵，应该对应于下式中的 $H_{11,new}$ ，协方差矩阵对应其逆 $H_{11,new}^{-1}$ 。