testGaussianFactorGraphB.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/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 
32 using namespace std;
33 using namespace gtsam;
34 using namespace example;
35 
36 double tol=1e-5;
37 
38 using symbol_shorthand::X;
39 using symbol_shorthand::L;
40 
41 static auto kUnit2 = noiseModel::Unit::Create(2);
42 
43 /* ************************************************************************* */
44 TEST( GaussianFactorGraph, equals ) {
45 
46  GaussianFactorGraph fg = createGaussianFactorGraph();
47  GaussianFactorGraph fg2 = createGaussianFactorGraph();
48  EXPECT(fg.equals(fg2));
49 }
50 
51 /* ************************************************************************* */
52 TEST( 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(cfg);
60  DOUBLES_EQUAL( 5.625, actual, 1e-9 );
61 }
62 
63 /* ************************************************************************* */
64 TEST(GaussianFactorGraph, eliminateOne_x1) {
65  GaussianFactorGraph fg = createGaussianFactorGraph();
66 
67  GaussianConditional::shared_ptr conditional;
68  auto result = fg.eliminatePartialSequential(Ordering{X(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(1), 15 * d, R11, L(1), S12, X(2), S13);
75 
76  EXPECT(assert_equal(expected, *conditional, tol));
77 }
78 
79 /* ************************************************************************* */
80 TEST(GaussianFactorGraph, eliminateOne_x2) {
81  const Ordering ordering{X(2), L(1), X(1)};
82  GaussianFactorGraph fg = createGaussianFactorGraph();
83  auto actual = EliminateQR(fg, Ordering{X(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(2), d, R11, L(1), S12, X(1), S13, kUnit2);
90 
91  EXPECT(assert_equal(expected, *actual, tol));
92 }
93 
94 /* ************************************************************************* */
95 TEST(GaussianFactorGraph, eliminateOne_l1) {
96  const Ordering ordering{L(1), X(1), X(2)};
97  GaussianFactorGraph fg = createGaussianFactorGraph();
98  auto actual = EliminateQR(fg, Ordering{L(1)}).first;
99 
100  // create expected Conditional Gaussian
101  double sigma = sqrt(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(1), d, R11, X(1), S12, X(2), S13, kUnit2);
105 
106  EXPECT(assert_equal(expected, *actual, tol));
107 }
108 
109 /* ************************************************************************* */
110 TEST(GaussianFactorGraph, eliminateOne_x1_fast) {
111  GaussianFactorGraph fg = createGaussianFactorGraph();
112  const auto [conditional, remaining] = EliminateQR(fg, Ordering{X(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(1), 15 * d, R11, L(1), S12, X(2), S13, kUnit2);
118 
119  // Create expected remaining new factor
120  JacobianFactor expectedFactor(
121  L(1), (Matrix(4, 2) << 6.87184, 0, 0, 6.87184, 0, 0, 0, 0).finished(),
122  X(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(4));
127 
128  EXPECT(assert_equal(expected, *conditional, tol));
129  EXPECT(assert_equal(expectedFactor, *remaining, tol));
130 }
131 
132 /* ************************************************************************* */
133 TEST(GaussianFactorGraph, eliminateOne_x2_fast) {
134  GaussianFactorGraph fg = createGaussianFactorGraph();
135  auto actual = EliminateQR(fg, Ordering{X(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(2), d, R11, L(1), S12, X(1), S13, kUnit2);
142 
143  EXPECT(assert_equal(expected, *actual, tol));
144 }
145 
146 /* ************************************************************************* */
147 TEST(GaussianFactorGraph, eliminateOne_l1_fast) {
148  GaussianFactorGraph fg = createGaussianFactorGraph();
149  auto actual = EliminateQR(fg, Ordering{L(1)}).first;
150 
151  // create expected Conditional Gaussian
152  double sigma = sqrt(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(1), d, R11, X(1), S12, X(2), S13, kUnit2);
156 
157  EXPECT(assert_equal(expected, *actual, tol));
158 }
159 
160 /* ************************************************************************* */
161 TEST(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 /* ************************************************************************* */
179 TEST(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 /* ************************************************************************* */
192 TEST(GaussianFactorGraph, optimize_Cholesky) {
193  // create a graph
194  GaussianFactorGraph fg = createGaussianFactorGraph();
195 
196  // optimize the graph
197  VectorValues actual = fg.optimize(EliminateCholesky);
198 
199  // verify
200  VectorValues expected = createCorrectDelta();
201  EXPECT(assert_equal(expected, actual));
202 }
203 
204 /* ************************************************************************* */
205 TEST( GaussianFactorGraph, optimize_QR )
206 {
207  // create a graph
208  GaussianFactorGraph fg = createGaussianFactorGraph();
209 
210  // optimize the graph
211  VectorValues actual = fg.optimize(EliminateQR);
212 
213  // verify
214  VectorValues expected = createCorrectDelta();
215  EXPECT(assert_equal(expected,actual));
216 }
217 
218 /* ************************************************************************* */
219 TEST(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(fg2);
232 
233  EXPECT(size1 + size2 == fg1.size());
234 }
235 
236 /* ************************************************************************* */
237 // print a vector of ints if needed for debugging
238 void print(vector<int> v) {
239  for (size_t k = 0; k < v.size(); k++) cout << v[k] << " ";
240  cout << endl;
241 }
242 
243 /* ************************************************************************* */
244 TEST(GaussianFactorGraph, createSmoother) {
245  GaussianFactorGraph fg1 = createSmoother(2);
246  LONGS_EQUAL(3, fg1.size());
247  GaussianFactorGraph fg2 = createSmoother(3);
248  LONGS_EQUAL(5, fg2.size());
249 }
250 
251 /* ************************************************************************* */
252 double error(const VectorValues& x) {
253  GaussianFactorGraph fg = createGaussianFactorGraph();
254  return fg.error(x);
255 }
256 
257 /* ************************************************************************* */
258 TEST(GaussianFactorGraph, multiplication) {
259  GaussianFactorGraph A = createGaussianFactorGraph();
260  VectorValues x = createCorrectDelta();
261  Errors actual = A * x;
262  Errors expected;
263  expected.push_back(Vector2(-1.0, -1.0));
264  expected.push_back(Vector2(2.0, -1.0));
265  expected.push_back(Vector2(0.0, 1.0));
266  expected.push_back(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
272 TEST(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(1, 2.0);
278  fg.emplace_shared<JacobianFactor>(X(1), An, X(2), Ap, b, sigma);
279  fg.emplace_shared<JacobianFactor>(X(1), Ap, b, sigma);
280  fg.emplace_shared<JacobianFactor>(X(2), Ap, b, sigma);
281 
282  // Eliminate
283  const Ordering ordering{X(1), X(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 /* ************************************************************************* */
300 TEST(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 /* ************************************************************************* */
314 TEST(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 /* ************************************************************************* */
328 TEST(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 
343 static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
344 
345 /* ************************************************************************* */
346 TEST(GaussianFactorGraph, replace)
347 {
348  SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
349 
350  GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
351  X(1), I_3x3, X(2), I_3x3, Z_3x1, noise));
352  GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
353  X(2), I_3x3, X(3), I_3x3, Z_3x1, noise));
354  GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
355  X(3), I_3x3, X(4), I_3x3, Z_3x1, noise));
356  GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
357  X(5), I_3x3, X(6), I_3x3, Z_3x1, noise));
358 
359  GaussianFactorGraph actual;
360  actual.push_back(f1);
361  actual.push_back(f2);
362  actual.push_back(f3);
363  actual.replace(0, f4);
364 
365  GaussianFactorGraph expected;
366  expected.push_back(f4);
367  expected.push_back(f2);
368  expected.push_back(f3);
369 
370  EXPECT(assert_equal(expected, actual));
371 }
372 
373 /* ************************************************************************* */
374 TEST(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 /* ************************************************************************* */
391 TEST( 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((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(1, 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(xC1,
415  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(l32, Point3(0.364081507, 6.89766221, -0.231582751) );
421  initValues.insert(l41, Point3(1.61051523, 6.7373052, -0.231582751) );
422 
423  NonlinearFactorGraph factors;
424  factors.addPrior(xC1,
425  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)), priorModel);
430  factors.emplace_shared<ProjectionFactor>(Point2(333.648615, 98.61535), measModel, xC1, l32, K);
431  factors.emplace_shared<ProjectionFactor>(Point2(218.508, 83.8022039), measModel, xC1, l41, K);
432  factors.emplace_shared<RangeFactor<Pose3,Point3>>(xC1, l32, relElevation, elevationModel);
433  factors.emplace_shared<RangeFactor<Pose3,Point3>>(xC1, l41, relElevation, elevationModel);
434 
435  // Check that sigmas are correct (i.e., unit)
436  GaussianFactorGraph lfg = *factors.linearize(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 /* ************************************************************************* */
450 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
451 /* ************************************************************************* */
gtsam::example::createMultiConstraintGraph
GaussianFactorGraph createMultiConstraintGraph()
Definition: smallExample.h:553
test_docs.d
d
Definition: test_docs.py:28
key
const gtsam::Symbol key('X', 0)
I
#define I
Definition: main.h:112
gtsam::example::createMultiConstraintValues
VectorValues createMultiConstraintValues()
Definition: smallExample.h:608
gtsam::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
Definition: FactorGraph.h:396
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
b
Scalar * b
Definition: benchVecAdd.cpp:17
Testable.h
Concept check for values that can be used in unit tests.
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
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:270
gtsam::BayesTree::nodes
const Nodes & nodes() const
Definition: BayesTree.h:146
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
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
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:149
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::example::createSingleConstraintValues
VectorValues createSingleConstraintValues()
Definition: smallExample.h:546
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:204
K
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.cpp:71
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
ordering
static enum @1107 ordering
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Definition: GaussianFactorGraph.cpp:309
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::GaussianFactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Definition: GaussianFactorGraph.cpp:42
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:334
gtsam::example::createCorrectDelta
VectorValues createCorrectDelta()
Definition: smallExample.h:248
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
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
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::example::createZeroDelta
VectorValues createZeroDelta()
Definition: smallExample.h:259
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778
model
static SharedDiagonal model
Definition: testGaussianFactorGraphB.cpp:343
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
kUnit2
static auto kUnit2
Definition: testGaussianFactorGraphB.cpp:41
print
void print(vector< int > v)
Definition: testGaussianFactorGraphB.cpp:238
gtsam::example::createSimpleConstraintGraph
GaussianFactorGraph createSimpleConstraintGraph()
Definition: smallExample.h:471
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam
traits
Definition: chartTesting.h:28
gtsam::GaussianBayesNet::matrix
std::pair< Matrix, Vector > matrix(const Ordering &ordering) const
Definition: GaussianBayesNet.cpp:205
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::example::createSimpleConstraintValues
VectorValues createSimpleConstraintValues()
Definition: smallExample.h:500
unary::f1
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Definition: testExpression.cpp:79
f4
double f4(double x, double y, double z)
Definition: testNumericalDerivative.cpp:105
f3
double f3(double x1, double x2)
Definition: testNumericalDerivative.cpp:76
smallExample.h
Create small example with two poses and one landmark.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:169
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
TEST
TEST(GaussianFactorGraph, equals)
Definition: testGaussianFactorGraphB.cpp:44
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
copy
int EIGEN_BLAS_FUNC() copy(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_impl.h:29
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::example::createSingleConstraintGraph
GaussianFactorGraph createSingleConstraintGraph()
Definition: smallExample.h:512
X
#define X
Definition: icosphere.cpp:20
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
Pose3.h
3D Pose
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Definition: GaussianFactorGraph.cpp:442
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
fov
static double fov
Definition: testProjectionFactor.cpp:33
error
double error(const VectorValues &x)
Definition: testGaussianFactorGraphB.cpp:252
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741

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