Pose2SLAMExample_lago.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>
 
using namespace std;
using namespace gtsam;
 
int main(const int argc, const char *argv[]) {
  // Read graph from file
  string g2oFile;
  if (argc < 2)
    g2oFile = findExampleDataFile("noisyToyGraph.txt");
  else
    g2oFile = argv[1];
 
  NonlinearFactorGraph::shared_ptr graph;
  Values::shared_ptr initial;
  std::tie(graph, initial) = readG2o(g2oFile);
 
  // Add prior on the pose having index (key) = 0
  auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
  graph->addPrior(0, Pose2(), priorModel);
  graph->print();
 
  std::cout << "Computing LAGO estimate" << std::endl;
  Values estimateLago = lago::initialize(*graph);
  std::cout << "done!" << std::endl;
 
  if (argc < 3) {
    estimateLago.print("estimateLago");
  } else {
    const string outputFile = argv[2];
    std::cout << "Writing results to file: " << outputFile << std::endl;
    NonlinearFactorGraph::shared_ptr graphNoKernel;
    Values::shared_ptr initial2;
    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
    writeG2o(*graphNoKernel, estimateLago, outputFile);
    std::cout << "done! " << std::endl;
  }
 
  return 0;
}