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