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 testGaussianJunctionTreeB.cpp
14 * @date Jul 8, 2010
15 * @author nikai
16 */
17
18#include <tests/smallExample.h>
19#include <gtsam/sam/BearingRangeFactor.h>
20#include <gtsam/slam/BetweenFactor.h>
21#include <gtsam/nonlinear/PriorFactor.h>
22#include <gtsam/geometry/Pose2.h>
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
24#include <gtsam/nonlinear/Values.h>
25#include <gtsam/linear/GaussianBayesNet.h>
26#include <gtsam/linear/GaussianConditional.h>
27#include <gtsam/linear/GaussianFactor.h>
28#include <gtsam/linear/GaussianFactorGraph.h>
29#include <gtsam/linear/GaussianEliminationTree.h>
30#include <gtsam/linear/GaussianJunctionTree.h>
31#include <gtsam/linear/HessianFactor.h>
32#include <gtsam/linear/JacobianFactor.h>
33#include <gtsam/linear/NoiseModel.h>
34#include <gtsam/linear/VectorValues.h>
35#include <gtsam/symbolic/SymbolicEliminationTree.h>
36#include <gtsam/inference/BayesTree.h>
37#include <gtsam/inference/ClusterTree.h>
38#include <gtsam/inference/Ordering.h>
39#include <gtsam/inference/Symbol.h>
40#include <gtsam/base/Matrix.h>
41#include <gtsam/base/Testable.h>
42
43#include <CppUnitLite/TestHarness.h>
44
45#include <cmath>
46#include <list>
47#include <utility>
48#include <vector>
49
50#include <iostream>
51
52using namespace std;
53using namespace gtsam;
54using namespace example;
55
56using symbol_shorthand::X;
57using symbol_shorthand::L;
58
59/* ************************************************************************* *
60 Bayes tree for smoother with "nested dissection" ordering:
61 C1 x5 x6 x4
62 C2 x3 x2 : x4
63 C3 x1 : x2
64 C4 x7 : x6
65 */
66TEST( GaussianJunctionTreeB, constructor2 ) {
67 // create a graph
68 const auto [nlfg, values] = createNonlinearSmoother(T: 7);
69 SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic();
70
71 // linearize
72 GaussianFactorGraph::shared_ptr fg = nlfg.linearize(linearizationPoint: values);
73
74 const Ordering ordering {X(j: 1), X(j: 3), X(j: 5), X(j: 7), X(j: 2), X(j: 6), X(j: 4)};
75
76 // create an ordering
77 GaussianEliminationTree etree(*fg, ordering);
78 SymbolicEliminationTree stree(*symbolic, ordering);
79 GaussianJunctionTree actual(etree);
80
81 const Ordering o324{X(j: 3), X(j: 2), X(j: 4)}, o56{X(j: 5), X(j: 6)}, o7{X(j: 7)}, o1{X(j: 1)};
82
83 GaussianJunctionTree::sharedNode x324 = actual.roots().front();
84 LONGS_EQUAL(2, x324->children.size());
85 GaussianJunctionTree::sharedNode x1 = x324->children.front();
86 GaussianJunctionTree::sharedNode x56 = x324->children.back();
87 if (x1->children.size() > 0)
88 x1.swap(other&: x56); // makes it work with different tie-breakers
89
90 LONGS_EQUAL(0, x1->children.size());
91 LONGS_EQUAL(1, x56->children.size());
92 GaussianJunctionTree::sharedNode x7 = x56->children[0];
93 LONGS_EQUAL(0, x7->children.size());
94
95 EXPECT(assert_equal(o324, x324->orderedFrontalKeys));
96 EXPECT_LONGS_EQUAL(5, x324->factors.size());
97 EXPECT_LONGS_EQUAL(9, x324->problemSize_);
98
99 EXPECT(assert_equal(o56, x56->orderedFrontalKeys));
100 EXPECT_LONGS_EQUAL(4, x56->factors.size());
101 EXPECT_LONGS_EQUAL(9, x56->problemSize_);
102
103 EXPECT(assert_equal(o7, x7->orderedFrontalKeys));
104 EXPECT_LONGS_EQUAL(2, x7->factors.size());
105 EXPECT_LONGS_EQUAL(4, x7->problemSize_);
106
107 EXPECT(assert_equal(o1, x1->orderedFrontalKeys));
108 EXPECT_LONGS_EQUAL(2, x1->factors.size());
109 EXPECT_LONGS_EQUAL(4, x1->problemSize_);
110}
111
112///* ************************************************************************* */
113TEST(GaussianJunctionTreeB, OptimizeMultiFrontal) {
114 // create a graph
115 const auto fg = createSmoother(T: 7);
116
117 // optimize the graph
118 const VectorValues actual = fg.optimize(function: &EliminateQR);
119
120 // verify
121 VectorValues expected;
122 const Vector v = Vector2(0., 0.);
123 for (int i = 1; i <= 7; i++) expected.emplace(j: X(j: i), args: v);
124 EXPECT(assert_equal(expected, actual));
125}
126
127/* ************************************************************************* */
128TEST(GaussianJunctionTreeB, optimizeMultiFrontal2) {
129 // create a graph
130 const auto nlfg = createNonlinearFactorGraph();
131 const auto noisy = createNoisyValues();
132 const auto fg = *nlfg.linearize(linearizationPoint: noisy);
133
134 // optimize the graph
135 VectorValues actual = fg.optimize(function: &EliminateQR);
136
137 // verify
138 VectorValues expected = createCorrectDelta(); // expected solution
139 EXPECT(assert_equal(expected, actual));
140}
141
142///* ************************************************************************* */
143//TEST(GaussianJunctionTreeB, slamlike) {
144// Values init;
145// NonlinearFactorGraph newfactors;
146// NonlinearFactorGraph fullgraph;
147// SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0));
148// SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1));
149//
150// size_t i = 0;
151//
152// newfactors = NonlinearFactorGraph();
153// newfactors.add(PriorFactor<Pose2>(X(0), Pose2(0.0, 0.0, 0.0), odoNoise));
154// init.insert(X(0), Pose2(0.01, 0.01, 0.01));
155// fullgraph.push_back(newfactors);
156//
157// for( ; i<5; ++i) {
158// newfactors = NonlinearFactorGraph();
159// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
160// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
161// fullgraph.push_back(newfactors);
162// }
163//
164// newfactors = NonlinearFactorGraph();
165// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
166// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise));
167// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise));
168// init.insert(X(i+1), Pose2(1.01, 0.01, 0.01));
169// init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
170// init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
171// fullgraph.push_back(newfactors);
172// ++ i;
173//
174// for( ; i<5; ++i) {
175// newfactors = NonlinearFactorGraph();
176// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
177// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
178// fullgraph.push_back(newfactors);
179// }
180//
181// newfactors = NonlinearFactorGraph();
182// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
183// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise));
184// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise));
185// init.insert(X(i+1), Pose2(6.9, 0.1, 0.01));
186// fullgraph.push_back(newfactors);
187// ++ i;
188//
189// // Compare solutions
190// Ordering ordering = *fullgraph.orderingCOLAMD(init);
191// GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering);
192//
193// GaussianJunctionTree gjt(linearized);
194// VectorValues deltaactual = gjt.optimize(&EliminateQR);
195// Values actual = init.retract(deltaactual, ordering);
196//
197// GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
198// VectorValues delta = optimize(gbn);
199// Values expected = init.retract(delta, ordering);
200//
201// EXPECT(assert_equal(expected, actual));
202//}
203//
204///* ************************************************************************* */
205//TEST(GaussianJunctionTreeB, simpleMarginal) {
206//
207// typedef BayesTree<GaussianConditional> GaussianBayesTree;
208//
209// // Create a simple graph
210// NonlinearFactorGraph fg;
211// fg.add(PriorFactor<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
212// fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0))));
213//
214// Values init;
215// init.insert(X(0), Pose2());
216// init.insert(X(1), Pose2(1.0, 0.0, 0.0));
217//
218// const Ordering ordering{X(1), X(0)};
219//
220// GaussianFactorGraph gfg = *fg.linearize(init, ordering);
221//
222// // Compute marginals with both sequential and multifrontal
223// Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1);
224//
225// Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1);
226//
227// // Compute marginal directly from marginal factor
228// GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1);
229// JacobianFactor::shared_ptr marginalJacobian = std::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
230// Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
231//
232// // Compute marginal directly from BayesTree
233// GaussianBayesTree gbt;
234// gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky));
235// marginalFactor = gbt.marginalFactor(1, EliminateCholesky);
236// marginalJacobian = std::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
237// Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
238//
239// EXPECT(assert_equal(expected, actual1));
240// EXPECT(assert_equal(expected, actual2));
241// EXPECT(assert_equal(expected, actual3));
242//}
243
244/* ************************************************************************* */
245int main() {
246 TestResult tr;
247 return TestRegistry::runAllTests(result&: tr);
248}
249/* ************************************************************************* */
250
251