010 gtsam/examples/ImuFactorsExample2.cpp

使用的因子是ImuFactor
采用PreintegrationParams形式

const double kGravity = 9.81;
1

一、main函数

1.1 初始值设置

  auto params = PreintegrationParams::MakeSharedU(kGravity);
  params->setAccelerometerCovariance(I_3x3 * 0.1);
  params->setGyroscopeCovariance(I_3x3 * 0.1);
  params->setIntegrationCovariance(I_3x3 * 0.1);
  params->setUse2ndOrderCoriolis(false);
  params->setOmegaCoriolis(Vector3(0, 0, 0));
  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));//Pose的一个小的扰动
1
2
3
4
5
6
7

1.2 相机

  // Start with a camera on x-axis looking at origin
  double radius = 30;
  const Point3 up(0, 0, 1), target(0, 0, 0);
  const Point3 position(radius, 0, 0);
  const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
  const auto pose_0 = camera.pose();
1
2
3
4
5
6

1.2.1 Lookat 函数用法

在给定的position(eye)位置创建一个相机，在up(upVector)方向上观察target点

  /**
   * Create a camera at the given eye position looking at a target point in the scene
   * with the specified up direction vector.
   * @param eye specifies the camera position
   * @param target the point to look at
   * @param upVector specifies the camera up direction vector,
   *        doesn't need to be on the image plane nor orthogonal to the viewing axis
   * @param K optional calibration parameter
   */
  static PinholeCamera Lookat(const Point3& eye, const Point3& target,
      const Point3& upVector, const Calibration& K = Calibration()) {
    return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13

1.3 创建scenario

constant-twist使得相机创建旋转场景

  // Now, create a constant-twist scenario that makes the camera orbit the origin
  double angular_velocity = M_PI,  // rad/sec
      delta_t = 1.0 / 18;          // makes for 10 degrees per step
  Vector3 angular_velocity_vector(0, -angular_velocity, 0);
  Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
  auto scenario = ConstantTwistScenario(angular_velocity_vector,
                                        linear_velocity_vector, pose_0);
1
2
3
4
5
6
7

1.3.1 ConstantTwistScenario类

创建3D轨迹场景

/**
 * Scenario with constant twist 3D trajectory.
 * Note that a plane flying level does not feel any acceleration: gravity is
 * being perfectly compensated by the lift generated by the wings, and drag is
 * compensated by thrust. The accelerometer will add the gravity component back
 * in, as modeled by the specificForce method in ScenarioRunner.
 */
class ConstantTwistScenario : public Scenario {
 public:
  /// Construct scenario with constant twist [w,v]
  ConstantTwistScenario(const Vector3& w, const Vector3& v,
                        const Pose3& nTb0 = Pose3())
      : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}

  Pose3 pose(double t) const override {
    return nTb0_ * Pose3::Expmap(twist_ * t);
  }
  Vector3 omega_b(double t) const override { return twist_.head<3>(); }
  Vector3 velocity_n(double t) const override {
    return rotation(t).matrix() * twist_.tail<3>();
  }
  Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }

 private:
  const Vector6 twist_;
  const Vector3 a_b_;  // constant centripetal acceleration in body = w_b * v_b
  const Pose3 nTb0_;
}; 
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

1.4 创建因子图、优化器、初始化Values、添加先验

  // Create a factor graph
  NonlinearFactorGraph newgraph;

  // Create (incremental) ISAM2 solver
  ISAM2 isam;

  // Create the initial estimate to the solution
  // Intentionally initialize the variables off from the ground truth
  Values initialEstimate, totalEstimate, result;

  // Add a prior on pose x0. This indirectly specifies where the origin is.
  // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
  auto noise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
  newgraph.addPrior(X(0), pose_0, noise);

  // Add imu priors
  Key biasKey = Symbol('b', 0);
  auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
  newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
  initialEstimate.insert(biasKey, imuBias::ConstantBias());
  auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  Vector n_velocity(3);
  n_velocity << 0, angular_velocity * radius, 0;
  newgraph.addPrior(V(0), n_velocity, velnoise);

  initialEstimate.insert(V(0), n_velocity);

  // IMU preintegrator
  PreintegratedImuMeasurements accum(params);//智能指针的引用传值构造
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

1.5 向图中添加因子执行优化

  // Simulate poses and imu measurements, adding them to the factor graph
  for (size_t i = 0; i < 36; ++i) {
    double t = i * delta_t;
    if (i == 0) {  // First time add two poses
      auto pose_1 = scenario.pose(delta_t);
      initialEstimate.insert(X(0), pose_0.compose(delta));
      initialEstimate.insert(X(1), pose_1.compose(delta));
    } else if (i >= 2) {  // Add more poses as necessary
      auto pose_i = scenario.pose(t);
      initialEstimate.insert(X(i), pose_i.compose(delta));
    }//仿真加入位姿,第一次是加入两个位姿
    
	
    if (i > 0) {
      // Add Bias variables periodically
      // 周期性地加入bias
      if (i % 5 == 0) {
        biasKey++;
        Symbol b1 = biasKey - 1;
        Symbol b2 = biasKey;
        Vector6 covvec;
        covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
        auto cov = noiseModel::Diagonal::Variances(covvec);
        auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
            b1, b2, imuBias::ConstantBias(), cov);
        newgraph.add(f);
        initialEstimate.insert(biasKey, imuBias::ConstantBias());
      }
      // Predict acceleration and gyro measurements in (actual) body frame
      // body系下得到acc omega进行预积分
      Vector3 measuredAcc = scenario.acceleration_b(t) -
                            scenario.rotation(t).transpose() * params->n_gravity;
      Vector3 measuredOmega = scenario.omega_b(t);
      accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);

      // Add Imu Factor
      ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
      newgraph.add(imufac);

      // insert new velocity, which is wrong
      // 加入新速度，重置预积分测量
      initialEstimate.insert(V(i), n_velocity);
      accum.resetIntegration();
    }

    // Incremental solution
    //求解
    isam.update(newgraph, initialEstimate);
    result = isam.calculateEstimate();
    newgraph = NonlinearFactorGraph();
    initialEstimate.clear();
  }
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