015 gtsam/examples/METISOrderingExample.cpp

使用METIS进行排序的2D定位示例

一、main function

1.1 初值、里程计、待优化值

  Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  graph.addPrior(1, priorMean, priorNoise);

  Pose2 odometry(2.0, 0.0, 0.0);
  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
  graph.print("\nFactor Graph:\n");  // print

  Values initial;
  initial.insert(1, Pose2(0.5, 0.0, 0.2));
  initial.insert(2, Pose2(2.3, 0.1, -0.2));
  initial.insert(3, Pose2(4.1, 0.1, 0.1));
  initial.print("\nInitial Estimate:\n");  // print

1.2 优化

指定排序类型，默认是OrderingType::COLAMD

  // optimize using Levenberg-Marquardt optimization
  LevenbergMarquardtParams params;
  // In order to specify the ordering type, we need to se the "orderingType". By
  // default this parameter is set to OrderingType::COLAMD
  params.orderingType = Ordering::METIS;
  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
  Values result = optimizer.optimize();
  result.print("Final Result:\n");

  return 0;