testGaussianJunctionTreeB.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/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 
52 using namespace std;
53 using namespace gtsam;
54 using namespace example;
55 
56 using symbol_shorthand::X;
57 using 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  */
66 TEST( GaussianJunctionTreeB, constructor2 ) {
67  // create a graph
68  const auto [nlfg, values] = createNonlinearSmoother(7);
69  SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic();
70 
71  // linearize
72  GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values);
73 
74  const Ordering ordering {X(1), X(3), X(5), X(7), X(2), X(6), X(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(3), X(2), X(4)}, o56{X(5), X(6)}, o7{X(7)}, o1{X(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(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 
113 TEST(GaussianJunctionTreeB, OptimizeMultiFrontal) {
114  // create a graph
115  const auto fg = createSmoother(7);
116 
117  // optimize the graph
118  const VectorValues actual = fg.optimize(&EliminateQR);
119 
120  // verify
121  VectorValues expected;
122  const Vector v = Vector2(0., 0.);
123  for (int i = 1; i <= 7; i++) expected.emplace(X(i), v);
124  EXPECT(assert_equal(expected, actual));
125 }
126 
127 /* ************************************************************************* */
128 TEST(GaussianJunctionTreeB, optimizeMultiFrontal2) {
129  // create a graph
130  const auto nlfg = createNonlinearFactorGraph();
131  const auto noisy = createNoisyValues();
132  const auto fg = *nlfg.linearize(noisy);
133 
134  // optimize the graph
135  VectorValues actual = fg.optimize(&EliminateQR);
136 
137  // verify
138  VectorValues expected = createCorrectDelta(); // expected solution
139  EXPECT(assert_equal(expected, actual));
140 }
141 
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 //
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 /* ************************************************************************* */
245 int main() {
246  TestResult tr;
247  return TestRegistry::runAllTests(tr);
248 }
249 /* ************************************************************************* */
250 
TEST
TEST(GaussianJunctionTreeB, constructor2)
Definition: testGaussianJunctionTreeB.cpp:66
sharedNode
SymbolicEliminationTree::sharedNode sharedNode
Definition: testSymbolicEliminationTree.cpp:31
gtsam::example::createNoisyValues
Values createNoisyValues()
Definition: smallExample.h:243
Testable.h
Concept check for values that can be used in unit tests.
gtsam::example::createSmoother
GaussianFactorGraph createSmoother(int T)
Definition: smallExample.h:465
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Values.h
A non-templated config holding any types of Manifold-group elements.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
leaf::values
leaf::MyValues values
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: BFloat16.h:88
ordering
static enum @1107 ordering
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Definition: VectorValues.h:185
gtsam::ClusterTree::roots
const FastVector< sharedNode > & roots() const
Definition: ClusterTree.h:159
VectorValues.h
Factor Graph Values.
gtsam::example::createCorrectDelta
VectorValues createCorrectDelta()
Definition: smallExample.h:248
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::example::createNonlinearSmoother
std::pair< NonlinearFactorGraph, Values > createNonlinearSmoother(int T)
Definition: smallExample.h:431
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::example::createNonlinearFactorGraph
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:206
GaussianConditional.h
Conditional Gaussian Base class.
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
gtsam
traits
Definition: chartTesting.h:28
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
gtsam::SymbolicFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: SymbolicFactorGraph.h:70
x1
Pose3 x1
Definition: testPose3.cpp:663
smallExample.h
Create small example with two poses and one landmark.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
Pose2.h
2D Pose
X
#define X
Definition: icosphere.cpp:20
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.

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