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

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