013 gtsam/examples/ISAM2Example_SmartFactor.cpp

issue367

一、预先定义

typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;

Cal3_S2构造函数

inline gtsam::Cal3_S2::Cal3_S2(double fx, double fy, double s, double u0, double v0)
+5 overloads

constructor from doubles

二、main函数

内参定义

Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));

像素的噪声模型

  auto measurementNoise =
      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v

位姿的噪声模型

  Vector6 sigmas;
  sigmas << Vector3::Constant(0.1), Vector3::Constant(0.3);
  auto noise = noiseModel::Diagonal::Sigmas(sigmas);

isam参数设置

  ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.01;
  parameters.relinearizeSkip = 1;
  parameters.cacheLinearizedFactors = false;
  parameters.enableDetailedResults = true;
  parameters.print();
  ISAM2 isam(parameters);

创建因子图,

  // Create a factor graph
  NonlinearFactorGraph graph;
  Values initialEstimate;

  Point3 point(0.0, 0.0, 1.0);

  // Intentionally initialize the variables off from the ground truth
  Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20));

  Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0));
  Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0));
  Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0));
  Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0));
  Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0));

  vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};

添加第一个位姿

  // Add first pose
  graph.addPrior(X(0), poses[0], noise);
  initialEstimate.insert(X(0), poses[0].compose(delta));

创建smart factor

  // Create smart factor with measurement from first pose only
  SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
  smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
  graph.push_back(smartFactor);

1.1 主循环

  for (size_t i = 1; i < 5; i++) {
    cout << "****************************************************" << endl;
    cout << "i = " << i << endl;

    // Add prior on new pose
    // 添加先验位姿
    graph.addPrior(X(i), poses[i], noise);
    initialEstimate.insert(X(i), poses[i].compose(delta));

    // "Simulate" measurement from this pose
    // 得到该位姿下对地图点的观测
    PinholePose<Cal3_S2> camera(poses[i], K);
    Point2 measurement = camera.project(point);
    cout << "Measurement " << i << "" << measurement << endl;

    // Add measurement to smart factor
    // 向smart factor中添加观测
    smartFactor->add(measurement, X(i));

    // Update iSAM2
    // 更新优化器 //返回更新的isam2结果
    ISAM2Result result = isam.update(graph, initialEstimate);
    result.print();

    cout << "Detailed results:" << endl;
    for (auto keyedStatus : result.detail->variableStatus) {
      const auto& status = keyedStatus.second;
      PrintKey(keyedStatus.first);
      cout << " {" << endl;
      cout << "reeliminated: " << status.isReeliminated << endl;
      cout << "relinearized above thresh: " << status.isAboveRelinThreshold
           << endl;
      cout << "relinearized involved: " << status.isRelinearizeInvolved << endl;
      cout << "relinearized: " << status.isRelinearized << endl;
      cout << "observed: " << status.isObserved << endl;
      cout << "new: " << status.isNew << endl;
      cout << "in the root clique: " << status.inRootClique << endl;
      cout << "}" << endl;
    }
	// 根据更新结果计算估计值
    Values currentEstimate = isam.calculateEstimate();
    currentEstimate.print("Current estimate: ");
	// 根据位姿获取landmark
    boost::optional<Point3> pointEstimate = smartFactor->point(currentEstimate);
    if (pointEstimate) {
      cout << *pointEstimate << endl;
    } else {
      cout << "Point degenerate." << endl;
    }

    // Reset graph and initial estimate for next iteration
    graph.resize(0);
    initialEstimate.clear();
  }