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>
25 #include <gtsam/geometry/Rot2.h>
26 
28 
29 #include <boost/assign/std/list.hpp> // for operator +=
30 using namespace boost::assign;
31 
32 using namespace std;
33 using namespace gtsam;
34 using namespace example;
35 
38 
39 /* ************************************************************************* */
40 // Some numbers that should be consistent among all smoother tests
41 
42 static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 =
43  0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;
44 
45 static const double tol = 1e-4;
46 
47 /* ************************************************************************* *
48  Bayes tree for smoother with "natural" ordering:
49 C1 x6 x7
50 C2 x5 : x6
51 C3 x4 : x5
52 C4 x3 : x4
53 C5 x2 : x3
54 C6 x1 : x2
55 **************************************************************************** */
56 TEST( GaussianBayesTree, linear_smoother_shortcuts )
57 {
58  // Create smoother with 7 nodes
59  GaussianFactorGraph smoother = createSmoother(7);
60 
61  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal();
62 
63  // Create the Bayes tree
64  LONGS_EQUAL(6, (long)bayesTree.size());
65 
66  // Check the conditional P(Root|Root)
68  GaussianBayesTree::sharedClique R = bayesTree.roots().front();
69  GaussianBayesNet actual1 = R->shortcut(R);
70  EXPECT(assert_equal(empty,actual1,tol));
71 
72  // Check the conditional P(C2|Root)
73  GaussianBayesTree::sharedClique C2 = bayesTree[X(5)];
74  GaussianBayesNet actual2 = C2->shortcut(R);
75  EXPECT(assert_equal(empty,actual2,tol));
76 
77  // Check the conditional P(C3|Root)
78  double sigma3 = 0.61808;
79  Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
80  GaussianBayesNet expected3;
81  expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
82  GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
83  GaussianBayesNet actual3 = C3->shortcut(R);
84  EXPECT(assert_equal(expected3,actual3,tol));
85 
86  // Check the conditional P(C4|Root)
87  double sigma4 = 0.661968;
88  Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
89  GaussianBayesNet expected4;
90  expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
91  GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
92  GaussianBayesNet actual4 = C4->shortcut(R);
93  EXPECT(assert_equal(expected4,actual4,tol));
94 }
95 
96 /* ************************************************************************* *
97  Bayes tree for smoother with "nested dissection" ordering:
98 
99  Node[x1] P(x1 | x2)
100  Node[x3] P(x3 | x2 x4)
101  Node[x5] P(x5 | x4 x6)
102  Node[x7] P(x7 | x6)
103  Node[x2] P(x2 | x4)
104  Node[x6] P(x6 | x4)
105  Node[x4] P(x4)
106 
107  becomes
108 
109  C1 x5 x6 x4
110  C2 x3 x2 : x4
111  C3 x1 : x2
112  C4 x7 : x6
113 
114 ************************************************************************* */
115 TEST( GaussianBayesTree, balanced_smoother_marginals )
116 {
117  // Create smoother with 7 nodes
118  GaussianFactorGraph smoother = createSmoother(7);
119 
120  // Create the Bayes tree
122  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
123  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
124 
125  VectorValues actualSolution = bayesTree.optimize();
126  VectorValues expectedSolution = VectorValues::Zero(actualSolution);
127  EXPECT(assert_equal(expectedSolution,actualSolution,tol));
128 
129  LONGS_EQUAL(4, (long)bayesTree.size());
130 
131  double tol=1e-5;
132 
133  // Check marginal on x1
134  JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
135  JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
136  Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
137  Matrix actualCovarianceX1;
139  actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
140  EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
141  EXPECT(assert_equal(expected1,actual1,tol));
142 
143  // Check marginal on x2
144  double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
145  JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
146  JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
147  EXPECT(assert_equal(expected2,actual2,tol));
148 
149  // Check marginal on x3
150  JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
151  JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
152  EXPECT(assert_equal(expected3,actual3,tol));
153 
154  // Check marginal on x4
155  JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
156  JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
157  EXPECT(assert_equal(expected4,actual4,tol));
158 
159  // Check marginal on x7 (should be equal to x1)
160  JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
161  JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
162  EXPECT(assert_equal(expected7,actual7,tol));
163 }
164 
165 /* ************************************************************************* */
166 TEST( GaussianBayesTree, balanced_smoother_shortcuts )
167 {
168  // Create smoother with 7 nodes
169  GaussianFactorGraph smoother = createSmoother(7);
170 
171  // Create the Bayes tree
173  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
174  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
175 
176  // Check the conditional P(Root|Root)
178  GaussianBayesTree::sharedClique R = bayesTree.roots().front();
179  GaussianBayesNet actual1 = R->shortcut(R);
180  EXPECT(assert_equal(empty,actual1,tol));
181 
182  // Check the conditional P(C2|Root)
183  GaussianBayesTree::sharedClique C2 = bayesTree[X(3)];
184  GaussianBayesNet actual2 = C2->shortcut(R);
185  EXPECT(assert_equal(empty,actual2,tol));
186 
187  // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
192 // GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
193 // p_x2_x4->print("Conditional p_x2_x4: ");
194 // GaussianBayesNet expected3(p_x2_x4);
195 // GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
196 // GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
197 // EXPECT(assert_equal(expected3,actual3,tol));
198 }
199 
201 //TEST( BayesTree, balanced_smoother_clique_marginals )
202 //{
203 // // Create smoother with 7 nodes
204 // Ordering ordering;
205 // ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
206 // GaussianFactorGraph smoother = createSmoother(7, ordering).first;
207 //
208 // // Create the Bayes tree
209 // GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
210 // GaussianISAM bayesTree(chordalBayesNet);
211 //
212 // // Check the clique marginal P(C3)
213 // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
214 // GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt);
215 // push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
216 // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
217 // GaussianFactorGraph marginal = C3->marginal(R);
218 // GaussianVariableIndex varIndex(marginal);
219 // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
220 // Permutation toFrontInverse(*toFront.inverse());
221 // varIndex.permute(toFront);
222 // for(const GaussianFactor::shared_ptr& factor: marginal) {
223 // factor->permuteWithInverse(toFrontInverse); }
224 // GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
225 // actual.permuteWithInverse(toFront);
226 // EXPECT(assert_equal(expected,actual,tol));
227 //}
228 
229 /* ************************************************************************* */
230 TEST( GaussianBayesTree, balanced_smoother_joint )
231 {
232  // Create smoother with 7 nodes
234  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
235  GaussianFactorGraph smoother = createSmoother(7);
236 
237  // Create the Bayes tree, expected to look like:
238  // x5 x6 x4
239  // x3 x2 : x4
240  // x1 : x2
241  // x7 : x6
242  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
243 
244  // Conditional density elements reused by both tests
245  const Matrix I = I_2x2, A = -0.00429185*I;
246 
247  // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
248  GaussianBayesNet expected1 = list_of
249  // Why does the sign get flipped on the prior?
250  (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7))
251  (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7));
252  GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7));
253  EXPECT(assert_equal(expected1, actual1, tol));
254 
255  // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
256  // GaussianBayesNet expected2;
257  // GaussianConditional::shared_ptr
258  // parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2)));
259  // expected2.push_front(parent2);
260  // push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma);
261  // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
262  // EXPECT(assert_equal(expected2,actual2,tol));
263 
264  // Check the joint density P(x1,x4), i.e. with a root variable
265  double sig14 = 0.784465;
266  Matrix A14 = -0.0769231*I;
267  GaussianBayesNet expected3 = list_of
268  (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14))
270  GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
271  EXPECT(assert_equal(expected3,actual3,tol));
272 
273  // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
274  // GaussianBayesNet expected4;
275  // GaussianConditional::shared_ptr
276  // parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2)));
277  // expected4.push_front(parent4);
278  // double sig41 = 0.668096;
279  // Matrix A41 = -0.055794*I;
280  // push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma);
281  // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
282  // EXPECT(assert_equal(expected4,actual4,tol));
283 }
284 
285 /* ************************************************************************* */
286 TEST(GaussianBayesTree, shortcut_overlapping_separator)
287 {
288  // Test computing shortcuts when the separator overlaps. This previously
289  // would have highlighted a problem where information was duplicated.
290 
291  // Create factor graph:
292  // f(1,2,5)
293  // f(3,4,5)
294  // f(5,6)
295  // f(6,7)
297  noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
298  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);
299  fg.add(1, (Matrix(1, 1) << 5.0).finished(), (Vector(1) << 6.0).finished(), model);
300  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);
301  fg.add(2, (Matrix(1, 1) << 11.0).finished(), (Vector(1) << 12.0).finished(), model);
302  fg.add(5, (Matrix(1, 1) << 13.0).finished(), 6, (Matrix(1, 1) << 14.0).finished(), (Vector(1) << 15.0).finished(), model);
303  fg.add(6, (Matrix(1, 1) << 17.0).finished(), 7, (Matrix(1, 1) << 18.0).finished(), (Vector(1) << 19.0).finished(), model);
304  fg.add(7, (Matrix(1, 1) << 20.0).finished(), (Vector(1) << 21.0).finished(), model);
305 
306  // Eliminate into BayesTree
307  // c(6,7)
308  // c(5|6)
309  // c(1,2|5)
310  // c(3,4|5)
311  Ordering ordering(fg.keys());
312  GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted.
313 
314  GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR);
315 
316  Matrix expectedJointJ = (Matrix(2,3) <<
317  5, 0, 6,
318  0, -11, -12
319  ).finished();
320 
321  Matrix actualJointJ = joint.augmentedJacobian();
322 
323  // PR 315: sign of rows in joint are immaterial
324  if (signbit(expectedJointJ(0, 2)) != signbit(actualJointJ(0, 2)))
325  expectedJointJ.row(0) = -expectedJointJ.row(0);
326 
327  if (signbit(expectedJointJ(1, 2)) != signbit(actualJointJ(1, 2)))
328  expectedJointJ.row(1) = -expectedJointJ.row(1);
329 
330  EXPECT(assert_equal(expectedJointJ, actualJointJ));
331 }
332 
333 /* ************************************************************************* */
334 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
335 /* ************************************************************************* */
Matrix3f m
static double sigmax7
GaussianFactorGraph createSmoother(int T)
Definition: smallExample.h:464
static int runAllTests(TestResult &result)
noiseModel::Diagonal::shared_ptr model
static enum @843 ordering
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Rot2 R(Rot2::fromAngle(0.1))
bool signbit(const mpreal &x)
Definition: mpreal.h:2089
MatrixXd L
Definition: LLT_example.cpp:6
Definition: Half.h:150
static double sigmax1
Matrix augmentedJacobian(const Ordering &ordering) const
static double sigmax3
TEST(GaussianBayesTree, linear_smoother_shortcuts)
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
A Gaussian Density.
void add(const GaussianFactor &factor)
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
#define EXPECT(condition)
Definition: Test.h:151
Contains the HessianFactor class, a general quadratic factor.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Conditional Gaussian Base class.
static const Matrix I
Definition: lago.cpp:35
static double sigmax4
boost::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:301
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
static const double tol
size_t size() const
VectorValues optimize() const
Create small example with two poses and one landmark.
Chordal Bayes Net, the result of eliminating a factor graph.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
#define X
Definition: icosphere.cpp:20
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
const Roots & roots() const
Definition: BayesTree.h:147
2D rotation


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:38