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 testPCGSolver.cpp
14 * @brief Unit tests for PCGSolver class
15 * @author Yong-Dian Jian
16 * @date Aug 06, 2014
17 */
18
19#include <tests/smallExample.h>
20#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
21#include <gtsam/linear/GaussianFactorGraph.h>
22#include <gtsam/linear/PCGSolver.h>
23#include <gtsam/linear/SubgraphPreconditioner.h>
24#include <gtsam/inference/Symbol.h>
25#include <gtsam/base/Matrix.h>
26
27#include <CppUnitLite/TestHarness.h>
28
29#include <iostream>
30#include <fstream>
31
32using namespace std;
33using namespace gtsam;
34
35const double tol = 1e-3;
36
37using symbol_shorthand::X;
38using symbol_shorthand::L;
39
40/* ************************************************************************* */
41// Test cholesky decomposition
42TEST( PCGSolver, llt ) {
43 Matrix R = (Matrix(3,3) <<
44 1., -1., -1.,
45 0., 2., -1.,
46 0., 0., 1.).finished();
47 Matrix AtA = R.transpose() * R;
48
49 Vector Rvector = (Vector(9) << 1., -1., -1.,
50 0., 2., -1.,
51 0., 0., 1.).finished();
52// Vector Rvector = (Vector(6) << 1., -1., -1.,
53// 2., -1.,
54// 1.).finished();
55
56 Vector b = Vector3(1., 2., 3.);
57
58 Vector x = Vector3(6.5, 2.5, 3.) ;
59
60 /* test cholesky */
61 Matrix Rhat = AtA.llt().matrixL().transpose();
62 EXPECT(assert_equal(R, Rhat, 1e-5));
63
64 /* test backward substitution */
65 Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(other: b);
66 EXPECT(assert_equal(x, xhat, 1e-5));
67
68 /* test in-place back substitution */
69 xhat = b;
70 Rhat.triangularView<Eigen::Upper>().solveInPlace(other: xhat);
71 EXPECT(assert_equal(x, xhat, 1e-5));
72
73 /* test triangular matrix map */
74 Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
75 xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(other: b);
76 EXPECT(assert_equal(x, xhat, 1e-5));
77
78}
79
80/* ************************************************************************* */
81// Test GaussianFactorGraphSystem::multiply and getb
82TEST( GaussianFactorGraphSystem, multiply_getb)
83{
84 // Create a Gaussian Factor Graph
85 GaussianFactorGraph simpleGFG;
86 SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.5, 0.3));
87 simpleGFG.emplace_shared<JacobianFactor>(args: 2, args&: (Matrix(2,2)<< 10, 0, 0, 10).finished(), args&: (Vector(2) << -1, -1).finished(), args&: unit2);
88 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);
89 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);
90 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);
91 simpleGFG.emplace_shared<JacobianFactor>(args: 0, args&: (Matrix(2,2)<< 1, 0, 0, 1).finished(), args&: (Vector(2) << 0, 0).finished(), args&: unit2);
92 simpleGFG.emplace_shared<JacobianFactor>(args: 1, args&: (Matrix(2,2)<< 1, 0, 0, 1).finished(), args&: (Vector(2) << 0, 0).finished(), args&: unit2);
93 simpleGFG.emplace_shared<JacobianFactor>(args: 2, args&: (Matrix(2,2)<< 1, 0, 0, 1).finished(), args&: (Vector(2) << 0, 0).finished(), args&: unit2);
94
95 // Create a dummy-preconditioner and a GaussianFactorGraphSystem
96 DummyPreconditioner dummyPreconditioner;
97 KeyInfo keyInfo(simpleGFG);
98 std::map<Key,Vector> lambda;
99 dummyPreconditioner.build(gfg: simpleGFG, info: keyInfo, lambda);
100 GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);
101
102 // Prepare container for each variable
103 Vector initial, residual, preconditionedResidual, p, actualAp;
104 initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
105
106 // Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
107 gfgs.residual(x: initial, r&: residual); /* r = b-Ax */
108 gfgs.leftPrecondition(x: residual, y&: preconditionedResidual); /* pr = L^{-1} (b-Ax) */
109 gfgs.rightPrecondition(x: preconditionedResidual, y&: p); /* p = L^{-T} pr */
110 gfgs.multiply(x: p, y&: actualAp); /* A p */
111
112 // Expected value of Ap for the first iteration of this example problem
113 Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
114 EXPECT(assert_equal(expectedAp, actualAp, 1e-3));
115
116 // Expected value of getb
117 Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
118 Vector actualb;
119 gfgs.getb(b&: actualb);
120 EXPECT(assert_equal(expectedb, actualb, 1e-3));
121}
122
123/* ************************************************************************* */
124// Test Dummy Preconditioner
125TEST(PCGSolver, dummy) {
126 LevenbergMarquardtParams params;
127 params.linearSolverType = LevenbergMarquardtParams::Iterative;
128 auto pcg = std::make_shared<PCGSolverParameters>(
129 args: std::make_shared<DummyPreconditionerParameters>());
130 params.iterativeParams = pcg;
131
132 NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
133
134 Point2 x0(10, 10);
135 Values c0;
136 c0.insert(j: X(j: 1), val: x0);
137
138 Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
139
140 DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
141}
142
143/* ************************************************************************* */
144// Test Block-Jacobi Precondioner
145TEST(PCGSolver, blockjacobi) {
146 LevenbergMarquardtParams params;
147 params.linearSolverType = LevenbergMarquardtParams::Iterative;
148 auto pcg = std::make_shared<PCGSolverParameters>(
149 args: std::make_shared<BlockJacobiPreconditionerParameters>());
150 params.iterativeParams = pcg;
151
152 NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
153
154 Point2 x0(10, 10);
155 Values c0;
156 c0.insert(j: X(j: 1), val: x0);
157
158 Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
159
160 DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
161}
162
163/* ************************************************************************* */
164// Test Incremental Subgraph PCG Solver
165TEST(PCGSolver, subgraph) {
166 LevenbergMarquardtParams params;
167 params.linearSolverType = LevenbergMarquardtParams::Iterative;
168 auto pcg = std::make_shared<PCGSolverParameters>(
169 args: std::make_shared<SubgraphPreconditionerParameters>());
170 params.iterativeParams = pcg;
171
172 NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
173
174 Point2 x0(10, 10);
175 Values c0;
176 c0.insert(j: X(j: 1), val: x0);
177
178 Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
179
180 DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
181}
182
183/* ************************************************************************* */
184int main() {
185 TestResult tr;
186 return TestRegistry::runAllTests(result&: tr);
187}
188
189