002_2 gtsam/unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp

利用stereo相机模式和imu的集成对iSAM2算法进行的测试

一、imu结构体

struct IMUHelper {
  IMUHelper() {
    {
      auto gaussian = noiseModel::Diagonal::Sigmas(
          (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
              .finished());
      auto huber = noiseModel::Robust::Create(
          noiseModel::mEstimator::Huber::Create(1.345), gaussian);

      biasNoiseModel = huber;
    }

    {
      auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
      auto huber = noiseModel::Robust::Create(
          noiseModel::mEstimator::Huber::Create(1.345), gaussian);

      velocityNoiseModel = huber;
    }

    // expect IMU to be rotated in image space co-ords
    auto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>(
        Vector3(0.0, 9.8, 0.0));

    p->accelerometerCovariance =
        I_3x3 * pow(0.0565, 2.0);  // acc white noise in continuous
    p->integrationCovariance =
        I_3x3 * 1e-9;  // integration uncertainty continuous
    p->gyroscopeCovariance =
        I_3x3 * pow(4.0e-5, 2.0);  // gyro white noise in continuous
    p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0);  // acc bias in continuous
    p->biasOmegaCovariance =
        I_3x3 * pow(0.001, 2.0);  // gyro bias in continuous
    p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;

    // body to IMU rotation
    Rot3 iRb(0.036129, -0.998727, 0.035207,
             0.045417, -0.033553, -0.998404,
             0.998315, 0.037670, 0.044147);

    // body to IMU translation (meters)
    Point3 iTb(0.03, -0.025, -0.06);

    // body in this example is the left camera
    p->body_P_sensor = Pose3(iRb, iTb);

    Rot3 prior_rotation = Rot3(I_3x3);
    Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));

    Vector3 acc_bias(0.0, -0.0942015, 0.0);  // in camera frame
    Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);

    priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);

    prevState = NavState(prior_pose, Vector3(0, 0, 0));
    propState = prevState;
    prevBias = priorImuBias;

    preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
  }

  imuBias::ConstantBias priorImuBias;  // assume zero initial bias
  noiseModel::Robust::shared_ptr velocityNoiseModel;
  noiseModel::Robust::shared_ptr biasNoiseModel;
  NavState prevState;
  NavState propState;
  imuBias::ConstantBias prevBias;
  PreintegratedCombinedMeasurements* preintegrated;
};

1.1 IMUHelper()构造函数

gaussian构造一个高斯噪声模型
huber核函数：系数$$\delta$$和加上高斯噪声模型

velocityNoiseModel也是类似的操作

1.2 预积分模型的参数设置

p：
高斯白噪声
bias的噪声
预测的从速度进行位置积分的误差 和 预积分的偏置协方差

1.3 坐标系

    // body in this example is the left camera
    p->body_P_sensor = Pose3(iRb, iTb);

本来body系是和imu系重合的，但此时body系在左目相机上 设置一个T矩阵

1.4 其他

bias初值设置
先验位姿初值设置

先前和传播状态设置
预积分初始化

1.5 成员变量

  imuBias::ConstantBias priorImuBias;  // assume zero initial bias
  noiseModel::Robust::shared_ptr velocityNoiseModel;
  noiseModel::Robust::shared_ptr biasNoiseModel;
  NavState prevState;
  NavState propState;
  imuBias::ConstantBias prevBias;
  PreintegratedCombinedMeasurements* preintegrated;

二、main函数

2.1 构造双目相机模型

Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));

这里的K包含了相机内参和基线长度(双目)

2.2 创建因子图

  // Create a factor graph
  std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
  NonlinearFactorGraph graph;
  Values initialEstimate;
  IMUHelper imu;

2.2.1 SmartStereoProjectionPoseFactor

This factor assumes that camera calibration is fixed, but each camera has its own calibration. The factor only constrains poses (variable dimension is 6). This factor requires that values contains the involved poses (Pose3).
只约束位姿
一元因子

2.3 初值初始化

  // Pose prior - at identity
  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
  graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
  initialEstimate.insert(X(0), Pose3::identity());

  // Bias prior
  graph.addPrior(B(1), imu.priorImuBias,
                                               imu.biasNoiseModel);
  initialEstimate.insert(B(0), imu.priorImuBias);

  // Velocity prior - assume stationary
  graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);
  initialEstimate.insert(V(0), Vector3(0, 0, 0));

2.4读取传感器数据进行向因子图中加入因子

    if (type == 'i') {  // Process IMU measurement
      double ax, ay, az;
      double gx, gy, gz;
      double dt = 1 / 800.0;  // IMU at ~800Hz

      ss >> ax;
      ss >> ay;
      ss >> az;

      ss >> gx;
      ss >> gy;
      ss >> gz;

      Vector3 acc(ax, ay, az);
      Vector3 gyr(gx, gy, gz);

      imu.preintegrated->integrateMeasurement(acc, gyr, dt);
    } else if (type == 's') {  // Process stereo measurement
      int landmark;
      double xl, xr, y;

      ss >> landmark;
      ss >> xl;
      ss >> xr;
      ss >> y;

      if (smartFactors.count(landmark) == 0) {
        auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);

        SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);

        smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(
            new SmartStereoProjectionPoseFactor(gaussian, params));
        graph.push_back(smartFactors[landmark]);
      }

      smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
    } else {
      throw runtime_error("unexpected data type: " + string(1, type));
    }

2.5 优化

有数据进去时候进行优化

    if (frame != lastFrame || in.eof()) {
      cout << "Running iSAM for frame: " << lastFrame << "\n";

      initialEstimate.insert(X(lastFrame), Pose3::identity());
      initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
      initialEstimate.insert(B(lastFrame), imu.prevBias);

      CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),
                                  X(lastFrame), V(lastFrame), B(lastFrame - 1),
                                  B(lastFrame), *imu.preintegrated);

      graph.add(imuFactor);

      isam.update(graph, initialEstimate);

      Values currentEstimate = isam.calculateEstimate();

      imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);
      imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),
                               currentEstimate.at<Vector3>(V(lastFrame)));
      imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));
      imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias);

      graph.resize(0);
      initialEstimate.clear();

      if (in.eof()) {
        break;
      }
    }
    /*......*/
    /*......*/
    lastFrame = frame;