/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

/**
 * @file GNCExample.cpp
 * @brief Simple example showcasing a Graduated Non-Convexity based solver
 * @author Achintya Mohan
 */

#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/GncOptimizer.h>
#include <gtsam/nonlinear/GncParams.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>

#include <iostream>

using namespace std;
using namespace gtsam;

int main() {
  cout << "Graduated Non-Convexity Example\n";

  NonlinearFactorGraph graph;

  // Add a prior to the first point, set to the origin
  auto priorNoise = noiseModel::Isotropic::Sigma(2, 0.1);
  graph.addPrior(1, Point2(0.0, 0.0), priorNoise);

  // Add additional factors, noise models must be Gaussian 
  Point2 x1(1.0, 0.0);
  graph.emplace_shared<BetweenFactor<Point2>>(1, 2, x1, noiseModel::Isotropic::Sigma(2, 0.2));
  Point2 x2(1.1, 0.1);
  graph.emplace_shared<BetweenFactor<Point2>>(2, 3, x2, noiseModel::Isotropic::Sigma(2, 0.4));

  // Initial estimates
  Values initial;
  initial.insert(1, Point2(0.5, -0.1));
  initial.insert(2, Point2(1.3, 0.1));
  initial.insert(3, Point2(0.8, 0.2));

  // Set options for the non-minimal solver
  LevenbergMarquardtParams lmParams;
  lmParams.setMaxIterations(1000);
  lmParams.setRelativeErrorTol(1e-5);

  // Set GNC-specific options
  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
  gncParams.setLossType(GncLossType::TLS);

  // Optimize the graph and print results
  GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph, initial, gncParams);
  Values result = optimizer.optimize();
  result.print("Final Result:");

  return 0;
}