021 gtsam/examples/Pose3SLAMExample_changeKeys.cpp

从g2o数据中读入位姿，利用InitializePose3优化Pose3位姿

改变key

  if (argc < 3) {
    std::cout << "Please provide output file to write "   << std::endl;
  } else {
    const string inputFileRewritten = argv[2];
    std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
    // Additional: rewrite input with simplified keys 0,1,...
    Values simpleInitial;
    for(const auto key_value: *initial) {
      Key key;
      if(add)
        key = key_value.key + firstKey;
      else
        key = key_value.key - firstKey;

      simpleInitial.insert(key, initial->at(key_value.key));
    }
    NonlinearFactorGraph simpleGraph;
    for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
      boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
          boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
      if (pose3Between){
        Key key1, key2;
        if(add){
          key1 = pose3Between->key1() + firstKey;
          key2 = pose3Between->key2() + firstKey;
        }else{
          key1 = pose3Between->key1() - firstKey;
          key2 = pose3Between->key2() - firstKey;
        }
        NonlinearFactor::shared_ptr simpleFactor(
            new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));
        simpleGraph.add(simpleFactor);
      }
    }
    writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
  }