testGaussianFactorGraph.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 
24 #include <gtsam/inference/Symbol.h>
27 #include <gtsam/base/debug.h>
29 
32 
33 using namespace std;
34 using namespace gtsam;
35 
36 typedef std::tuple<size_t, size_t, double> SparseTriplet;
38  if (get<0>(a) == get<0>(b) && get<1>(a) == get<1>(b) &&
39  get<2>(a) == get<2>(b)) return true;
40 
41  cout << "not equal:" << endl;
42  cout << "\texpected: "
43  "(" << get<0>(a) << ", " << get<1>(a) << ") = " << get<2>(a) << endl;
44  cout << "\tactual: "
45  "(" << get<0>(b) << ", " << get<1>(b) << ") = " << get<2>(b) << endl;
46  return false;
47 }
48 
49 /* ************************************************************************* */
50 TEST(GaussianFactorGraph, initialization) {
51  // Create empty graph
53  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
54 
55  fg.emplace_shared<JacobianFactor>(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2);
56  fg.emplace_shared<JacobianFactor>(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2);
57  fg.emplace_shared<JacobianFactor>(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2);
58  fg.emplace_shared<JacobianFactor>(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
59 
60  EXPECT_LONGS_EQUAL(4, (long)fg.size());
61 
62  // Test sparse, which takes a vector and returns a matrix, used in MATLAB
63  // Note that this the augmented vector and the RHS is in column 7
64  Matrix expectedIJS =
65  (Matrix(3, 21) <<
66  1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
67  1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
68  10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5.,
69  1., -5., -5., 5., 5., -1., 1.5).finished();
70  Matrix actualIJS = fg.sparseJacobian_();
71  EQUALITY(expectedIJS, actualIJS);
72 }
73 
74 /* ************************************************************************* */
76  // Create empty graph
78  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
79 
80  auto f1 =
81  make_shared<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
82  auto f2 = make_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
83  Vector2(2.0, -1.0), unit2);
84  auto f3 = make_shared<JacobianFactor>(0, -5 * I_2x2, 2, 5 * I_2x2,
85  Vector2(0.0, 1.0), unit2);
86 
87  fg += f1;
88  fg += f2;
89  EXPECT_LONGS_EQUAL(2, fg.size());
90 
91  fg = GaussianFactorGraph();
92  fg += f1, f2, f3;
93  EXPECT_LONGS_EQUAL(3, fg.size());
94 }
95 
96 /* ************************************************************************* */
97 TEST(GaussianFactorGraph, sparseJacobian) {
98  // Create factor graph:
99  // x1 x2 x3 x4 x5 b
100  // 1 2 3 0 0 4
101  // 5 6 7 0 0 8
102  // 9 10 0 11 12 13
103  // 0 0 0 14 15 16
104 
105  // Expected
106  Matrix expected = (Matrix(16, 3) <<
107  1., 1., 2.,
108  1., 2., 4.,
109  1., 3., 6.,
110  2., 1.,10.,
111  2., 2.,12.,
112  2., 3.,14.,
113  1., 6., 8.,
114  2., 6.,16.,
115  3., 1.,18.,
116  3., 2.,20.,
117  3., 4.,22.,
118  3., 5.,24.,
119  4., 4.,28.,
120  4., 5.,30.,
121  3., 6.,26.,
122  4., 6.,32.).finished();
123 
124  // expected: in matlab format - NOTE the transpose!)
125  Matrix expectedMatlab = expected.transpose();
126 
128  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
129  const Key x123 = 0, x45 = 1;
130  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
131  Vector2(4, 8), model);
132  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
133  x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
134  Vector2(13, 16), model);
135 
136  Matrix actual = gfg.sparseJacobian_();
137 
138  EXPECT(assert_equal(expectedMatlab, actual));
139 
140  // SparseTriplets
141  auto boostActual = gfg.sparseJacobian();
142  // check the triplets size...
143  EXPECT_LONGS_EQUAL(16, boostActual.size());
144  // check content
145  for (int i = 0; i < 16; i++) {
147  SparseTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
148  boostActual.at(i)));
149  }
150 }
151 
152 /* ************************************************************************* */
154  // Create factor graph:
155  // x1 x2 x3 x4 x5 b
156  // 1 2 3 0 0 4
157  // 5 6 7 0 0 8
158  // 9 10 0 11 12 13
159  // 0 0 0 14 15 16
160 
161  Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
162  Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished();
163  Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished();
164 
166  SharedDiagonal model = noiseModel::Unit::Create(2);
167  gfg.add(0, A00, Vector2(4., 8.), model);
168  gfg.add(0, A10, 1, A11, Vector2(13., 16.), model);
169 
170  Matrix Ab(4, 6);
171  Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16;
172 
173  // augmented versions
175  EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));
176 
177  // jacobian
178  Matrix A = Ab.leftCols(Ab.cols() - 1);
179  Vector b = Ab.col(Ab.cols() - 1);
180  const auto [actualA, actualb] = gfg.jacobian();
181  EXPECT(assert_equal(A, actualA));
182  EXPECT(assert_equal(b, actualb));
183 
184  // hessian
185  Matrix L = A.transpose() * A;
186  Vector eta = A.transpose() * b;
187  const auto [actualL, actualEta] = gfg.hessian();
188  EXPECT(assert_equal(L, actualL));
189  EXPECT(assert_equal(eta, actualEta));
190 
191  // hessianBlockDiagonal
192  VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
193  expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49));
194  expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225));
195  EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
196 
197  // hessianBlockDiagonal
198  map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal();
199  LONGS_EQUAL(2, actualBD.size());
200  EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0]));
201  EXPECT(assert_equal(A11.transpose() * A11, actualBD[1]));
202 }
203 
204 /* ************************************************************************* */
208  Key x1 = 2, x2 = 0, l1 = 1;
209  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
210  // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
211  fg.emplace_shared<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
212  // odometry between x1 and x2: x2-x1=[0.2;-0.1]
213  fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
214  // measurement between x1 and l1: l1-x1=[0.0;0.2]
215  fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
216  // measurement between x2 and l1: l1-x2=[-0.2;0.3]
217  fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
218  return fg;
219 }
220 
221 /* ************************************************************************* */
224 
225  // Construct expected gradient
226  // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 +
227  // 25*(l1-x2-[-0.2;0.3])^2
228  // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
229  VectorValues expected{{1, Vector2(5.0, -12.5)},
230  {2, Vector2(30.0, 5.0)},
231  {0, Vector2(-25.0, 17.5)}};
232 
233  // Check the gradient at delta=0
234  VectorValues zero = VectorValues::Zero(expected);
235  VectorValues actual = fg.gradient(zero);
236  EXPECT(assert_equal(expected, actual));
238 
239  // Check the gradient at the solution (should be zero)
240  VectorValues solution = fg.optimize();
241  VectorValues actual2 = fg.gradient(solution);
242  EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
243 }
244 
245 /* ************************************************************************* */
246 TEST(GaussianFactorGraph, transposeMultiplication) {
248 
249  Errors e = {Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0),
250  Vector2(-7.5, -5.0)};
251 
253  expected.insert(1, Vector2(-37.5, -50.0));
254  expected.insert(2, Vector2(-150.0, 25.0));
255  expected.insert(0, Vector2(187.5, 25.0));
256 
257  VectorValues actual = A.transposeMultiply(e);
258  EXPECT(assert_equal(expected, actual));
259 }
260 
261 /* ************************************************************************* */
262 TEST(GaussianFactorGraph, eliminate_empty) {
263  // eliminate an empty factor
265  gfg.add(JacobianFactor());
266  const auto [actualBN, remainingGFG] = gfg.eliminatePartialSequential(Ordering());
267 
268  // expected Bayes net is empty
269  GaussianBayesNet expectedBN;
270 
271  // expected remaining graph should be the same as the original, still containing the empty factor
272  GaussianFactorGraph expectedLF = gfg;
273 
274  // check if the result matches
275  EXPECT(assert_equal(*actualBN, expectedBN));
276  EXPECT(assert_equal(*remainingGFG, expectedLF));
277 }
278 
279 /* ************************************************************************* */
282  const auto [A, b] = gfg.jacobian();
283  const auto [AtA, eta] = gfg.hessian();
284  EXPECT(assert_equal(A.transpose() * A, AtA));
285  EXPECT(assert_equal(A.transpose() * b, eta));
286  Matrix expectedAtA(6, 6);
287  expectedAtA << 125, 0, -25, 0, -100, 0, //
288  0, 125, 0, -25, 0, -100, //
289  -25, 0, 50, 0, -25, 0, //
290  0, -25, 0, 50, 0, -25, //
291  -100, 0, -25, 0, 225, 0, //
292  0, -100, 0, -25, 0, 225;
293  EXPECT(assert_equal(expectedAtA, AtA));
294 }
295 
296 /* ************************************************************************* */
297 TEST(GaussianFactorGraph, multiplyHessianAdd) {
299 
300  const VectorValues x{{0, Vector2(1, 2)}, {1, Vector2(3, 4)}, {2, Vector2(5, 6)}};
301 
303  expected.insert(0, Vector2(-450, -450));
304  expected.insert(1, Vector2(0, 0));
305  expected.insert(2, Vector2(950, 1050));
306 
307  VectorValues actual;
308  gfg.multiplyHessianAdd(1.0, x, actual);
309  EXPECT(assert_equal(expected, actual));
310 
311  // now, do it with non-zero y
312  gfg.multiplyHessianAdd(1.0, x, actual);
313  EXPECT(assert_equal(2 * expected, actual));
314 }
315 
316 /* ************************************************************************* */
319  gfg.emplace_shared<HessianFactor>(1, 2, 100 * I_2x2, Z_2x2, Vector2(0.0, 1.0),
320  400 * I_2x2, Vector2(1.0, 1.0), 3.0);
321  return gfg;
322 }
323 
324 /* ************************************************************************* */
325 TEST(GaussianFactorGraph, multiplyHessianAdd2) {
327 
328  // brute force
329  const auto [AtA, eta] = gfg.hessian();
330  Vector X(6);
331  X << 1, 2, 3, 4, 5, 6;
332  Vector Y(6);
333  Y << -450, -450, 300, 400, 2950, 3450;
334  EXPECT(assert_equal(Y, AtA * X));
335 
336  const VectorValues x {{0, Vector2(1, 2)}, {1, Vector2(3, 4)}, {2, Vector2(5, 6)}};
338  expected.insert(0, Vector2(-450, -450));
339  expected.insert(1, Vector2(300, 400));
340  expected.insert(2, Vector2(2950, 3450));
341 
342  VectorValues actual;
343  gfg.multiplyHessianAdd(1.0, x, actual);
344  EXPECT(assert_equal(expected, actual));
345 
346  // now, do it with non-zero y
347  gfg.multiplyHessianAdd(1.0, x, actual);
348  EXPECT(assert_equal(2 * expected, actual));
349 }
350 
351 /* ************************************************************************* */
352 TEST(GaussianFactorGraph, matricesMixed) {
354  const auto [A, b] = gfg.jacobian(); // incorrect !
355  const auto [AtA, eta] = gfg.hessian(); // correct
356  EXPECT(assert_equal(A.transpose() * A, AtA));
357  Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished();
359  EXPECT(assert_equal(A.transpose() * b, eta));
360 }
361 
362 /* ************************************************************************* */
363 TEST(GaussianFactorGraph, gradientAtZero) {
366  VectorValues actual = gfg.gradientAtZero();
367  expected.insert(0, Vector2(-25, 17.5));
368  expected.insert(1, Vector2(5, -13.5));
369  expected.insert(2, Vector2(29, 4));
370  EXPECT(assert_equal(expected, actual));
371 }
372 
373 /* ************************************************************************* */
375  // 2 variables, frontal has dim=4
376  VerticalBlockMatrix blockMatrix(KeyVector{4, 2, 1}, 4);
377  blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0,
378  0.0, 3.0, 0.0, 4.0, 0.0, 0.3, 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
379  GaussianConditional cg(KeyVector{1, 2}, 1, blockMatrix);
380 
382  init_graph.push_back(GaussianFactor::shared_ptr());
383  init_graph.push_back(GaussianConditional(cg));
384 
385  GaussianFactorGraph exp_graph =
386  createGaussianFactorGraphWithHessianFactor(); // Created separately
388  exp_graph.push_back(GaussianConditional(cg));
389 
390  GaussianFactorGraph actCloned = init_graph.clone();
391  EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version
392 
393  // Apply an in-place change to init_graph and compare
394  JacobianFactor::shared_ptr jacFactor0 = init_graph.at<JacobianFactor>(0);
395  CHECK(jacFactor0);
396  jacFactor0->getA(jacFactor0->begin()) *= 7.;
397  EXPECT(assert_inequal(init_graph, exp_graph));
398  EXPECT(assert_equal(exp_graph, actCloned));
399 }
400 
401 /* ************************************************************************* */
404  init_graph.push_back(GaussianFactor::shared_ptr());
405  GaussianFactorGraph actNegation = init_graph.negate();
406  GaussianFactorGraph expNegation;
407  expNegation.push_back(init_graph.at(0)->negate());
408  expNegation.push_back(init_graph.at(1)->negate());
409  expNegation.push_back(init_graph.at(2)->negate());
410  expNegation.push_back(init_graph.at(3)->negate());
411  expNegation.push_back(init_graph.at(4)->negate());
412  expNegation.push_back(GaussianFactor::shared_ptr());
413  EXPECT(assert_equal(expNegation, actNegation));
414 }
415 
416 /* ************************************************************************* */
417 TEST(GaussianFactorGraph, hessianDiagonal) {
420  Matrix infoMatrix = gfg.hessian().first;
421  Vector d = infoMatrix.diagonal();
422 
423  VectorValues actual = gfg.hessianDiagonal();
424  expected.insert(0, d.segment<2>(0));
425  expected.insert(1, d.segment<2>(2));
426  expected.insert(2, d.segment<2>(4));
427  EXPECT(assert_equal(expected, actual));
428 }
429 
430 /* ************************************************************************* */
431 TEST(GaussianFactorGraph, DenseSolve) {
434  VectorValues actual = fg.optimizeDensely();
435  EXPECT(assert_equal(expected, actual));
436 }
437 
438 /* ************************************************************************* */
441  gfg.emplace_shared<JacobianFactor>(1, I_1x1, Z_1x1,
442  noiseModel::Isotropic::Sigma(1, 1.0));
443 
445  values.insert(1, I_1x1);
446 
447  // We are testing the normal distribution PDF where info matrix Σ = 1,
448  // mean mu = 0 and x = 1.
449  // Therefore factor squared error: y = 0.5 * (Σ*x - mu)^2 =
450  // 0.5 * (1.0 - 0)^2 = 0.5
451  // NOTE the 0.5 constant is a part of the factor error.
452  EXPECT_DOUBLES_EQUAL(0.5, gfg.error(values), 1e-12);
453 
454  // The gaussian PDF value is: exp^(-0.5 * (Σ*x - mu)^2) / sqrt(2 * PI)
455  // Ignore the denominator and we get: exp^(-0.5 * (1.0)^2) = exp^(-0.5)
456  double expected = exp(-0.5);
458 }
459 
460 TEST(GaussianFactorGraph, InconsistentEliminationMessage) {
461  // Create empty graph
463  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
464 
466  fg.emplace_shared<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2),
467  unit2);
468  fg.emplace_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
469  Vector2(2.0, -1.0), unit2);
470  fg.emplace_shared<JacobianFactor>(1, -5 * I_2x2, 2, 5 * I_2x2,
471  Vector2(-1.0, 1.5), unit2);
472  fg.emplace_shared<JacobianFactor>(2, -5 * I_2x2, X(3), 5 * I_2x2,
473  Vector2(-1.0, 1.5), unit2);
474 
475  Ordering ordering{0, 1};
476 
477  try {
479  } catch (const exception& exc) {
480  std::string expected_exception_message = "An inference algorithm was called with inconsistent "
481  "arguments. "
482  "The\n"
483  "factor graph, ordering, or variable index were "
484  "inconsistent with "
485  "each\n"
486  "other, or a full elimination routine was called with "
487  "an ordering "
488  "that\n"
489  "does not include all of the variables.\n"
490  "Leftover keys after elimination: 2, x3.";
491  EXPECT(expected_exception_message == exc.what());
492  }
493 
494  // Test large number of keys
495  fg = GaussianFactorGraph();
496  for (size_t i = 0; i < 1000; i++) {
497  fg.emplace_shared<JacobianFactor>(i, -I_2x2, i + 1, I_2x2,
498  Vector2(2.0, -1.0), unit2);
499  }
500 
501  try {
503  } catch (const exception& exc) {
504  std::string expected_exception_message = "An inference algorithm was called with inconsistent "
505  "arguments. "
506  "The\n"
507  "factor graph, ordering, or variable index were "
508  "inconsistent with "
509  "each\n"
510  "other, or a full elimination routine was called with "
511  "an ordering "
512  "that\n"
513  "does not include all of the variables.\n"
514  "Leftover keys after elimination: 2, 3, 4, 5, ... (total 999 keys).";
515  EXPECT(expected_exception_message == exc.what());
516  }
517 }
518 /* ************************************************************************* */
519 int main() {
520  TestResult tr;
521  return TestRegistry::runAllTests(tr);
522 }
523 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
createGaussianFactorGraphWithHessianFactor
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor()
Definition: testGaussianFactorGraph.cpp:317
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
gtsam::GaussianFactorGraph::hessianDiagonal
virtual VectorValues hessianDiagonal() const
Definition: GaussianFactorGraph.cpp:279
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::GaussianFactorGraph::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
‍** y += alpha*A'A*x *‍/
Definition: GaussianFactorGraph.cpp:420
gtsam::GaussianFactorGraph::hessianBlockDiagonal
virtual std::map< Key, Matrix > hessianBlockDiagonal() const
Definition: GaussianFactorGraph.cpp:290
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::GaussianFactorGraph::optimizeDensely
VectorValues optimizeDensely() const
Definition: GaussianFactorGraph.cpp:323
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:88
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
gtsam::GaussianFactorGraph::hessian
std::pair< Matrix, Vector > hessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:264
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::GaussianFactorGraph::jacobian
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:232
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
gtsam::GaussianFactorGraph::augmentedJacobian
Matrix augmentedJacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:217
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
X
#define X
Definition: icosphere.cpp:20
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::GaussianFactorGraph::sparseJacobian_
Matrix sparseJacobian_() const
Definition: GaussianFactorGraph.cpp:199
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
EQUALITY
#define EQUALITY(expected, actual)
Definition: Test.h:127
TestableAssertions.h
Provides additional testing facilities for common data structures.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::GaussianFactorGraph::gradient
VectorValues gradient(const VectorValues &x0) const
Definition: GaussianFactorGraph.cpp:357
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
SparseTriplet
std::tuple< size_t, size_t, double > SparseTriplet
Definition: testGaussianFactorGraph.cpp:36
createSimpleGaussianFactorGraph
static GaussianFactorGraph createSimpleGaussianFactorGraph()
Factor graph with 2 2D factors on 3 2D variables.
Definition: testGaussianFactorGraph.cpp:206
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::VectorValues
Definition: VectorValues.h:74
Eigen::internal::negate
T negate(const T &x)
Definition: packetmath_test_shared.h:24
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:151
gtsam::GaussianFactorGraph::gradientAtZero
virtual VectorValues gradientAtZero() const
Definition: GaussianFactorGraph.cpp:369
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and GaussianConditional.
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
zero
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:296
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::FastList< Vector >
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
gtsam::GaussianFactorGraph::probPrime
double probPrime(const VectorValues &c) const
Definition: GaussianFactorGraph.cpp:81
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
A10
static const double A10[]
Definition: expn.h:15
gtsam::GaussianFactorGraph::augmentedHessian
Matrix augmentedHessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:247
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
f3
double f3(double x1, double x2)
Definition: testNumericalDerivative.cpp:76
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.cpp:71
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Definition: VerticalBlockMatrix.h:188
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
VariableIndex.h
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
A11
static const double A11[]
Definition: expn.h:16
TEST
TEST(GaussianFactorGraph, initialization)
Definition: testGaussianFactorGraph.cpp:50
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
main
int main()
Definition: testGaussianFactorGraph.cpp:519
gtsam::GaussianFactorGraph::sparseJacobian
std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering &ordering, size_t &nrows, size_t &ncols) const
Definition: GaussianFactorGraph.cpp:119
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: GaussianFactorGraph.cpp:309
gtsam::GaussianFactorGraph::negate
GaussianFactorGraph negate() const
Definition: GaussianFactorGraph.cpp:106
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
triplet_equal
bool triplet_equal(SparseTriplet a, SparseTriplet b)
Definition: testGaussianFactorGraph.cpp:37
gtsam::GaussianFactorGraph::clone
virtual GaussianFactorGraph clone() const
Definition: GaussianFactorGraph.cpp:94
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
debug.h
Global debugging flags.


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:57