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 testGaussianISAM.cpp
14 * @brief Unit tests for GaussianISAM
15 * @author Michael Kaess
16 */
17
18#include <tests/smallExample.h>
19#include <gtsam/inference/Symbol.h>
20#include <gtsam/linear/GaussianBayesTree.h>
21#include <gtsam/linear/GaussianBayesNet.h>
22#include <gtsam/linear/GaussianConditional.h>
23#include <gtsam/linear/GaussianDensity.h>
24#include <gtsam/linear/HessianFactor.h>
25#include <gtsam/geometry/Rot2.h>
26
27#include <CppUnitLite/TestHarness.h>
28
29using namespace std;
30using namespace gtsam;
31using namespace example;
32
33using symbol_shorthand::X;
34using symbol_shorthand::L;
35
36/* ************************************************************************* */
37// Some numbers that should be consistent among all smoother tests
38
39static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 =
40 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;
41
42static const double tol = 1e-4;
43
44/* ************************************************************************* *
45 Bayes tree for smoother with "natural" ordering:
46C1 x6 x7
47C2 x5 : x6
48C3 x4 : x5
49C4 x3 : x4
50C5 x2 : x3
51C6 x1 : x2
52**************************************************************************** */
53TEST( GaussianBayesTree, linear_smoother_shortcuts )
54{
55 // Create smoother with 7 nodes
56 GaussianFactorGraph smoother = createSmoother(T: 7);
57
58 GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal();
59
60 // Create the Bayes tree
61 LONGS_EQUAL(6, (long)bayesTree.size());
62
63 // Check the conditional P(Root|Root)
64 GaussianBayesNet empty;
65 GaussianBayesTree::sharedClique R = bayesTree.roots().front();
66 GaussianBayesNet actual1 = R->shortcut(root: R);
67 EXPECT(assert_equal(empty,actual1,tol));
68
69 // Check the conditional P(C2|Root)
70 GaussianBayesTree::sharedClique C2 = bayesTree[X(j: 5)];
71 GaussianBayesNet actual2 = C2->shortcut(root: R);
72 EXPECT(assert_equal(empty,actual2,tol));
73
74 // Check the conditional P(C3|Root)
75 double sigma3 = 0.61808;
76 Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
77 GaussianBayesNet expected3;
78 expected3.emplace_shared<GaussianConditional>(args: X(j: 5), args: Z_2x1, args: I_2x2/sigma3, args: X(j: 6), args: A56/sigma3);
79 GaussianBayesTree::sharedClique C3 = bayesTree[X(j: 4)];
80 GaussianBayesNet actual3 = C3->shortcut(root: R);
81 EXPECT(assert_equal(expected3,actual3,tol));
82
83 // Check the conditional P(C4|Root)
84 double sigma4 = 0.661968;
85 Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
86 GaussianBayesNet expected4;
87 expected4.emplace_shared<GaussianConditional>(args: X(j: 4), args: Z_2x1, args: I_2x2/sigma4, args: X(j: 6), args: A46/sigma4);
88 GaussianBayesTree::sharedClique C4 = bayesTree[X(j: 3)];
89 GaussianBayesNet actual4 = C4->shortcut(root: R);
90 EXPECT(assert_equal(expected4,actual4,tol));
91}
92
93/* ************************************************************************* *
94 Bayes tree for smoother with "nested dissection" ordering:
95
96 Node[x1] P(x1 | x2)
97 Node[x3] P(x3 | x2 x4)
98 Node[x5] P(x5 | x4 x6)
99 Node[x7] P(x7 | x6)
100 Node[x2] P(x2 | x4)
101 Node[x6] P(x6 | x4)
102 Node[x4] P(x4)
103
104 becomes
105
106 C1 x5 x6 x4
107 C2 x3 x2 : x4
108 C3 x1 : x2
109 C4 x7 : x6
110
111************************************************************************* */
112TEST(GaussianBayesTree, balanced_smoother_marginals) {
113 // Create smoother with 7 nodes
114 GaussianFactorGraph smoother = createSmoother(T: 7);
115
116 // Create the Bayes tree
117 const Ordering ordering{X(j: 1), X(j: 3), X(j: 5), X(j: 7), X(j: 2), X(j: 6), X(j: 4)};
118 GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
119
120 VectorValues actualSolution = bayesTree.optimize();
121 VectorValues expectedSolution = VectorValues::Zero(other: actualSolution);
122 EXPECT(assert_equal(expectedSolution, actualSolution, tol));
123
124 LONGS_EQUAL(4, bayesTree.size());
125
126 double tol = 1e-5;
127
128 // Check marginal on x1
129 JacobianFactor actual1 = *bayesTree.marginalFactor(j: X(j: 1));
130 Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
131 auto m = bayesTree.marginalFactor(j: X(j: 1), function: EliminateCholesky);
132 Matrix actualCovarianceX1 = m->information().inverse();
133 EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
134
135 // Check marginal on x2
136 double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically
137 JacobianFactor actual2 = *bayesTree.marginalFactor(j: X(j: 2));
138 Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2);
139 EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol));
140
141 // Check marginal on x3
142 JacobianFactor actual3 = *bayesTree.marginalFactor(j: X(j: 3));
143 Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3);
144 EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol));
145
146 // Check marginal on x4
147 JacobianFactor actual4 = *bayesTree.marginalFactor(j: X(j: 4));
148 Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4);
149 EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol));
150
151 // Check marginal on x7 (should be equal to x1)
152 JacobianFactor actual7 = *bayesTree.marginalFactor(j: X(j: 7));
153 Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
154 EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
155}
156
157/* ************************************************************************* */
158TEST( GaussianBayesTree, balanced_smoother_shortcuts )
159{
160 // Create smoother with 7 nodes
161 GaussianFactorGraph smoother = createSmoother(T: 7);
162
163 // Create the Bayes tree
164 const Ordering ordering{X(j: 1), X(j: 3), X(j: 5), X(j: 7), X(j: 2), X(j: 6), X(j: 4)};
165 GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
166
167 // Check the conditional P(Root|Root)
168 GaussianBayesNet empty;
169 GaussianBayesTree::sharedClique R = bayesTree.roots().front();
170 GaussianBayesNet actual1 = R->shortcut(root: R);
171 EXPECT(assert_equal(empty,actual1,tol));
172
173 // Check the conditional P(C2|Root)
174 GaussianBayesTree::sharedClique C2 = bayesTree[X(j: 3)];
175 GaussianBayesNet actual2 = C2->shortcut(root: R);
176 EXPECT(assert_equal(empty,actual2,tol));
177
178 // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
179 /** TODO: Note for multifrontal conditional:
180 * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
181 * We don't know yet how to take it out.
182 */
183// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
184// p_x2_x4->print("Conditional p_x2_x4: ");
185// GaussianBayesNet expected3(p_x2_x4);
186// GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
187// GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
188// EXPECT(assert_equal(expected3,actual3,tol));
189}
190
191///* ************************************************************************* */
192//TEST( BayesTree, balanced_smoother_clique_marginals )
193//{
194// // Create smoother with 7 nodes
195// const Ordering ordering{X(1),X(3),X(5),X(7),X(2),X(6),X(4)};
196// GaussianFactorGraph smoother = createSmoother(7, ordering).first;
197//
198// // Create the Bayes tree
199// GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
200// GaussianISAM bayesTree(chordalBayesNet);
201//
202// // Check the clique marginal P(C3)
203// double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
204// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt);
205// push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
206// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
207// GaussianFactorGraph marginal = C3->marginal(R);
208// GaussianVariableIndex varIndex(marginal);
209// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
210// Permutation toFrontInverse(*toFront.inverse());
211// varIndex.permute(toFront);
212// for(const GaussianFactor::shared_ptr& factor: marginal) {
213// factor->permuteWithInverse(toFrontInverse); }
214// GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
215// actual.permuteWithInverse(toFront);
216// EXPECT(assert_equal(expected,actual,tol));
217//}
218
219/* ************************************************************************* */
220TEST( GaussianBayesTree, balanced_smoother_joint )
221{
222 // Create smoother with 7 nodes
223 const Ordering ordering{X(j: 1), X(j: 3), X(j: 5), X(j: 7), X(j: 2), X(j: 6), X(j: 4)};
224 GaussianFactorGraph smoother = createSmoother(T: 7);
225
226 // Create the Bayes tree, expected to look like:
227 // x5 x6 x4
228 // x3 x2 : x4
229 // x1 : x2
230 // x7 : x6
231 GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
232
233 // Conditional density elements reused by both tests
234 const Matrix I = I_2x2, A = -0.00429185*I;
235
236 // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
237 GaussianBayesNet expected1;
238 // Why does the sign get flipped on the prior?
239 expected1.emplace_shared<GaussianConditional>(args: X(j: 1), args: Z_2x1, args: I/sigmax7, args: X(j: 7), args: A/sigmax7);
240 expected1.emplace_shared<GaussianConditional>(args: X(j: 7), args: Z_2x1, args: -1*I/sigmax7);
241 GaussianBayesNet actual1 = *bayesTree.jointBayesNet(j1: X(j: 1),j2: X(j: 7));
242 EXPECT(assert_equal(expected1, actual1, tol));
243
244 // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
245 // GaussianBayesNet expected2;
246 // GaussianConditional::shared_ptr
247 // parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2)));
248 // expected2.push_front(parent2);
249 // push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma);
250 // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
251 // EXPECT(assert_equal(expected2,actual2,tol));
252
253 // Check the joint density P(x1,x4), i.e. with a root variable
254 double sig14 = 0.784465;
255 Matrix A14 = -0.0769231*I;
256 GaussianBayesNet expected3;
257 expected3.emplace_shared<GaussianConditional>(args: X(j: 1), args: Z_2x1, args: I/sig14, args: X(j: 4), args: A14/sig14);
258 expected3.emplace_shared<GaussianConditional>(args: X(j: 4), args: Z_2x1, args: I/sigmax4);
259 GaussianBayesNet actual3 = *bayesTree.jointBayesNet(j1: X(j: 1),j2: X(j: 4));
260 EXPECT(assert_equal(expected3,actual3,tol));
261
262 // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
263 // GaussianBayesNet expected4;
264 // GaussianConditional::shared_ptr
265 // parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2)));
266 // expected4.push_front(parent4);
267 // double sig41 = 0.668096;
268 // Matrix A41 = -0.055794*I;
269 // push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma);
270 // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
271 // EXPECT(assert_equal(expected4,actual4,tol));
272}
273
274/* ************************************************************************* */
275TEST(GaussianBayesTree, shortcut_overlapping_separator)
276{
277 // Test computing shortcuts when the separator overlaps. This previously
278 // would have highlighted a problem where information was duplicated.
279
280 // Create factor graph:
281 // f(1,2,5)
282 // f(3,4,5)
283 // f(5,6)
284 // f(6,7)
285 GaussianFactorGraph fg;
286 noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(dim: 1);
287 fg.add(key1: 1, A1: (Matrix(1, 1) << 1.0).finished(), key2: 3, A2: (Matrix(1, 1) << 2.0).finished(), key3: 5, A3: (Matrix(1, 1) << 3.0).finished(), b: (Vector(1) << 4.0).finished(), model);
288 fg.add(key1: 1, A1: (Matrix(1, 1) << 5.0).finished(), b: (Vector(1) << 6.0).finished(), model);
289 fg.add(key1: 2, A1: (Matrix(1, 1) << 7.0).finished(), key2: 4, A2: (Matrix(1, 1) << 8.0).finished(), key3: 5, A3: (Matrix(1, 1) << 9.0).finished(), b: (Vector(1) << 10.0).finished(), model);
290 fg.add(key1: 2, A1: (Matrix(1, 1) << 11.0).finished(), b: (Vector(1) << 12.0).finished(), model);
291 fg.add(key1: 5, A1: (Matrix(1, 1) << 13.0).finished(), key2: 6, A2: (Matrix(1, 1) << 14.0).finished(), b: (Vector(1) << 15.0).finished(), model);
292 fg.add(key1: 6, A1: (Matrix(1, 1) << 17.0).finished(), key2: 7, A2: (Matrix(1, 1) << 18.0).finished(), b: (Vector(1) << 19.0).finished(), model);
293 fg.add(key1: 7, A1: (Matrix(1, 1) << 20.0).finished(), b: (Vector(1) << 21.0).finished(), model);
294
295 // Eliminate into BayesTree
296 // c(6,7)
297 // c(5|6)
298 // c(1,2|5)
299 // c(3,4|5)
300 Ordering ordering(fg.keys());
301 GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted.
302
303 GaussianFactorGraph joint = *bt.joint(j1: 1,j2: 2, function: EliminateQR);
304
305 Matrix expectedJointJ = (Matrix(2,3) <<
306 5, 0, 6,
307 0, -11, -12
308 ).finished();
309
310 Matrix actualJointJ = joint.augmentedJacobian();
311
312 // PR 315: sign of rows in joint are immaterial
313 if (signbit(x: expectedJointJ(0, 2)) != signbit(x: actualJointJ(0, 2)))
314 expectedJointJ.row(i: 0) = -expectedJointJ.row(i: 0);
315
316 if (signbit(x: expectedJointJ(1, 2)) != signbit(x: actualJointJ(1, 2)))
317 expectedJointJ.row(i: 1) = -expectedJointJ.row(i: 1);
318
319 EXPECT(assert_equal(expectedJointJ, actualJointJ));
320}
321
322/* ************************************************************************* */
323int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
324/* ************************************************************************* */
325