testGaussianBayesTreeB.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 #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 
29 using namespace std;
30 using namespace gtsam;
31 using namespace example;
32 
33 using symbol_shorthand::X;
34 using symbol_shorthand::L;
35 
36 /* ************************************************************************* */
37 // Some numbers that should be consistent among all smoother tests
38 
39 static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 =
40  0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;
41 
42 static const double tol = 1e-4;
43 
44 /* ************************************************************************* *
45  Bayes tree for smoother with "natural" ordering:
46 C1 x6 x7
47 C2 x5 : x6
48 C3 x4 : x5
49 C4 x3 : x4
50 C5 x2 : x3
51 C6 x1 : x2
52 **************************************************************************** */
53 TEST( GaussianBayesTree, linear_smoother_shortcuts )
54 {
55  // Create smoother with 7 nodes
56  GaussianFactorGraph smoother = createSmoother(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(R);
67  EXPECT(assert_equal(empty,actual1,tol));
68 
69  // Check the conditional P(C2|Root)
70  GaussianBayesTree::sharedClique C2 = bayesTree[X(5)];
71  GaussianBayesNet actual2 = C2->shortcut(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>(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
79  GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
80  GaussianBayesNet actual3 = C3->shortcut(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>(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
88  GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
89  GaussianBayesNet actual4 = C4->shortcut(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 ************************************************************************* */
112 TEST(GaussianBayesTree, balanced_smoother_marginals) {
113  // Create smoother with 7 nodes
114  GaussianFactorGraph smoother = createSmoother(7);
115 
116  // Create the Bayes tree
117  const Ordering ordering{X(1), X(3), X(5), X(7), X(2), X(6), X(4)};
118  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
119 
120  VectorValues actualSolution = bayesTree.optimize();
121  VectorValues expectedSolution = VectorValues::Zero(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(X(1));
130  Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
131  auto m = bayesTree.marginalFactor(X(1), 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(X(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(X(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(X(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(X(7));
153  Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
154  EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
155 }
156 
157 /* ************************************************************************* */
158 TEST( GaussianBayesTree, balanced_smoother_shortcuts )
159 {
160  // Create smoother with 7 nodes
161  GaussianFactorGraph smoother = createSmoother(7);
162 
163  // Create the Bayes tree
164  const Ordering ordering{X(1), X(3), X(5), X(7), X(2), X(6), X(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(R);
171  EXPECT(assert_equal(empty,actual1,tol));
172 
173  // Check the conditional P(C2|Root)
174  GaussianBayesTree::sharedClique C2 = bayesTree[X(3)];
175  GaussianBayesNet actual2 = C2->shortcut(R);
176  EXPECT(assert_equal(empty,actual2,tol));
177 
178  // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
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 
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 /* ************************************************************************* */
220 TEST( GaussianBayesTree, balanced_smoother_joint )
221 {
222  // Create smoother with 7 nodes
223  const Ordering ordering{X(1), X(3), X(5), X(7), X(2), X(6), X(4)};
224  GaussianFactorGraph smoother = createSmoother(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>(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7);
240  expected1.emplace_shared<GaussianConditional>(X(7), Z_2x1, -1*I/sigmax7);
241  GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(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>(X(1), Z_2x1, I/sig14, X(4), A14/sig14);
258  expected3.emplace_shared<GaussianConditional>(X(4), Z_2x1, I/sigmax4);
259  GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(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 /* ************************************************************************* */
275 TEST(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(1);
287  fg.add(1, (Matrix(1, 1) << 1.0).finished(), 3, (Matrix(1, 1) << 2.0).finished(), 5, (Matrix(1, 1) << 3.0).finished(), (Vector(1) << 4.0).finished(), model);
288  fg.add(1, (Matrix(1, 1) << 5.0).finished(), (Vector(1) << 6.0).finished(), model);
289  fg.add(2, (Matrix(1, 1) << 7.0).finished(), 4, (Matrix(1, 1) << 8.0).finished(), 5, (Matrix(1, 1) << 9.0).finished(), (Vector(1) << 10.0).finished(), model);
290  fg.add(2, (Matrix(1, 1) << 11.0).finished(), (Vector(1) << 12.0).finished(), model);
291  fg.add(5, (Matrix(1, 1) << 13.0).finished(), 6, (Matrix(1, 1) << 14.0).finished(), (Vector(1) << 15.0).finished(), model);
292  fg.add(6, (Matrix(1, 1) << 17.0).finished(), 7, (Matrix(1, 1) << 18.0).finished(), (Vector(1) << 19.0).finished(), model);
293  fg.add(7, (Matrix(1, 1) << 20.0).finished(), (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(1,2, 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(expectedJointJ(0, 2)) != signbit(actualJointJ(0, 2)))
314  expectedJointJ.row(0) = -expectedJointJ.row(0);
315 
316  if (signbit(expectedJointJ(1, 2)) != signbit(actualJointJ(1, 2)))
317  expectedJointJ.row(1) = -expectedJointJ.row(1);
318 
319  EXPECT(assert_equal(expectedJointJ, actualJointJ));
320 }
321 
322 /* ************************************************************************* */
323 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
324 /* ************************************************************************* */
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
I
#define I
Definition: main.h:112
sigmax7
static double sigmax7
Definition: testGaussianBayesTreeB.cpp:40
gtsam::BayesTree::size
size_t size() const
Definition: BayesTree-inst.h:133
gtsam::example::createSmoother
GaussianFactorGraph createSmoother(int T)
Definition: smallExample.h:465
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
R
Rot2 R(Rot2::fromAngle(0.1))
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::JacobianFactor::information
Matrix information() const override
Definition: JacobianFactor.cpp:505
std
Definition: BFloat16.h:88
sigmax1
static double sigmax1
Definition: testGaussianBayesTreeB.cpp:39
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
sigmax3
static double sigmax3
Definition: testGaussianBayesTreeB.cpp:39
ordering
static enum @1107 ordering
TEST
TEST(GaussianBayesTree, linear_smoother_shortcuts)
Definition: testGaussianBayesTreeB.cpp:53
GaussianDensity.h
A Gaussian Density.
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
gtsam::GaussianBayesTree::optimize
VectorValues optimize() const
Definition: GaussianBayesTree.cpp:67
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::EliminateCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:517
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778
sigmax4
static double sigmax4
Definition: testGaussianBayesTreeB.cpp:39
gtsam::GaussianFactorGraph::augmentedJacobian
Matrix augmentedJacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:217
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam
traits
Definition: chartTesting.h:28
tol
static const double tol
Definition: testGaussianBayesTreeB.cpp:42
simple_graph::sigma3
double sigma3
Definition: testJacobianFactor.cpp:197
gtsam::BayesTree::roots
const Roots & roots() const
Definition: BayesTree.h:152
gtsam::EliminateableFactorGraph::eliminateMultifrontal
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:88
smallExample.h
Create small example with two poses and one landmark.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::BayesTree::jointBayesNet
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: BayesTree-inst.h:341
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
X
#define X
Definition: icosphere.cpp:20
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::BayesTree::marginalFactor
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: BayesTree-inst.h:309
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
Rot2.h
2D rotation

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:08