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 testGaussianFactorGraphB.cpp
14 * @brief Unit tests for Linear Factor Graph
15 * @author Christian Potthast
16 **/
17
18#include <tests/smallExample.h>
19#include <gtsam/inference/Symbol.h>
20#include <gtsam/linear/GaussianBayesNet.h>
21#include <gtsam/linear/GaussianBayesTree.h>
22#include <gtsam/linear/GaussianFactorGraph.h>
23#include <gtsam/base/numericalDerivative.h>
24#include <gtsam/base/Matrix.h>
25#include <gtsam/base/Testable.h>
26
27#include <CppUnitLite/TestHarness.h>
28
29#include <string.h>
30#include <iostream>
31
32using namespace std;
33using namespace gtsam;
34using namespace example;
35
36double tol=1e-5;
37
38using symbol_shorthand::X;
39using symbol_shorthand::L;
40
41static auto kUnit2 = noiseModel::Unit::Create(dim: 2);
42
43/* ************************************************************************* */
44TEST( GaussianFactorGraph, equals ) {
45
46 GaussianFactorGraph fg = createGaussianFactorGraph();
47 GaussianFactorGraph fg2 = createGaussianFactorGraph();
48 EXPECT(fg.equals(fg2));
49}
50
51/* ************************************************************************* */
52TEST( GaussianFactorGraph, error ) {
53 GaussianFactorGraph fg = createGaussianFactorGraph();
54 VectorValues cfg = createZeroDelta();
55
56 // note the error is the same as in testNonlinearFactorGraph as a
57 // zero delta config in the linear graph is equivalent to noisy in
58 // non-linear, which is really linear under the hood
59 double actual = fg.error(x: cfg);
60 DOUBLES_EQUAL( 5.625, actual, 1e-9 );
61}
62
63/* ************************************************************************* */
64TEST(GaussianFactorGraph, eliminateOne_x1) {
65 GaussianFactorGraph fg = createGaussianFactorGraph();
66
67 GaussianConditional::shared_ptr conditional;
68 auto result = fg.eliminatePartialSequential(ordering: Ordering{X(j: 1)});
69 conditional = result.first->front();
70
71 // create expected Conditional Gaussian
72 Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I;
73 Vector d = Vector2(-0.133333, -0.0222222);
74 GaussianConditional expected(X(j: 1), 15 * d, R11, L(j: 1), S12, X(j: 2), S13);
75
76 EXPECT(assert_equal(expected, *conditional, tol));
77}
78
79/* ************************************************************************* */
80TEST(GaussianFactorGraph, eliminateOne_x2) {
81 const Ordering ordering{X(j: 2), L(j: 1), X(j: 1)};
82 GaussianFactorGraph fg = createGaussianFactorGraph();
83 auto actual = EliminateQR(factors: fg, keys: Ordering{X(j: 2)}).first;
84
85 // create expected Conditional Gaussian
86 double sigma = 0.0894427;
87 Matrix I = I_2x2 / sigma, R11 = I, S12 = -0.2 * I, S13 = -0.8 * I;
88 Vector d = Vector2(0.2, -0.14) / sigma;
89 GaussianConditional expected(X(j: 2), d, R11, L(j: 1), S12, X(j: 1), S13, kUnit2);
90
91 EXPECT(assert_equal(expected, *actual, tol));
92}
93
94/* ************************************************************************* */
95TEST(GaussianFactorGraph, eliminateOne_l1) {
96 const Ordering ordering{L(j: 1), X(j: 1), X(j: 2)};
97 GaussianFactorGraph fg = createGaussianFactorGraph();
98 auto actual = EliminateQR(factors: fg, keys: Ordering{L(j: 1)}).first;
99
100 // create expected Conditional Gaussian
101 double sigma = sqrt(x: 2.0) / 10.;
102 Matrix I = I_2x2 / sigma, R11 = I, S12 = -0.5 * I, S13 = -0.5 * I;
103 Vector d = Vector2(-0.1, 0.25) / sigma;
104 GaussianConditional expected(L(j: 1), d, R11, X(j: 1), S12, X(j: 2), S13, kUnit2);
105
106 EXPECT(assert_equal(expected, *actual, tol));
107}
108
109/* ************************************************************************* */
110TEST(GaussianFactorGraph, eliminateOne_x1_fast) {
111 GaussianFactorGraph fg = createGaussianFactorGraph();
112 const auto [conditional, remaining] = EliminateQR(factors: fg, keys: Ordering{X(j: 1)});
113
114 // create expected Conditional Gaussian
115 Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I;
116 Vector d = Vector2(-0.133333, -0.0222222);
117 GaussianConditional expected(X(j: 1), 15 * d, R11, L(j: 1), S12, X(j: 2), S13, kUnit2);
118
119 // Create expected remaining new factor
120 JacobianFactor expectedFactor(
121 L(j: 1), (Matrix(4, 2) << 6.87184, 0, 0, 6.87184, 0, 0, 0, 0).finished(),
122 X(j: 2),
123 (Matrix(4, 2) << -5.25494, 0, 0, -5.25494, -7.27607, 0, 0, -7.27607)
124 .finished(),
125 (Vector(4) << -1.21268, 1.73817, -0.727607, 1.45521).finished(),
126 noiseModel::Unit::Create(dim: 4));
127
128 EXPECT(assert_equal(expected, *conditional, tol));
129 EXPECT(assert_equal(expectedFactor, *remaining, tol));
130}
131
132/* ************************************************************************* */
133TEST(GaussianFactorGraph, eliminateOne_x2_fast) {
134 GaussianFactorGraph fg = createGaussianFactorGraph();
135 auto actual = EliminateQR(factors: fg, keys: Ordering{X(j: 2)}).first;
136
137 // create expected Conditional Gaussian
138 double sigma = 0.0894427;
139 Matrix I = I_2x2 / sigma, R11 = -I, S12 = 0.2 * I, S13 = 0.8 * I;
140 Vector d = Vector2(-0.2, 0.14) / sigma;
141 GaussianConditional expected(X(j: 2), d, R11, L(j: 1), S12, X(j: 1), S13, kUnit2);
142
143 EXPECT(assert_equal(expected, *actual, tol));
144}
145
146/* ************************************************************************* */
147TEST(GaussianFactorGraph, eliminateOne_l1_fast) {
148 GaussianFactorGraph fg = createGaussianFactorGraph();
149 auto actual = EliminateQR(factors: fg, keys: Ordering{L(j: 1)}).first;
150
151 // create expected Conditional Gaussian
152 double sigma = sqrt(x: 2.0) / 10.;
153 Matrix I = I_2x2 / sigma, R11 = -I, S12 = 0.5 * I, S13 = 0.5 * I;
154 Vector d = Vector2(0.1, -0.25) / sigma;
155 GaussianConditional expected(L(j: 1), d, R11, X(j: 1), S12, X(j: 2), S13, kUnit2);
156
157 EXPECT(assert_equal(expected, *actual, tol));
158}
159
160/* ************************************************************************* */
161TEST(GaussianFactorGraph, copying) {
162 // Create a graph
163 GaussianFactorGraph actual = createGaussianFactorGraph();
164
165 // Copy the graph !
166 GaussianFactorGraph copy = actual;
167
168 // now eliminate the copy
169 GaussianBayesNet actual1 = *copy.eliminateSequential();
170
171 // Create the same graph, but not by copying
172 GaussianFactorGraph expected = createGaussianFactorGraph();
173
174 // and check that original is still the same graph
175 EXPECT(assert_equal(expected, actual));
176}
177
178/* ************************************************************************* */
179TEST(GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet) {
180 GaussianFactorGraph fg = createGaussianFactorGraph();
181
182 // render with a given ordering
183 GaussianBayesNet CBN = *fg.eliminateSequential();
184
185 // True GaussianFactorGraph
186 GaussianFactorGraph fg2(CBN);
187 GaussianBayesNet CBN2 = *fg2.eliminateSequential();
188 EXPECT(assert_equal(CBN, CBN2));
189}
190
191/* ************************************************************************* */
192TEST(GaussianFactorGraph, optimize_Cholesky) {
193 // create a graph
194 GaussianFactorGraph fg = createGaussianFactorGraph();
195
196 // optimize the graph
197 VectorValues actual = fg.optimize(function: EliminateCholesky);
198
199 // verify
200 VectorValues expected = createCorrectDelta();
201 EXPECT(assert_equal(expected, actual));
202}
203
204/* ************************************************************************* */
205TEST( GaussianFactorGraph, optimize_QR )
206{
207 // create a graph
208 GaussianFactorGraph fg = createGaussianFactorGraph();
209
210 // optimize the graph
211 VectorValues actual = fg.optimize(function: EliminateQR);
212
213 // verify
214 VectorValues expected = createCorrectDelta();
215 EXPECT(assert_equal(expected,actual));
216}
217
218/* ************************************************************************* */
219TEST(GaussianFactorGraph, combine) {
220 // create a test graph
221 GaussianFactorGraph fg1 = createGaussianFactorGraph();
222
223 // create another factor graph
224 GaussianFactorGraph fg2 = createGaussianFactorGraph();
225
226 // get sizes
227 size_t size1 = fg1.size();
228 size_t size2 = fg2.size();
229
230 // combine them
231 fg1.push_back(container: fg2);
232
233 EXPECT(size1 + size2 == fg1.size());
234}
235
236/* ************************************************************************* */
237// print a vector of ints if needed for debugging
238void print(vector<int> v) {
239 for (size_t k = 0; k < v.size(); k++) cout << v[k] << " ";
240 cout << endl;
241}
242
243/* ************************************************************************* */
244TEST(GaussianFactorGraph, createSmoother) {
245 GaussianFactorGraph fg1 = createSmoother(T: 2);
246 LONGS_EQUAL(3, fg1.size());
247 GaussianFactorGraph fg2 = createSmoother(T: 3);
248 LONGS_EQUAL(5, fg2.size());
249}
250
251/* ************************************************************************* */
252double error(const VectorValues& x) {
253 GaussianFactorGraph fg = createGaussianFactorGraph();
254 return fg.error(x);
255}
256
257/* ************************************************************************* */
258TEST(GaussianFactorGraph, multiplication) {
259 GaussianFactorGraph A = createGaussianFactorGraph();
260 VectorValues x = createCorrectDelta();
261 Errors actual = A * x;
262 Errors expected;
263 expected.push_back(x: Vector2(-1.0, -1.0));
264 expected.push_back(x: Vector2(2.0, -1.0));
265 expected.push_back(x: Vector2(0.0, 1.0));
266 expected.push_back(x: Vector2(-1.0, 1.5));
267 EXPECT(assert_equal(expected, actual));
268}
269
270/* ************************************************************************* */
271// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
272TEST(GaussianFactorGraph, elimination) {
273 // Create Gaussian Factor Graph
274 GaussianFactorGraph fg;
275 Matrix Ap = I_1x1, An = I_1x1 * -1;
276 Vector b = (Vector(1) << 0.0).finished();
277 SharedDiagonal sigma = noiseModel::Isotropic::Sigma(dim: 1, sigma: 2.0);
278 fg.emplace_shared<JacobianFactor>(args: X(j: 1), args&: An, args: X(j: 2), args&: Ap, args&: b, args&: sigma);
279 fg.emplace_shared<JacobianFactor>(args: X(j: 1), args&: Ap, args&: b, args&: sigma);
280 fg.emplace_shared<JacobianFactor>(args: X(j: 2), args&: Ap, args&: b, args&: sigma);
281
282 // Eliminate
283 const Ordering ordering{X(j: 1), X(j: 2)};
284 GaussianBayesNet bayesNet = *fg.eliminateSequential();
285
286 // Check matrix
287 const auto [R, d] = bayesNet.matrix();
288 Matrix expected =
289 (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished();
290 Matrix expected2 =
291 (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372).finished();
292 EXPECT(assert_equal(expected, R, 1e-6));
293 EXPECT(equal_with_abs_tol(expected, R, 1e-6) ||
294 equal_with_abs_tol(expected2, R, 1e-6));
295}
296
297/* ************************************************************************* */
298// Tests ported from ConstrainedGaussianFactorGraph
299/* ************************************************************************* */
300TEST(GaussianFactorGraph, constrained_simple) {
301 // get a graph with a constraint in it
302 GaussianFactorGraph fg = createSimpleConstraintGraph();
303 EXPECT(hasConstraints(fg));
304
305 // eliminate and solve
306 VectorValues actual = fg.eliminateSequential()->optimize();
307
308 // verify
309 VectorValues expected = createSimpleConstraintValues();
310 EXPECT(assert_equal(expected, actual));
311}
312
313/* ************************************************************************* */
314TEST(GaussianFactorGraph, constrained_single) {
315 // get a graph with a constraint in it
316 GaussianFactorGraph fg = createSingleConstraintGraph();
317 EXPECT(hasConstraints(fg));
318
319 // eliminate and solve
320 VectorValues actual = fg.eliminateSequential()->optimize();
321
322 // verify
323 VectorValues expected = createSingleConstraintValues();
324 EXPECT(assert_equal(expected, actual));
325}
326
327/* ************************************************************************* */
328TEST(GaussianFactorGraph, constrained_multi1) {
329 // get a graph with a constraint in it
330 GaussianFactorGraph fg = createMultiConstraintGraph();
331 EXPECT(hasConstraints(fg));
332
333 // eliminate and solve
334 VectorValues actual = fg.eliminateSequential()->optimize();
335
336 // verify
337 VectorValues expected = createMultiConstraintValues();
338 EXPECT(assert_equal(expected, actual));
339}
340
341/* ************************************************************************* */
342
343static SharedDiagonal model = noiseModel::Isotropic::Sigma(dim: 2,sigma: 1);
344
345/* ************************************************************************* */
346TEST(GaussianFactorGraph, replace)
347{
348 SharedDiagonal noise(noiseModel::Isotropic::Sigma(dim: 3, sigma: 1.0));
349
350 GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
351 X(j: 1), I_3x3, X(j: 2), I_3x3, Z_3x1, noise));
352 GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
353 X(j: 2), I_3x3, X(j: 3), I_3x3, Z_3x1, noise));
354 GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
355 X(j: 3), I_3x3, X(j: 4), I_3x3, Z_3x1, noise));
356 GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
357 X(j: 5), I_3x3, X(j: 6), I_3x3, Z_3x1, noise));
358
359 GaussianFactorGraph actual;
360 actual.push_back(factor: f1);
361 actual.push_back(factor: f2);
362 actual.push_back(factor: f3);
363 actual.replace(index: 0, factor: f4);
364
365 GaussianFactorGraph expected;
366 expected.push_back(factor: f4);
367 expected.push_back(factor: f2);
368 expected.push_back(factor: f3);
369
370 EXPECT(assert_equal(expected, actual));
371}
372
373/* ************************************************************************* */
374TEST(GaussianFactorGraph, hasConstraints)
375{
376 FactorGraph<GaussianFactor> fgc1 = createMultiConstraintGraph();
377 EXPECT(hasConstraints(fgc1));
378
379 FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
380 EXPECT(hasConstraints(fgc2));
381
382 GaussianFactorGraph fg = createGaussianFactorGraph();
383 EXPECT(!hasConstraints(fg));
384}
385
386#include <gtsam/slam/ProjectionFactor.h>
387#include <gtsam/geometry/Pose3.h>
388#include <gtsam/sam/RangeFactor.h>
389
390/* ************************************************************************* */
391TEST( GaussianFactorGraph, conditional_sigma_failure) {
392 // This system derives from a failure case in DDF in which a Bayes Tree
393 // has non-unit sigmas for conditionals in the Bayes Tree, which
394 // should never happen by construction
395
396 // Reason for the failure: using Vector_() is dangerous as having a non-float gets set to zero, resulting in constraints
397 gtsam::Key xC1 = 0, l32 = 1, l41 = 2;
398
399 // noisemodels at nonlinear level
400 gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas(sigmas: (Vector(6) << 0.05, 0.05, 3.0, 0.2, 0.2, 0.2).finished());
401 gtsam::SharedNoiseModel measModel = kUnit2;
402 gtsam::SharedNoiseModel elevationModel = noiseModel::Isotropic::Sigma(dim: 1, sigma: 3.0);
403
404 double fov = 60; // degrees
405 int imgW = 640; // pixels
406 int imgH = 480; // pixels
407 gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(fov, imgW, imgH));
408
409 typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
410
411 double relElevation = 6;
412
413 Values initValues;
414 initValues.insert(j: xC1,
415 val: Pose3(Rot3(
416 -1., 0.0, 1.2246468e-16,
417 0.0, 1., 0.0,
418 -1.2246468e-16, 0.0, -1.),
419 Point3(0.511832102, 8.42819594, 5.76841725)));
420 initValues.insert(j: l32, val: Point3(0.364081507, 6.89766221, -0.231582751) );
421 initValues.insert(j: l41, val: Point3(1.61051523, 6.7373052, -0.231582751) );
422
423 NonlinearFactorGraph factors;
424 factors.addPrior(key: xC1,
425 prior: Pose3(Rot3(
426 -1., 0.0, 1.2246468e-16,
427 0.0, 1., 0.0,
428 -1.2246468e-16, 0.0, -1),
429 Point3(0.511832102, 8.42819594, 5.76841725)), model: priorModel);
430 factors.emplace_shared<ProjectionFactor>(args: Point2(333.648615, 98.61535), args&: measModel, args&: xC1, args&: l32, args&: K);
431 factors.emplace_shared<ProjectionFactor>(args: Point2(218.508, 83.8022039), args&: measModel, args&: xC1, args&: l41, args&: K);
432 factors.emplace_shared<RangeFactor<Pose3,Point3>>(args&: xC1, args&: l32, args&: relElevation, args&: elevationModel);
433 factors.emplace_shared<RangeFactor<Pose3,Point3>>(args&: xC1, args&: l41, args&: relElevation, args&: elevationModel);
434
435 // Check that sigmas are correct (i.e., unit)
436 GaussianFactorGraph lfg = *factors.linearize(linearizationPoint: initValues);
437
438 GaussianBayesTree actBT = *lfg.eliminateMultifrontal();
439
440 // Check that all sigmas in an unconstrained bayes tree are set to one
441 for (const auto& [key, clique]: actBT.nodes()) {
442 GaussianConditional::shared_ptr conditional = clique->conditional();
443 //size_t dim = conditional->rows();
444 //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol));
445 EXPECT(!conditional->get_model());
446 }
447}
448
449/* ************************************************************************* */
450int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
451/* ************************************************************************* */
452