020 gtsam/examples/Pose3Localization.cpp

3D位姿SLAM例子
read input from g2o

一、main function

/*......*/ //读取数据

添加先验

  // Add prior on the first key
  auto priorModel = noiseModel::Diagonal::Variances(
      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
  Key firstKey = 0;
  for (const auto key_value : *initial) {
    std::cout << "Adding prior to g2o file " << std::endl;
    firstKey = key_value.key;
    graph->addPrior(firstKey, Pose3(), priorModel);
    break;
  }

进行优化

  std::cout << "Optimizing the factor graph" << std::endl;
  GaussNewtonParams params;
  params.setVerbosity("TERMINATION");  // show info about stopping conditions
  GaussNewtonOptimizer optimizer(*graph, *initial, params);
  Values result = optimizer.optimize();
  std::cout << "Optimization complete" << std::endl;

  std::cout << "initial error=" << graph->error(*initial) << std::endl;
  std::cout << "final error=" << graph->error(result) << std::endl;