011 gtsam/examples/IMUKittiExampleGPS.cpp

说明：
Example of application of ISAM2 for GPS-aided navigation on the KITTI

一、预先定义

1.1 symbol

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

1.2 kitti参数

外参数、噪声

struct KittiCalibration {
  double body_ptx;
  double body_pty;
  double body_ptz;
  double body_prx;
  double body_pry;
  double body_prz;
  double accelerometer_sigma;
  double gyroscope_sigma;
  double integration_sigma;
  double accelerometer_bias_sigma;
  double gyroscope_bias_sigma;
  double average_delta_t;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14

1.3 imu和gps测量

struct ImuMeasurement {
  double time;
  double dt;
  Vector3 accelerometer;
  Vector3 gyroscope;  // omega
};

struct GpsMeasurement {
  double time;
  Vector3 position;  // x,y,z
};
1
2
3
4
5
6
7
8
9
10
11

1.4 加载数据集

void loadKittiData(KittiCalibration& kitti_calibration,
                   vector<ImuMeasurement>& imu_measurements,
                   vector<GpsMeasurement>& gps_measurements) 
                   {
                   }
1
2
3
4
5

二、main函数

2.1 读入数据

  KittiCalibration kitti_calibration;
  vector<ImuMeasurement> imu_measurements;
  vector<GpsMeasurement> gps_measurements;
  loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
1
2
3
4

body系 imu系的位姿变换

  Vector6 BodyP =
      (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
       kitti_calibration.body_ptz, kitti_calibration.body_prx,
       kitti_calibration.body_pry, kitti_calibration.body_prz)
          .finished();
  auto body_T_imu = Pose3::Expmap(BodyP);
  if (!body_T_imu.equals(Pose3(), 1e-5)) {
    printf(
        "Currently only support IMUinBody is identity, i.e. IMU and body frame "
        "are the same");
    exit(-1);
  }
1
2
3
4
5
6
7
8
9
10
11
12

2.2 初始变量设置

  // Configure different variables
  // double t_offset = gps_measurements[0].time;
  size_t first_gps_pose = 1;
  size_t gps_skip = 10;  // Skip this many GPS measurements each time
  double g = 9.8;
  auto w_coriolis = Vector3::Zero();  // zero vector
1
2
3
4
5
6

$coriolisforce$

2.3 噪声和imu预积分参数设置

  // Configure noise models
  auto noise_model_gps = noiseModel::Diagonal::Precisions(
      (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
          .finished());

  // Set initial conditions for the estimated trajectory
  // initial pose is the reference frame (navigation frame)
  auto current_pose_global =
      Pose3(Rot3(), gps_measurements[first_gps_pose].position);
  // the vehicle is stationary at the beginning at position 0,0,0
  Vector3 current_velocity_global = Vector3::Zero();
  auto current_bias = imuBias::ConstantBias();  // init with zero bias

  auto sigma_init_x = noiseModel::Diagonal::Precisions(
      (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
  auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
  auto sigma_init_b = noiseModel::Diagonal::Sigmas(
      (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
          .finished());
  // Set IMU preintegration parameters
  Matrix33 measured_acc_cov =
      I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
  Matrix33 measured_omega_cov =
      I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
  // error committed in integrating position from velocities
  Matrix33 integration_error_cov =
      I_3x3 * pow(kitti_calibration.integration_sigma, 2);

  auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
  imu_params->accelerometerCovariance =
      measured_acc_cov;  // acc white noise in continuous
  imu_params->integrationCovariance =
      integration_error_cov;  // integration uncertainty continuous
  imu_params->gyroscopeCovariance =
      measured_omega_cov;  // gyro white noise in continuous
  imu_params->omegaCoriolis = w_coriolis;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

2.4 iSAM求解器、因子图、Values定义设置

  // Set ISAM2 parameters and create ISAM2 solver object
  ISAM2Params isam_params;
  isam_params.factorization = ISAM2Params::CHOLESKY;
  isam_params.relinearizeSkip = 10;

  ISAM2 isam(isam_params);

  // Create the factor graph and values object that will store new factors and
  // values to add to the incremental graph
  NonlinearFactorGraph new_factors;
  Values new_values;  // values storing the initial estimates of new nodes in
                      // the factor graph
1
2
3
4
5
6
7
8
9
10
11
12

2.5 main loop

2.5.1 添加因子

  size_t j = 0;
  size_t included_imu_measurement_count = 0;

  for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
    // At each non=IMU measurement we initialize a new node in the graph
    auto current_pose_key = X(i);
    auto current_vel_key = V(i);
    auto current_bias_key = B(i);
    double t = gps_measurements[i].time;

    if (i == first_gps_pose) {
      // Create initial estimate and prior on initial pose, velocity, and biases
      // 添加关于位姿、速度、bias的初始估计和prior factor
      new_values.insert(current_pose_key, current_pose_global);
      new_values.insert(current_vel_key, current_velocity_global);
      new_values.insert(current_bias_key, current_bias);
      new_factors.emplace_shared<PriorFactor<Pose3>>(
          current_pose_key, current_pose_global, sigma_init_x);
      new_factors.emplace_shared<PriorFactor<Vector3>>(
          current_vel_key, current_velocity_global, sigma_init_v);
      new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
          current_bias_key, current_bias, sigma_init_b);
    } else {
      double t_previous = gps_measurements[i - 1].time;

      // Summarize IMU data between the previous GPS measurement and now
      // 两个GPS帧之间进行IMU预积分
      current_summarized_measurement =
          std::make_shared<PreintegratedImuMeasurements>(imu_params,
                                                         current_bias);

      while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
        if (imu_measurements[j].time >= t_previous) {
          current_summarized_measurement->integrateMeasurement(
              imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
              imu_measurements[j].dt);
          included_imu_measurement_count++;
        }
        j++;
      }

      // Create IMU factor
      // 创建IMU Factor
      auto previous_pose_key = X(i - 1);
      auto previous_vel_key = V(i - 1);
      auto previous_bias_key = B(i - 1);

      new_factors.emplace_shared<ImuFactor>(
          previous_pose_key, previous_vel_key, current_pose_key,
          current_vel_key, previous_bias_key, *current_summarized_measurement);

      // Bias evolution as given in the IMU metadata
      // 从元数据中获取bias的噪声模型
      auto sigma_between_b = noiseModel::Diagonal::Sigmas(
          (Vector6() << Vector3::Constant(
               sqrt(included_imu_measurement_count) *
               kitti_calibration.accelerometer_bias_sigma),
           Vector3::Constant(sqrt(included_imu_measurement_count) *
                             kitti_calibration.gyroscope_bias_sigma))
              .finished());
      // 利用bias添加BetweenFactor
      new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(
          previous_bias_key, current_bias_key, imuBias::ConstantBias(),
          sigma_between_b);

      // Create GPS factor
      // 构建并向因子图中添加GPS factor
      auto gps_pose =
          Pose3(current_pose_global.rotation(), gps_measurements[i].position);
      if ((i % gps_skip) == 0) {//到达添加GPS因子的周期
        new_factors.emplace_shared<PriorFactor<Pose3>>(
            current_pose_key, gps_pose, noise_model_gps);
        new_values.insert(current_pose_key, gps_pose);

        printf("############ POSE INCLUDED AT TIME %.6lf ############\n",
               t);
        cout << gps_pose.translation();
        printf("\n\n");
      } else {
        new_values.insert(current_pose_key, current_pose_global);//否则只进行变量的添加
      }

      // Add initial values for velocity and bias based on the previous estimates
      // 添加速度和bias变量
      new_values.insert(current_vel_key, current_velocity_global);
      new_values.insert(current_bias_key, current_bias);

      // Update solver
      // =======================================================================
      // We accumulate 2*GPSskip GPS measurements before updating the solver a first so that the heading becomes observable.
      // 在第一次解算之前累积一定时间以便于可观测
      if (i > (first_gps_pose + 2 * gps_skip)) {
        printf("############ NEW FACTORS AT TIME %.6lf ############\n",
               t);
        new_factors.print();

        isam.update(new_factors, new_values);//更新解算

        // Reset the newFactors and newValues list
        // 重置graphs和Values
        new_factors.resize(0);
        new_values.clear();

        // Extract the result/current estimates
        Values result = isam.calculateEstimate();//从isam2求解器中读取值

        current_pose_global = result.at<Pose3>(current_pose_key);
        current_velocity_global = result.at<Vector3>(current_vel_key);
        current_bias = result.at<imuBias::ConstantBias>(current_bias_key);

        printf("\n############ POSE AT TIME %lf ############\n", t);
        current_pose_global.print();
        printf("\n\n");
      }
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116

2.5.2 保存文件

  // Save results to file
  printf("\nWriting results to file...\n");
  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)\n");

  Values result = isam.calculateEstimate();
  for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
    auto pose_key = X(i);
    auto vel_key = V(i);
    auto bias_key = B(i);

    auto pose = result.at<Pose3>(pose_key);
    auto velocity = result.at<Vector3>(vel_key);
    auto bias = result.at<imuBias::ConstantBias>(bias_key);

    auto pose_quat = pose.rotation().toQuaternion();
    auto gps = gps_measurements[i].position;

    cout << "State at #" << i << endl;
    cout << "Pose:" << endl << pose << endl;
    cout << "Velocity:" << endl << velocity << endl;
    cout << "Bias:" << endl << bias << endl;

    fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
            gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
            pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
            gps(1), gps(2));
  }

  fclose(fp_out);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

三、mention

        isam.update(new_factors, new_values);//更新解算

        // Reset the newFactors and newValues list
        // 重置graphs和Values
        new_factors.resize(0);
        new_values.clear();

        // Extract the result/current estimates
        Values result = isam.calculateEstimate();//从isam2求解器中读取值
1
2
3
4
5
6
7
8
9

3.1 update

graphs和Values都是const &传递
所以iSAM求解器和

        new_factors.resize(0);
        new_values.clear();
1
2

上述代码不会影响iSAM求解器中的值
isam优化器的重置见lio-sam有例子

    /**
     * 重置ISAM2优化器
    */
    void resetOptimization()
    {
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;
        optParameters.relinearizeSkip = 1;
        optimizer = gtsam::ISAM2(optParameters);

        gtsam::NonlinearFactorGraph newGraphFactors;
        graphFactors = newGraphFactors;

        gtsam::Values NewGraphValues;
        graphValues = NewGraphValues;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

相当于重新定义

3.1.1 gtsam中iSAM求解器的update成员函数

  /**
   * Add new factors, updating the solution and relinearizing as needed.
   *
   * Optionally, this function remove existing factors from the system to enable
   * behaviors such as swapping existing factors with new ones.
   *
   * Add new measurements, and optionally new variables, to the current system.
   * This runs a full step of the ISAM2 algorithm, relinearizing and updating
   * the solution as needed, according to the wildfire and relinearize
   * thresholds.
   *
   * @param newFactors The new factors to be added to the system
   * @param newTheta Initialization points for new variables to be added to the
   * system. You must include here all new variables occuring in newFactors
   * (which were not already in the system).  There must not be any variables
   * here that do not occur in newFactors, and additionally, variables that were
   * already in the system must not be included here.
   * @param removeFactorIndices Indices of factors to remove from system
   * @param force_relinearize Relinearize any variables whose delta magnitude is
   * sufficiently large (Params::relinearizeThreshold), regardless of the
   * relinearization interval (Params::relinearizeSkip).
   * @param constrainedKeys is an optional map of keys to group labels, such
   * that a variable can be constrained to a particular grouping in the
   * BayesTree
   * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will
   * hold at a constant linearization point, regardless of the size of the
   * linear delta
   * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will
   * re-eliminate, regardless of the size of the linear delta. This allows the
   * provided keys to be reordered.
   * @return An ISAM2Result struct containing information about the update
   */
  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);

  /**
   * Add new factors, updating the solution and relinearizing as needed.
   *
   * Alternative signature of update() (see its documentation above), with all
   * additional parameters in one structure. This form makes easier to keep
   * future API/ABI compatibility if parameters change.
   *
   * @param newFactors The new factors to be added to the system
   * @param newTheta Initialization points for new variables to be added to the
   * system. You must include here all new variables occuring in newFactors
   * (which were not already in the system).  There must not be any variables
   * here that do not occur in newFactors, and additionally, variables that were
   * already in the system must not be included here.
   * @param updateParams Additional parameters to control relinearization,
   * constrained keys, etc.
   * @return An ISAM2Result struct containing information about the update
   * @note No default parameters to avoid ambiguous call errors.
   */
  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
                             const Values& newTheta,
                             const ISAM2UpdateParams& updateParams);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62