1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file GNCExample.cpp
14 * @brief Simple example showcasing a Graduated Non-Convexity based solver
15 * @author Achintya Mohan
16 */
17
18/**
19 * A simple 2D pose graph optimization example
20 * - The robot is initially at origin (0.0, 0.0, 0.0)
21 * - We have full odometry measurements for 2 motions
22 * - The robot first moves to (1.0, 0.0, 0.1) and then to (1.0, 1.0, 0.2)
23 */
24
25#include <gtsam/geometry/Pose2.h>
26#include <gtsam/nonlinear/GncOptimizer.h>
27#include <gtsam/nonlinear/GncParams.h>
28#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
29#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
30#include <gtsam/nonlinear/NonlinearFactorGraph.h>
31#include <gtsam/slam/BetweenFactor.h>
32
33#include <iostream>
34
35using namespace std;
36using namespace gtsam;
37
38int main() {
39 cout << "Graduated Non-Convexity Example\n";
40
41 NonlinearFactorGraph graph;
42
43 // Add a prior to the first point, set to the origin
44 auto priorNoise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
45 graph.addPrior(key: 1, prior: Pose2(0.0, 0.0, 0.0), model: priorNoise);
46
47 // Add additional factors, noise models must be Gaussian
48 Pose2 x1(1.0, 0.0, 0.1);
49 graph.emplace_shared<BetweenFactor<Pose2>>(args: 1, args: 2, args&: x1, args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.2));
50 Pose2 x2(0.0, 1.0, 0.1);
51 graph.emplace_shared<BetweenFactor<Pose2>>(args: 2, args: 3, args&: x2, args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.4));
52
53 // Initial estimates
54 Values initial;
55 initial.insert(j: 1, val: Pose2(0.2, 0.5, -0.1));
56 initial.insert(j: 2, val: Pose2(0.8, 0.3, 0.1));
57 initial.insert(j: 3, val: Pose2(0.8, 0.2, 0.3));
58
59 // Set options for the non-minimal solver
60 LevenbergMarquardtParams lmParams;
61 lmParams.setMaxIterations(1000);
62 lmParams.setRelativeErrorTol(1e-5);
63
64 // Set GNC-specific options
65 GncParams<LevenbergMarquardtParams> gncParams(lmParams);
66 gncParams.setLossType(GncLossType::TLS);
67
68 // Optimize the graph and print results
69 GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph, initial, gncParams);
70 Values result = optimizer.optimize();
71 result.print(str: "Final Result:");
72
73 return 0;
74}
75
76