testSubgraphPreconditioner.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>
19 
22 #include <gtsam/inference/Symbol.h>
26 #include <gtsam/linear/iterative.h>
27 #include <gtsam/slam/dataset.h>
29 
31 
32 #include <fstream>
33 
34 using namespace std;
35 using namespace gtsam;
36 using namespace example;
37 
38 // define keys
39 // Create key for simulated planar graph
40 Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); }
41 
42 /* ************************************************************************* */
44  // Check canonical ordering
46  expected{key(3, 3), key(2, 3), key(1, 3), key(3, 2), key(2, 2),
47  key(1, 2), key(3, 1), key(2, 1), key(1, 1)};
49 }
50 
51 /* ************************************************************************* */
53 static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
54  double total_error = 0.;
55  for (const GaussianFactor::shared_ptr& factor : fg)
56  total_error += factor->error(x);
57  return total_error;
58 }
59 
60 /* ************************************************************************* */
62  // Check planar graph construction
63  const auto [A, xtrue] = planarGraph(3);
64  LONGS_EQUAL(13, A.size());
65  LONGS_EQUAL(9, xtrue.size());
66  DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue
67 
68  // Check that xtrue is optimal
69  GaussianBayesNet R1 = *A.eliminateSequential();
70  VectorValues actual = R1.optimize();
71  EXPECT(assert_equal(xtrue, actual));
72 }
73 
74 /* ************************************************************************* */
76  // Build a planar graph
77  const auto [A, xtrue] = planarGraph(3);
78 
79  // Get the spanning tree and constraints, and check their sizes
80  const auto [T, C] = splitOffPlanarTree(3, A);
81  LONGS_EQUAL(9, T.size());
82  LONGS_EQUAL(4, C.size());
83 
84  // Check that the tree can be solved to give the ground xtrue
85  GaussianBayesNet R1 = *T.eliminateSequential();
86  VectorValues xbar = R1.optimize();
87  EXPECT(assert_equal(xtrue, xbar));
88 }
89 
90 /* ************************************************************************* */
92  // Build a planar graph
93  size_t N = 3;
94  const auto [Ab, xtrue] = planarGraph(N); // A*x-b
95 
96  // Get the spanning tree and remaining graph
97  auto [Ab1, Ab2] = splitOffPlanarTree(N, Ab);
98 
99  // Eliminate the spanning tree to build a prior
100  const Ordering ord = planarOrdering(N);
101  auto Rc1 = *Ab1.eliminateSequential(ord); // R1*x-c1
102  VectorValues xbar = Rc1.optimize(); // xbar = inv(R1)*c1
103 
104  // Create Subgraph-preconditioned system
105  const SubgraphPreconditioner system(Ab2, Rc1, xbar);
106 
107  // Get corresponding matrices for tests. Add dummy factors to Ab2 to make
108  // sure it works with the ordering.
109  Ordering ordering = Rc1.ordering(); // not ord in general!
110  Ab2.add(key(1, 1), Z_2x2, Z_2x1);
111  Ab2.add(key(1, 2), Z_2x2, Z_2x1);
112  Ab2.add(key(1, 3), Z_2x2, Z_2x1);
113  const auto [A, b] = Ab.jacobian(ordering);
114  const auto [A1, b1] = Ab1.jacobian(ordering);
115  const auto [A2, b2] = Ab2.jacobian(ordering);
116  Matrix R1 = Rc1.matrix(ordering).first;
117  Matrix Abar(13 * 2, 9 * 2);
118  Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2);
119  Abar.bottomRows(8) = A2.topRows(8) * R1.inverse();
120 
121  // Helper function to vectorize in correct order, which is the order in which
122  // we eliminated the spanning tree.
123  auto vec = [ordering](const VectorValues& x) { return x.vector(ordering); };
124 
125  // Set up y0 as all zeros
126  const VectorValues y0 = system.zero();
127 
128  // y1 = perturbed y0
129  VectorValues y1 = system.zero();
130  y1[key(3, 3)] = Vector2(1.0, -1.0);
131 
132  // Check backSubstituteTranspose works with R1
133  VectorValues actual = Rc1.backSubstituteTranspose(y1);
134  Vector expected = R1.transpose().inverse() * vec(y1);
135  EXPECT(assert_equal(expected, vec(actual)));
136 
137  // Check corresponding x values
138  // for y = 0, we get xbar:
139  EXPECT(assert_equal(xbar, system.x(y0)));
140  // for non-zero y, answer is x = xbar + inv(R1)*y
141  const Vector expected_x1 = vec(xbar) + R1.inverse() * vec(y1);
142  const VectorValues x1 = system.x(y1);
143  EXPECT(assert_equal(expected_x1, vec(x1)));
144 
145  // Check errors
146  DOUBLES_EQUAL(0, error(Ab, xbar), 1e-9);
147  DOUBLES_EQUAL(0, system.error(y0), 1e-9);
148  DOUBLES_EQUAL(2, error(Ab, x1), 1e-9);
149  DOUBLES_EQUAL(2, system.error(y1), 1e-9);
150 
151  // Check that transposeMultiplyAdd <=> y += alpha * Abar' * e
152  // We check for e1 =[1;0] and e2=[0;1] corresponding to T and C
153  const double alpha = 0.5;
154  Errors e1, e2;
155  for (size_t i = 0; i < 13; i++) {
156  e1.push_back(i < 9 ? Vector2(1, 1) : Vector2(0, 0));
157  e2.push_back(i >= 9 ? Vector2(1, 1) : Vector2(0, 0));
158  }
159  Vector ee1(13 * 2), ee2(13 * 2);
160  ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2);
161  ee2 << Vector::Zero(9 * 2), Vector::Ones(4 * 2);
162 
163  // Check transposeMultiplyAdd for e1
164  VectorValues y = system.zero();
165  system.transposeMultiplyAdd(alpha, e1, y);
166  Vector expected_y = alpha * Abar.transpose() * ee1;
167  EXPECT(assert_equal(expected_y, vec(y)));
168 
169  // Check transposeMultiplyAdd for e2
170  y = system.zero();
171  system.transposeMultiplyAdd(alpha, e2, y);
172  expected_y = alpha * Abar.transpose() * ee2;
173  EXPECT(assert_equal(expected_y, vec(y)));
174 
175  // Test gradient in y
176  auto g = system.gradient(y0);
177  Vector expected_g = Vector::Zero(18);
178  EXPECT(assert_equal(expected_g, vec(g)));
179 }
180 
181 /* ************************************************************************* */
183  // Build a planar graph
184  size_t N = 3;
185  const auto [Ab, xtrue] = planarGraph(N); // A*x-b
186 
187  // Get the spanning tree
188  const auto [Ab1, Ab2] = splitOffPlanarTree(N, Ab);
189 
190  // Eliminate the spanning tree to build a prior
191  GaussianBayesNet Rc1 = *Ab1.eliminateSequential(); // R1*x-c1
192  VectorValues xbar = Rc1.optimize(); // xbar = inv(R1)*c1
193 
194  // Create Subgraph-preconditioned system
195  SubgraphPreconditioner system(Ab2, Rc1, xbar);
196 
197  // Create zero config y0 and perturbed config y1
198  VectorValues y0 = VectorValues::Zero(xbar);
199 
200  VectorValues y1 = y0;
201  y1[key(2, 2)] = Vector2(1.0, -1.0);
202  VectorValues x1 = system.x(y1);
203 
204  // Solve for the remaining constraints using PCG
207  VectorValues, Errors>(system, y1, parameters);
208  EXPECT(assert_equal(y0,actual));
209 
210  // Compare with non preconditioned version:
212  EXPECT(assert_equal(xtrue, actual2, 1e-4));
213 }
214 
215 /* ************************************************************************* */
216 int main() {
217  TestResult tr;
218  return TestRegistry::runAllTests(tr);
219 }
220 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:195
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
simple_graph::b1
Vector2 b1(2, -1)
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple_graph::b2
Vector2 b2(4, -5)
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::SubgraphPreconditioner::error
double error(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:109
error
static double error(const GaussianFactorGraph &fg, const VectorValues &x)
Definition: testSubgraphPreconditioner.cpp:53
gtsam::SubgraphPreconditioner
Definition: SubgraphPreconditioner.h:54
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Ordering.h
Variable ordering for the elimination algorithm.
X
#define X
Definition: icosphere.cpp:20
gtsam::example::splitOffPlanarTree
std::pair< GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N, const GaussianFactorGraph &original)
Definition: smallExample.h:677
gtsam::ConjugateGradientParameters
Definition: ConjugateGradientSolver.h:29
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
y0
double y0(double x)
Definition: j0.c:220
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
main
int main()
Definition: testSubgraphPreconditioner.cpp:216
example
Definition: testOrdering.cpp:28
gtsam::SubgraphPreconditioner::x
VectorValues x(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:104
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::conjugateGradientDescent
Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Definition: iterative.cpp:45
gtsam::SubgraphPreconditioner::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:168
GaussianEliminationTree.h
y1
double y1(double x)
Definition: j1.c:199
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::conjugateGradients
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Definition: iterative-inl.h:124
dataset.h
utility functions for loading datasets
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::SubgraphPreconditioner::gradient
VectorValues gradient(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:118
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::GaussianBayesNet::optimize
VectorValues optimize() const
Definition: GaussianBayesNet.cpp:44
Symbol.h
iterative.h
Iterative methods, implementation.
gtsam::example::planarOrdering
Ordering planarOrdering(size_t N)
Definition: smallExample.h:668
Eigen::Triplet< double >
gtsam::FastList< Vector >
SymbolicFactorGraph.h
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
TEST
TEST(SubgraphPreconditioner, planarOrdering)
Definition: testSubgraphPreconditioner.cpp:43
gtsam::example::planarGraph
std::pair< GaussianFactorGraph, VectorValues > planarGraph(size_t N)
Definition: smallExample.h:626
key
Symbol key(int x, int y)
Definition: testSubgraphPreconditioner.cpp:40
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: chartTesting.h:28
std
Definition: BFloat16.h:88
SubgraphPreconditioner.h
A1
static const double A1[]
Definition: expn.h:6
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
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
N
#define N
Definition: igam.h:9
smallExample.h
Create small example with two poses and one landmark.
gtsam::Ordering
Definition: inference/Ordering.h:33
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::SubgraphPreconditioner::zero
VectorValues zero() const
Definition: SubgraphPreconditioner.h:104


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:10:34