diff --git a/examples/GNCExample.cpp b/examples/GNCExample.cpp new file mode 100644 index 000000000..8a4f0706f --- /dev/null +++ b/examples/GNCExample.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +/** + * A simple 2D pose graph optimization example + * - The robot is initially at origin (0.0, 0.0, 0.0) + * - We have full odometry measurements for 2 motions + * - The robot first moves to (1.0, 0.0, 0.1) and then to (1.0, 1.0, 0.2) + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +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(3, 0.1); + graph.addPrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); + + // Add additional factors, noise models must be Gaussian + Pose2 x1(1.0, 0.0, 0.1); + graph.emplace_shared>(1, 2, x1, noiseModel::Isotropic::Sigma(3, 0.2)); + Pose2 x2(0.0, 1.0, 0.1); + graph.emplace_shared>(2, 3, x2, noiseModel::Isotropic::Sigma(3, 0.4)); + + // Initial estimates + Values initial; + initial.insert(1, Pose2(0.2, 0.5, -0.1)); + initial.insert(2, Pose2(0.8, 0.3, 0.1)); + initial.insert(3, Pose2(0.8, 0.2, 0.3)); + + // Set options for the non-minimal solver + LevenbergMarquardtParams lmParams; + lmParams.setMaxIterations(1000); + lmParams.setRelativeErrorTol(1e-5); + + // Set GNC-specific options + GncParams gncParams(lmParams); + gncParams.setLossType(GncLossType::TLS); + + // Optimize the graph and print results + GncOptimizer> optimizer(graph, initial, gncParams); + Values result = optimizer.optimize(); + result.print("Final Result:"); + + return 0; +} +