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 testPreconditioner.cpp
14 * @brief Unit tests for Preconditioners
15 * @author Sungtae An
16 * @date Nov 6, 2014
17 **/
18
19#include <CppUnitLite/TestHarness.h>
20
21#include <gtsam/nonlinear/Values.h>
22#include <gtsam/linear/GaussianFactorGraph.h>
23#include <gtsam/linear/VectorValues.h>
24#include <gtsam/linear/Preconditioner.h>
25#include <gtsam/linear/PCGSolver.h>
26#include <gtsam/geometry/Point2.h>
27
28using namespace std;
29using namespace gtsam;
30
31/* ************************************************************************* */
32TEST( PCGsolver, verySimpleLinearSystem) {
33
34 // Ax = [4 1][u] = [1] x0 = [2]
35 // [1 3][v] [2] [1]
36 //
37 // exact solution x = [1/11, 7/11]';
38 //
39
40 // Create a Gaussian Factor Graph
41 GaussianFactorGraph simpleGFG;
42 simpleGFG.emplace_shared<JacobianFactor>(args: 0, args&: (Matrix(2,2)<< 4, 1, 1, 3).finished(), args&: (Vector(2) << 1,2 ).finished(), args: noiseModel::Unit::Create(dim: 2));
43
44 // Exact solution already known
45 VectorValues exactSolution;
46 exactSolution.insert(j: 0, value: (Vector(2) << 1./11., 7./11.).finished());
47 //exactSolution.print("Exact");
48
49 // Solve the system using direct method
50 VectorValues deltaDirect = simpleGFG.optimize();
51 EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7));
52 //deltaDirect.print("Direct");
53
54 // Solve the system using Preconditioned Conjugate Gradient solver
55 // Common PCG parameters
56 gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>();
57 pcg->maxIterations = 500;
58 pcg->epsilon_abs = 0.0;
59 pcg->epsilon_rel = 0.0;
60 //pcg->setVerbosity("ERROR");
61
62 // With Dummy preconditioner
63 pcg->preconditioner =
64 std::make_shared<gtsam::DummyPreconditionerParameters>();
65 VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(gfg: simpleGFG);
66 EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7));
67 //deltaPCGDummy.print("PCG Dummy");
68
69 // With Block-Jacobi preconditioner
70 pcg->preconditioner =
71 std::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
72 // It takes more than 1000 iterations for this test
73 pcg->maxIterations = 1500;
74 VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(gfg: simpleGFG);
75
76 EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5));
77 //deltaPCGJacobi.print("PCG Jacobi");
78}
79
80/* ************************************************************************* */
81TEST(PCGSolver, simpleLinearSystem) {
82 // Create a Gaussian Factor Graph
83 GaussianFactorGraph simpleGFG;
84 //SharedDiagonal unit2 = noiseModel::Unit::Create(2);
85 SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.5, 0.3));
86 simpleGFG.emplace_shared<JacobianFactor>(args: 2, args&: (Matrix(2,2)<< 10, 0, 0, 10).finished(), args&: (Vector(2) << -1, -1).finished(), args&: unit2);
87 simpleGFG.emplace_shared<JacobianFactor>(args: 2, args&: (Matrix(2,2)<< -10, 0, 0, -10).finished(), args: 0, args&: (Matrix(2,2)<< 10, 0, 0, 10).finished(), args&: (Vector(2) << 2, -1).finished(), args&: unit2);
88 simpleGFG.emplace_shared<JacobianFactor>(args: 2, args&: (Matrix(2,2)<< -5, 0, 0, -5).finished(), args: 1, args&: (Matrix(2,2)<< 5, 0, 0, 5).finished(), args&: (Vector(2) << 0, 1).finished(), args&: unit2);
89 simpleGFG.emplace_shared<JacobianFactor>(args: 0, args&: (Matrix(2,2)<< -5, 0, 0, -5).finished(), args: 1, args&: (Matrix(2,2)<< 5, 0, 0, 5).finished(), args&: (Vector(2) << -1, 1.5).finished(), args&: unit2);
90 simpleGFG.emplace_shared<JacobianFactor>(args: 0, args&: (Matrix(2,2)<< 1, 0, 0, 1).finished(), args&: (Vector(2) << 0, 0).finished(), args&: unit2);
91 simpleGFG.emplace_shared<JacobianFactor>(args: 1, args&: (Matrix(2,2)<< 1, 0, 0, 1).finished(), args&: (Vector(2) << 0, 0).finished(), args&: unit2);
92 simpleGFG.emplace_shared<JacobianFactor>(args: 2, args&: (Matrix(2,2)<< 1, 0, 0, 1).finished(), args&: (Vector(2) << 0, 0).finished(), args&: unit2);
93 //simpleGFG.print("system");
94
95 // Expected solution
96 VectorValues expectedSolution;
97 expectedSolution.insert(j: 0, value: (Vector(2) << 0.100498, -0.196756).finished());
98 expectedSolution.insert(j: 2, value: (Vector(2) << -0.0990413, -0.0980577).finished());
99 expectedSolution.insert(j: 1, value: (Vector(2) << -0.0973252, 0.100582).finished());
100 //expectedSolution.print("Expected");
101
102 // Solve the system using direct method
103 VectorValues deltaDirect = simpleGFG.optimize();
104 EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5));
105 //deltaDirect.print("Direct");
106
107 // Solve the system using Preconditioned Conjugate Gradient solver
108 // Common PCG parameters
109 gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>();
110 pcg->maxIterations = 500;
111 pcg->epsilon_abs = 0.0;
112 pcg->epsilon_rel = 0.0;
113 //pcg->setVerbosity("ERROR");
114
115 // With Dummy preconditioner
116 pcg->preconditioner =
117 std::make_shared<gtsam::DummyPreconditionerParameters>();
118 VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(gfg: simpleGFG);
119 EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
120 //deltaPCGDummy.print("PCG Dummy");
121
122 // With Block-Jacobi preconditioner
123 pcg->preconditioner =
124 std::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
125 VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(gfg: simpleGFG);
126 EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
127 //deltaPCGJacobi.print("PCG Jacobi");
128
129}
130
131/* ************************************************************************* */
132int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
133/* ************************************************************************* */
134