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
12/**
13 * @file testSubgraphSolver.cpp
14 * @brief Unit tests for SubgraphSolver
15 * @author Yong-Dian Jian
16 **/
17
18#include <gtsam/linear/SubgraphSolver.h>
19
20#include <tests/smallExample.h>
21#include <gtsam/linear/GaussianBayesNet.h>
22#include <gtsam/linear/iterative.h>
23#include <gtsam/linear/GaussianFactorGraph.h>
24#include <gtsam/linear/SubgraphBuilder.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/inference/Ordering.h>
27#include <gtsam/base/numericalDerivative.h>
28
29#include <CppUnitLite/TestHarness.h>
30
31using namespace std;
32using namespace gtsam;
33
34static size_t N = 3;
35static SubgraphSolverParameters kParameters;
36static auto kOrdering = example::planarOrdering(N);
37
38/* ************************************************************************* */
39/** unnormalized error */
40static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
41 double total_error = 0.;
42 for(const GaussianFactor::shared_ptr& factor: fg)
43 total_error += factor->error(c: x);
44 return total_error;
45}
46
47/* ************************************************************************* */
48TEST( SubgraphSolver, Parameters )
49{
50 LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity());
51 LONGS_EQUAL(500, kParameters.maxIterations);
52}
53
54/* ************************************************************************* */
55TEST( SubgraphSolver, splitFactorGraph )
56{
57 // Build a planar graph
58 const auto [Ab, xtrue] = example::planarGraph(N); // A*x-b
59
60 SubgraphBuilderParameters params;
61 params.augmentationFactor = 0.0;
62 SubgraphBuilder builder(params);
63 auto subgraph = builder(Ab);
64 EXPECT_LONGS_EQUAL(9, subgraph.size());
65
66 const auto [Ab1, Ab2] = splitFactorGraph(factorGraph: Ab, subgraph);
67 EXPECT_LONGS_EQUAL(9, Ab1.size());
68 EXPECT_LONGS_EQUAL(13, Ab2.size());
69}
70
71/* ************************************************************************* */
72TEST( SubgraphSolver, constructor1 )
73{
74 // Build a planar graph
75 const auto [Ab, xtrue] = example::planarGraph(N); // A*x-b
76
77 // The first constructor just takes a factor graph (and kParameters)
78 // and it will split the graph into A1 and A2, where A1 is a spanning tree
79 SubgraphSolver solver(Ab, kParameters, kOrdering);
80 VectorValues optimized = solver.optimize(); // does PCG optimization
81 DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
82}
83
84/* ************************************************************************* */
85TEST( SubgraphSolver, constructor2 )
86{
87 // Build a planar graph
88 size_t N = 3;
89 const auto [Ab, xtrue] = example::planarGraph(N); // A*x-b
90
91 // Get the spanning tree, A1*x-b1 and A2*x-b2
92 const auto [Ab1, Ab2] = example::splitOffPlanarTree(N, original: Ab);
93
94 // The second constructor takes two factor graphs, so the caller can specify
95 // the preconditioner (Ab1) and the constraints that are left out (Ab2)
96 SubgraphSolver solver(Ab1, Ab2, kParameters, kOrdering);
97 VectorValues optimized = solver.optimize();
98 DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
99}
100
101/* ************************************************************************* */
102TEST( SubgraphSolver, constructor3 )
103{
104 // Build a planar graph
105 size_t N = 3;
106 const auto [Ab, xtrue] = example::planarGraph(N); // A*x-b
107
108 // Get the spanning tree and corresponding kOrdering
109 // A1*x-b1 and A2*x-b2
110 const auto [Ab1, Ab2] = example::splitOffPlanarTree(N, original: Ab);
111
112 // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
113 auto Rc1 = *Ab1.eliminateSequential();
114
115 // The third constructor allows the caller to pass an already solved preconditioner Rc1_
116 // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
117 SubgraphSolver solver(Rc1, Ab2, kParameters);
118 VectorValues optimized = solver.optimize();
119 DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
120}
121
122/* ************************************************************************* */
123int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
124/* ************************************************************************* */
125