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?
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);
69  CHECK(assert_equal(expectedX,actualX,1e-9));
70 
71  // Do conjugate gradient descent, Matrix version
73  CHECK(assert_equal(expectedX,actualX2,1e-9));
74 
75  // Do conjugate gradient descent on factor graph
76  VectorValues zero = VectorValues::Zero(expected);
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 
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.epsilon_abs = 1e-3;
126  parameters.epsilon_rel = 1e-5;
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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
gtsam::steepestDescent
Vector steepestDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Definition: iterative.cpp:40
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestHarness.h
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::GaussianFactorGraph::jacobian
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:232
TEST
TEST(Iterative, steepestDescent)
Definition: testIterative.cpp:38
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
main
int main()
Definition: testIterative.cpp:137
example
Definition: testOrdering.cpp:28
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::conjugateGradientDescent
Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Definition: iterative.cpp:45
gtsam::VectorValues
Definition: VectorValues.h:74
BetweenFactor.h
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:270
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
zero
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:296
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
x0
static Symbol x0('x', 0)
gtsam::System
Definition: iterative.h:44
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
iterative.h
Iterative methods, implementation.
NonlinearEquality.h
TestResult
Definition: TestResult.h:26
gtsam::ConjugateGradientParameters::epsilon_rel
double epsilon_rel
threshold for relative error decrease
Definition: ConjugateGradientSolver.h:37
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Definition: Values.cpp:260
gtsam
traits
Definition: SFMdata.h:40
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::ConjugateGradientParameters
Definition: ConjugateGradientSolver.h:29
gtsam::ConjugateGradientParameters::maxIterations
size_t maxIterations
maximum number of cg iterations
Definition: ConjugateGradientSolver.h:35
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: GaussianFactorGraph.cpp:309
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
smallExample.h
Create small example with two poses and one landmark.
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::ConjugateGradientParameters::epsilon_abs
double epsilon_abs
threshold for absolute error decrease
Definition: ConjugateGradientSolver.h:38
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:26