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 testIterative.cpp
14 * @brief Unit tests for iterative methods
15 * @author Frank Dellaert
16 **/
17
18#include <tests/smallExample.h>
19#include <gtsam/slam/BetweenFactor.h>
20#include <gtsam/nonlinear/NonlinearEquality.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/linear/iterative.h>
23#include <gtsam/geometry/Pose2.h>
24
25#include <CppUnitLite/TestHarness.h>
26
27using namespace std;
28using namespace gtsam;
29using namespace example;
30using symbol_shorthand::X; // to create pose keys
31using symbol_shorthand::L; // to create landmark keys
32
33static ConjugateGradientParameters parameters;
34// add following below to add printing:
35// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY;
36
37/* ************************************************************************* */
38TEST( Iterative, steepestDescent )
39{
40 // Create factor graph
41 GaussianFactorGraph fg = createGaussianFactorGraph();
42
43 // eliminate and solve
44 VectorValues expected = fg.optimize();
45
46 // Do gradient descent
47 VectorValues zero = VectorValues::Zero(other: expected); // TODO, how do we do this normally?
48 VectorValues actual = steepestDescent(fg, x: zero, parameters);
49 CHECK(assert_equal(expected,actual,1e-2));
50}
51
52/* ************************************************************************* */
53TEST( Iterative, conjugateGradientDescent )
54{
55 // Create factor graph
56 GaussianFactorGraph fg = createGaussianFactorGraph();
57
58 // eliminate and solve
59 VectorValues expected = fg.optimize();
60
61 // get matrices
62 Vector x0 = Z_6x1;
63 const auto [A, b] = fg.jacobian();
64 Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished();
65
66 // Do conjugate gradient descent, System version
67 System Ab(A, b);
68 Vector actualX = conjugateGradientDescent(Ab, x: x0, parameters);
69 CHECK(assert_equal(expectedX,actualX,1e-9));
70
71 // Do conjugate gradient descent, Matrix version
72 Vector actualX2 = conjugateGradientDescent(A, b, x: x0, parameters);
73 CHECK(assert_equal(expectedX,actualX2,1e-9));
74
75 // Do conjugate gradient descent on factor graph
76 VectorValues zero = VectorValues::Zero(other: expected);
77 VectorValues actual = conjugateGradientDescent(fg, x: zero, parameters);
78 CHECK(assert_equal(expected,actual,1e-2));
79}
80
81/* ************************************************************************* */
82TEST( Iterative, conjugateGradientDescent_hard_constraint )
83{
84 Values config;
85 Pose2 pose1 = Pose2(0.,0.,0.);
86 config.insert(j: X(j: 1), val: pose1);
87 config.insert(j: X(j: 2), val: Pose2(1.5,0.,0.));
88
89 NonlinearFactorGraph graph;
90 graph.emplace_shared<NonlinearEquality<Pose2>>(args: X(j: 1), args&: pose1);
91 graph.emplace_shared<BetweenFactor<Pose2>>(args: X(j: 1),args: X(j: 2), args: Pose2(1.,0.,0.), args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 1));
92
93 std::shared_ptr<GaussianFactorGraph> fg = graph.linearize(linearizationPoint: config);
94
95 VectorValues zeros = config.zeroVectors();
96
97 ConjugateGradientParameters parameters;
98 parameters.epsilon_abs = 1e-3;
99 parameters.epsilon_rel = 1e-5;
100 parameters.maxIterations = 100;
101 VectorValues actual = conjugateGradientDescent(fg: *fg, x: zeros, parameters);
102
103 VectorValues expected;
104 expected.insert(j: X(j: 1), value: Z_3x1);
105 expected.insert(j: X(j: 2), value: Vector3(-0.5,0.,0.));
106 CHECK(assert_equal(expected, actual));
107}
108
109/* ************************************************************************* */
110TEST( Iterative, conjugateGradientDescent_soft_constraint )
111{
112 Values config;
113 config.insert(j: X(j: 1), val: Pose2(0.,0.,0.));
114 config.insert(j: X(j: 2), val: Pose2(1.5,0.,0.));
115
116 NonlinearFactorGraph graph;
117 graph.addPrior(key: X(j: 1), prior: Pose2(0.,0.,0.), model: noiseModel::Isotropic::Sigma(dim: 3, sigma: 1e-10));
118 graph.emplace_shared<BetweenFactor<Pose2>>(args: X(j: 1),args: X(j: 2), args: Pose2(1.,0.,0.), args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 1));
119
120 std::shared_ptr<GaussianFactorGraph> fg = graph.linearize(linearizationPoint: config);
121
122 VectorValues zeros = config.zeroVectors();
123
124 ConjugateGradientParameters parameters;
125 parameters.epsilon_abs = 1e-3;
126 parameters.epsilon_rel = 1e-5;
127 parameters.maxIterations = 100;
128 VectorValues actual = conjugateGradientDescent(fg: *fg, x: zeros, parameters);
129
130 VectorValues expected;
131 expected.insert(j: X(j: 1), value: Z_3x1);
132 expected.insert(j: X(j: 2), value: Vector3(-0.5,0.,0.));
133 CHECK(assert_equal(expected, actual));
134}
135
136/* ************************************************************************* */
137int main() {
138 TestResult tr;
139 return TestRegistry::runAllTests(result&: tr);
140}
141/* ************************************************************************* */
142