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 <CppUnitLite/TestHarness.h>
19
20#include <tests/smallExample.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/linear/GaussianISAM.h>
23#include <gtsam/inference/Ordering.h>
24
25using namespace std;
26using namespace gtsam;
27using namespace example;
28
29using symbol_shorthand::X;
30using symbol_shorthand::L;
31
32/* ************************************************************************* */
33TEST( ISAM, iSAM_smoother )
34{
35 Ordering ordering;
36 for (int t = 1; t <= 7; t++) ordering.push_back(x: X(j: t));
37
38 // Create smoother with 7 nodes
39 GaussianFactorGraph smoother = createSmoother(T: 7);
40
41 // run iSAM for every factor
42 GaussianISAM actual;
43 for(std::shared_ptr<GaussianFactor> factor: smoother) {
44 GaussianFactorGraph factorGraph;
45 factorGraph.push_back(factor);
46 actual.update(newFactors: factorGraph);
47 }
48
49 // Create expected Bayes Tree by solving smoother with "natural" ordering
50 GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
51
52 // Verify sigmas in the bayes tree
53 for (const auto& [key, clique] : expected.nodes()) {
54 GaussianConditional::shared_ptr conditional = clique->conditional();
55 EXPECT(!conditional->get_model());
56 }
57
58 // Check whether BayesTree is correct
59 EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));
60
61 // obtain solution
62 VectorValues e; // expected solution
63 for (int t = 1; t <= 7; t++) e.insert(j: X(j: t), value: Vector::Zero(size: 2));
64 VectorValues optimized = actual.optimize(); // actual solution
65 EXPECT(assert_equal(e, optimized));
66}
67
68/* ************************************************************************* */
69int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
70/* ************************************************************************* */
71