testIterative.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 
18 #include <tests/smallExample.h>
21 #include <gtsam/inference/Symbol.h>
22 #include <gtsam/linear/iterative.h>
23 #include <gtsam/geometry/Pose2.h>
24 
26 
27 using namespace std;
28 using namespace gtsam;
29 using namespace example;
30 using symbol_shorthand::X; // to create pose keys
31 using symbol_shorthand::L; // to create landmark keys
32 
34 // add following below to add printing:
35 // parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY;
36 
37 /* ************************************************************************* */
38 TEST( Iterative, steepestDescent )
39 {
40  // Create factor graph
42 
43  // eliminate and solve
45 
46  // Do gradient descent
47  VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally?
48  VectorValues actual = steepestDescent(fg, zero, parameters);
49  CHECK(assert_equal(expected,actual,1e-2));
50 }
51 
52 /* ************************************************************************* */
54 {
55  // Create factor graph
57 
58  // eliminate and solve
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, x0, parameters);
69  CHECK(assert_equal(expectedX,actualX,1e-9));
70 
71  // Do conjugate gradient descent, Matrix version
72  Vector actualX2 = conjugateGradientDescent(A, b, x0, parameters);
73  CHECK(assert_equal(expectedX,actualX2,1e-9));
74 
75  // Do conjugate gradient descent on factor graph
76  VectorValues zero = VectorValues::Zero(expected);
77  VectorValues actual = conjugateGradientDescent(fg, zero, parameters);
78  CHECK(assert_equal(expected,actual,1e-2));
79 }
80 
81 /* ************************************************************************* */
82 TEST( Iterative, conjugateGradientDescent_hard_constraint )
83 {
84  Values config;
85  Pose2 pose1 = Pose2(0.,0.,0.);
86  config.insert(X(1), pose1);
87  config.insert(X(2), Pose2(1.5,0.,0.));
88 
91  graph.emplace_shared<BetweenFactor<Pose2>>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
92 
93  std::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);
94 
95  VectorValues zeros = config.zeroVectors();
96 
98  parameters.setEpsilon_abs(1e-3);
99  parameters.setEpsilon_rel(1e-5);
100  parameters.setMaxIterations(100);
101  VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
102 
104  expected.insert(X(1), Z_3x1);
105  expected.insert(X(2), Vector3(-0.5,0.,0.));
106  CHECK(assert_equal(expected, actual));
107 }
108 
109 /* ************************************************************************* */
110 TEST( Iterative, conjugateGradientDescent_soft_constraint )
111 {
112  Values config;
113  config.insert(X(1), Pose2(0.,0.,0.));
114  config.insert(X(2), Pose2(1.5,0.,0.));
115 
117  graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
118  graph.emplace_shared<BetweenFactor<Pose2>>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
119 
120  std::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);
121 
122  VectorValues zeros = config.zeroVectors();
123 
125  parameters.setEpsilon_abs(1e-3);
126  parameters.setEpsilon_rel(1e-5);
127  parameters.setMaxIterations(100);
128  VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
129 
131  expected.insert(X(1), Z_3x1);
132  expected.insert(X(2), Vector3(-0.5,0.,0.));
133  CHECK(assert_equal(expected, actual));
134 }
135 
136 /* ************************************************************************* */
137 int main() {
138  TestResult tr;
139  return TestRegistry::runAllTests(tr);
140 }
141 /* ************************************************************************* */
#define CHECK(condition)
Definition: Test.h:108
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
Scalar * b
Definition: benchVecAdd.cpp:17
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:270
Matrix expected
Definition: testMatrix.cpp:971
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:296
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Iterative methods, implementation.
MatrixXd L
Definition: LLT_example.cpp:6
Definition: BFloat16.h:88
iterator insert(const std::pair< Key, Vector > &key_value)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
NonlinearFactorGraph graph
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Definition: iterative.cpp:45
Eigen::VectorXd Vector
Definition: Vector.h:38
VectorValues zeroVectors() const
Definition: Values.cpp:260
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static ConjugateGradientParameters parameters
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
TEST(Iterative, steepestDescent)
traits
Definition: chartTesting.h:28
static Symbol x0('x', 0)
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
Create small example with two poses and one landmark.
void insert(Key j, const Value &val)
Definition: Values.cpp:155
2D Pose
#define X
Definition: icosphere.cpp:20
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Vector steepestDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Definition: iterative.cpp:40
int main()


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:33