testPCGSolver.cpp
Go to the documentation of this file.
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 
19 #include <tests/smallExample.h>
22 #include <gtsam/linear/PCGSolver.h>
24 #include <gtsam/inference/Symbol.h>
25 #include <gtsam/base/Matrix.h>
26 
28 
29 #include <boost/shared_ptr.hpp>
30 #include <boost/assign/std/list.hpp> // for operator +=
31 using namespace boost::assign;
32 
33 #include <iostream>
34 #include <fstream>
35 
36 using namespace std;
37 using namespace gtsam;
38 
39 const double tol = 1e-3;
40 
43 
44 /* ************************************************************************* */
45 // Test cholesky decomposition
46 TEST( PCGSolver, llt ) {
47  Matrix R = (Matrix(3,3) <<
48  1., -1., -1.,
49  0., 2., -1.,
50  0., 0., 1.).finished();
51  Matrix AtA = R.transpose() * R;
52 
53  Vector Rvector = (Vector(9) << 1., -1., -1.,
54  0., 2., -1.,
55  0., 0., 1.).finished();
56 // Vector Rvector = (Vector(6) << 1., -1., -1.,
57 // 2., -1.,
58 // 1.).finished();
59 
60  Vector b = Vector3(1., 2., 3.);
61 
62  Vector x = Vector3(6.5, 2.5, 3.) ;
63 
64  /* test cholesky */
65  Matrix Rhat = AtA.llt().matrixL().transpose();
66  EXPECT(assert_equal(R, Rhat, 1e-5));
67 
68  /* test backward substitution */
69  Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
70  EXPECT(assert_equal(x, xhat, 1e-5));
71 
72  /* test in-place back substitution */
73  xhat = b;
74  Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
75  EXPECT(assert_equal(x, xhat, 1e-5));
76 
77  /* test triangular matrix map */
78  Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
79  xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
80  EXPECT(assert_equal(x, xhat, 1e-5));
81 
82 }
83 
84 /* ************************************************************************* */
85 // Test GaussianFactorGraphSystem::multiply and getb
87 {
88  // Create a Gaussian Factor Graph
89  GaussianFactorGraph simpleGFG;
90  SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
91  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
92  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
93  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
94  simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
95  simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
96  simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
97  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
98 
99  // Create a dummy-preconditioner and a GaussianFactorGraphSystem
100  DummyPreconditioner dummyPreconditioner;
101  KeyInfo keyInfo(simpleGFG);
102  std::map<Key,Vector> lambda;
103  dummyPreconditioner.build(simpleGFG, keyInfo, lambda);
104  GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);
105 
106  // Prepare container for each variable
107  Vector initial, residual, preconditionedResidual, p, actualAp;
108  initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
109 
110  // Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
111  gfgs.residual(initial, residual); /* r = b-Ax */
112  gfgs.leftPrecondition(residual, preconditionedResidual); /* pr = L^{-1} (b-Ax) */
113  gfgs.rightPrecondition(preconditionedResidual, p); /* p = L^{-T} pr */
114  gfgs.multiply(p, actualAp); /* A p */
115 
116  // Expected value of Ap for the first iteration of this example problem
117  Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
118  EXPECT(assert_equal(expectedAp, actualAp, 1e-3));
119 
120  // Expected value of getb
121  Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
122  Vector actualb;
123  gfgs.getb(actualb);
124  EXPECT(assert_equal(expectedb, actualb, 1e-3));
125 }
126 
127 /* ************************************************************************* */
128 // Test Dummy Preconditioner
129 TEST(PCGSolver, dummy) {
131  params.linearSolverType = LevenbergMarquardtParams::Iterative;
132  auto pcg = boost::make_shared<PCGSolverParameters>();
133  pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
134  params.iterativeParams = pcg;
135 
137 
138  Point2 x0(10, 10);
139  Values c0;
140  c0.insert(X(1), x0);
141 
142  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
143 
144  DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
145 }
146 
147 /* ************************************************************************* */
148 // Test Block-Jacobi Precondioner
149 TEST(PCGSolver, blockjacobi) {
151  params.linearSolverType = LevenbergMarquardtParams::Iterative;
152  auto pcg = boost::make_shared<PCGSolverParameters>();
153  pcg->preconditioner_ =
154  boost::make_shared<BlockJacobiPreconditionerParameters>();
155  params.iterativeParams = pcg;
156 
158 
159  Point2 x0(10, 10);
160  Values c0;
161  c0.insert(X(1), x0);
162 
163  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
164 
165  DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
166 }
167 
168 /* ************************************************************************* */
169 // Test Incremental Subgraph PCG Solver
170 TEST(PCGSolver, subgraph) {
172  params.linearSolverType = LevenbergMarquardtParams::Iterative;
173  auto pcg = boost::make_shared<PCGSolverParameters>();
174  pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
175  params.iterativeParams = pcg;
176 
178 
179  Point2 x0(10, 10);
180  Values c0;
181  c0.insert(X(1), x0);
182 
183  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
184 
185  DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
186 }
187 
188 /* ************************************************************************* */
189 int main() {
190  TestResult tr;
191  return TestRegistry::runAllTests(tr);
192 }
193 
TEST(PCGSolver, llt)
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
virtual const Values & optimize()
Scalar * b
Definition: benchVecAdd.cpp:17
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
void insert(Key j, const Value &val)
Definition: Values.cpp:140
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
Vector2 Point2
Definition: Point2.h:27
NonlinearFactorGraph createReallyNonlinearFactorGraph()
Definition: smallExample.h:377
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Rot2 R(Rot2::fromAngle(0.1))
Values initial
MatrixXd L
Definition: LLT_example.cpp:6
Definition: Half.h:150
void leftPrecondition(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:124
void residual(const Vector &x, Vector &r) const
Definition: PCGSolver.cpp:83
Eigen::VectorXd Vector
Definition: Vector.h:38
Matrix2 transpose() const
Definition: Rot2.cpp:96
#define EXPECT(condition)
Definition: Test.h:151
const double tol
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Linear Factor Graph where all factors are Gaussians.
void multiply(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:96
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
static SmartStereoProjectionParams params
int main()
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
static Symbol x0('x', 0)
static SharedNoiseModel unit2(noiseModel::Unit::Create(2))
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Eigen::Vector2d Vector2
Definition: Vector.h:42
void getb(Vector &b) const
Definition: PCGSolver.cpp:113
float * p
Create small example with two poses and one landmark.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
double error(const Values &values) const
#define X
Definition: icosphere.cpp:20
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
void rightPrecondition(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:132
LinearSolverType linearSolverType
The type of linear solver to use in the nonlinear optimizer.


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:34