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 METISOrdering.cpp
14 * @brief Simple robot motion example, with prior and two odometry measurements,
15 * using a METIS ordering
16 * @author Frank Dellaert
17 * @author Andrew Melim
18 */
19
20/**
21 * Example of a simple 2D localization example optimized using METIS ordering
22 * - For more details on the full optimization pipeline, see OdometryExample.cpp
23 */
24
25#include <gtsam/geometry/Pose2.h>
26#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
27#include <gtsam/nonlinear/Marginals.h>
28#include <gtsam/nonlinear/NonlinearFactorGraph.h>
29#include <gtsam/nonlinear/Values.h>
30#include <gtsam/slam/BetweenFactor.h>
31
32using namespace std;
33using namespace gtsam;
34
35int main(int argc, char** argv) {
36 NonlinearFactorGraph graph;
37
38 Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
39 auto priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.3, 0.3, 0.1));
40 graph.addPrior(key: 1, prior: priorMean, model: priorNoise);
41
42 Pose2 odometry(2.0, 0.0, 0.0);
43 auto odometryNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
44 graph.emplace_shared<BetweenFactor<Pose2> >(args: 1, args: 2, args&: odometry, args&: odometryNoise);
45 graph.emplace_shared<BetweenFactor<Pose2> >(args: 2, args: 3, args&: odometry, args&: odometryNoise);
46 graph.print(str: "\nFactor Graph:\n"); // print
47
48 Values initial;
49 initial.insert(j: 1, val: Pose2(0.5, 0.0, 0.2));
50 initial.insert(j: 2, val: Pose2(2.3, 0.1, -0.2));
51 initial.insert(j: 3, val: Pose2(4.1, 0.1, 0.1));
52 initial.print(str: "\nInitial Estimate:\n"); // print
53
54 // optimize using Levenberg-Marquardt optimization
55 LevenbergMarquardtParams params;
56 // In order to specify the ordering type, we need to se the "orderingType". By
57 // default this parameter is set to OrderingType::COLAMD
58 params.orderingType = Ordering::METIS;
59 LevenbergMarquardtOptimizer optimizer(graph, initial, params);
60 Values result = optimizer.optimize();
61 result.print(str: "Final Result:\n");
62
63 return 0;
64}
65