smallExample.h
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 
19 // \callgraph
20 
21 
22 #pragma once
23 
24 #include <tests/simulated2D.h>
25 #include <gtsam/inference/Symbol.h>
29 
30 namespace gtsam {
31 namespace example {
32 
36 // inline std::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph();
37 // inline NonlinearFactorGraph createNonlinearFactorGraph();
38 
43 // inline Values createValues();
44 
46 // inline VectorValues createVectorValues();
47 
51 // inline std::shared_ptr<const Values> sharedNoisyValues();
52 // inline Values createNoisyValues();
53 
57 // inline VectorValues createZeroDelta();
58 
62 // inline VectorValues createCorrectDelta();
63 
68 // inline GaussianFactorGraph createGaussianFactorGraph();
69 
73 // inline GaussianBayesNet createSmallGaussianBayesNet();
74 
78 // inline std::shared_ptr<const NonlinearFactorGraph>
79 //sharedReallyNonlinearFactorGraph();
80 // inline NonlinearFactorGraph createReallyNonlinearFactorGraph();
81 
86 // inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T);
87 
92 // inline GaussianFactorGraph createSmoother(int T);
93 
94 /* ******************************************************* */
95 // Linear Constrained Examples
96 /* ******************************************************* */
97 
102 // inline GaussianFactorGraph createSimpleConstraintGraph();
103 // inline VectorValues createSimpleConstraintValues();
104 
109 // inline GaussianFactorGraph createSingleConstraintGraph();
110 // inline VectorValues createSingleConstraintValues();
111 
116 // inline GaussianFactorGraph createMultiConstraintGraph();
117 // inline VectorValues createMultiConstraintValues();
118 
119 /* ******************************************************* */
120 // Planar graph with easy subtree for SubgraphPreconditioner
121 /* ******************************************************* */
122 
123 /*
124  * Create factor graph with N^2 nodes, for example for N=3
125  * x13-x23-x33
126  * | | |
127  * x12-x22-x32
128  * | | |
129  * -x11-x21-x31
130  * with x11 clamped at (1,1), and others related by 2D odometry.
131  */
132 // inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N);
133 
134 /*
135  * Create canonical ordering for planar graph that also works for tree
136  * With x11 the root, e.g. for N=3
137  * x33 x23 x13 x32 x22 x12 x31 x21 x11
138  */
139 // inline Ordering planarOrdering(size_t N);
140 
141 /*
142  * Split graph into tree and loop closing constraints, e.g., with N=3
143  * x13-x23-x33
144  * |
145  * x12-x22-x32
146  * |
147  * -x11-x21-x31
148  */
149 // inline std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
150 // size_t N, const GaussianFactorGraph& original);
151 
152 
153 
154 // Implementations
155 
156 // using namespace gtsam::noiseModel;
157 
158 namespace impl {
159 typedef std::shared_ptr<NonlinearFactor> shared_nlf;
160 
165 
166 static const Key _l1_=0, _x1_=1, _x2_=2;
167 static const Key _x_=0, _y_=1, _z_=2;
168 } // \namespace impl
169 
170 
171 /* ************************************************************************* */
172 inline std::shared_ptr<const NonlinearFactorGraph>
174  const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
175  using namespace impl;
176  using symbol_shorthand::L;
177  using symbol_shorthand::X;
178  // Create
179  std::shared_ptr<NonlinearFactorGraph> nlfg(new NonlinearFactorGraph);
180 
181  // prior on x1
182  Point2 mu(0, 0);
183  shared_nlf f1(new simulated2D::Prior(mu, noiseModel1, X(1)));
184  nlfg->push_back(f1);
185 
186  // odometry between x1 and x2
187  Point2 z2(1.5, 0);
188  shared_nlf f2(new simulated2D::Odometry(z2, noiseModel1, X(1), X(2)));
189  nlfg->push_back(f2);
190 
191  // measurement between x1 and l1
192  Point2 z3(0, -1);
193  shared_nlf f3(new simulated2D::Measurement(z3, noiseModel2, X(1), L(1)));
194  nlfg->push_back(f3);
195 
196  // measurement between x2 and l1
197  Point2 z4(-1.5, -1.);
198  shared_nlf f4(new simulated2D::Measurement(z4, noiseModel2, X(2), L(1)));
199  nlfg->push_back(f4);
200 
201  return nlfg;
202 }
203 
204 /* ************************************************************************* */
207  const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
208  return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2);
209 }
210 
211 /* ************************************************************************* */
213  using symbol_shorthand::X;
214  using symbol_shorthand::L;
215  Values c;
216  c.insert(X(1), Point2(0.0, 0.0));
217  c.insert(X(2), Point2(1.5, 0.0));
218  c.insert(L(1), Point2(0.0, -1.0));
219  return c;
220 }
221 
222 /* ************************************************************************* */
224  using namespace impl;
225  VectorValues c {{_l1_, Vector2(0.0, -1.0)},
226  {_x1_, Vector2(0.0, 0.0)},
227  {_x2_, Vector2(1.5, 0.0)}};
228  return c;
229 }
230 
231 /* ************************************************************************* */
232 inline std::shared_ptr<const Values> sharedNoisyValues() {
233  using symbol_shorthand::X;
234  using symbol_shorthand::L;
235  std::shared_ptr<Values> c(new Values);
236  c->insert(X(1), Point2(0.1, 0.1));
237  c->insert(X(2), Point2(1.4, 0.2));
238  c->insert(L(1), Point2(0.1, -1.1));
239  return c;
240 }
241 
242 /* ************************************************************************* */
244  return *sharedNoisyValues();
245 }
246 
247 /* ************************************************************************* */
249  using symbol_shorthand::X;
250  using symbol_shorthand::L;
251  VectorValues c;
252  c.insert(L(1), Vector2(-0.1, 0.1));
253  c.insert(X(1), Vector2(-0.1, -0.1));
254  c.insert(X(2), Vector2(0.1, -0.2));
255  return c;
256 }
257 
258 /* ************************************************************************* */
260  using symbol_shorthand::X;
261  using symbol_shorthand::L;
262  VectorValues c;
263  c.insert(L(1), Z_2x1);
264  c.insert(X(1), Z_2x1);
265  c.insert(X(2), Z_2x1);
266  return c;
267 }
268 
269 /* ************************************************************************* */
271  using symbol_shorthand::X;
272  using symbol_shorthand::L;
273  // Create empty graph
275 
276  // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
277  fg.emplace_shared<JacobianFactor>(X(1), 10*I_2x2, -1.0*Vector::Ones(2));
278 
279  // odometry between x1 and x2: x2-x1=[0.2;-0.1]
280  fg.emplace_shared<JacobianFactor>(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0));
281 
282  // measurement between x1 and l1: l1-x1=[0.0;0.2]
283  fg.emplace_shared<JacobianFactor>(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0));
284 
285  // measurement between x2 and l1: l1-x2=[-0.2;0.3]
286  fg.emplace_shared<JacobianFactor>(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5));
287 
288  return fg;
289 }
290 
291 /* ************************************************************************* */
298  using namespace impl;
299  Matrix R11 = (Matrix(1, 1) << 1.0).finished(), S12 = (Matrix(1, 1) << 1.0).finished();
300  Matrix R22 = (Matrix(1, 1) << 1.0).finished();
301  Vector d1(1), d2(1);
302  d1(0) = 9;
303  d2(0) = 5;
304 
305  // define nodes and specify in reverse topological sort (i.e. parents last)
308  GaussianBayesNet cbn;
309  cbn.push_back(Px_y);
310  cbn.push_back(Py);
311 
312  return cbn;
313 }
314 
315 /* ************************************************************************* */
316 // Some nonlinear functions to optimize
317 /* ************************************************************************* */
318 namespace smallOptimize {
319 
320 inline Point2 h(const Point2& v) {
321  return Point2(cos(v.x()), sin(v.y()));
322 }
323 
324 inline Matrix H(const Point2& v) {
325  return (Matrix(2, 2) <<
326  -sin(v.x()), 0.0,
327  0.0, cos(v.y())).finished();
328 }
329 
330 struct UnaryFactor: public gtsam::NoiseModelFactorN<Point2> {
331 
332  // Provide access to the Matrix& version of evaluateError:
333  using gtsam::NoiseModelFactor1<Point2>::evaluateError;
334 
336 
339  }
340 
341  Vector evaluateError(const Point2& x, OptionalMatrixType A) const override {
342  if (A) *A = H(x);
343  return (h(x) - z_);
344  }
345 
347  return std::static_pointer_cast<gtsam::NonlinearFactor>(
349 };
350 
351 }
352 
353 /* ************************************************************************* */
355  using symbol_shorthand::X;
356  using symbol_shorthand::L;
357  std::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
358  Point2 z(1.0, 0.0);
359  std::shared_ptr<smallOptimize::UnaryFactor> factor(
361  fg->push_back(factor);
362  return *fg;
363 }
364 
365 /* ************************************************************************* */
366 inline std::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
367  using symbol_shorthand::X;
368  using symbol_shorthand::L;
369  std::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
370  Point2 z(1.0, 0.0);
371  double sigma = 0.1;
372  std::shared_ptr<smallOptimize::UnaryFactor> factor(
374  fg->push_back(factor);
375  return fg;
376 }
377 
380 }
381 
382 /* ************************************************************************* */
384  using symbol_shorthand::X;
385  std::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
386  Point2 z(0.0, 0.0);
387  double sigma = 0.1;
388 
389  std::shared_ptr<PriorFactor<Point2>> factor(
391  // 3 noiseless inliers
392  fg->push_back(factor);
393  fg->push_back(factor);
394  fg->push_back(factor);
395 
396  // 1 outlier
397  Point2 z_out(1.0, 0.0);
398  std::shared_ptr<PriorFactor<Point2>> factor_out(
400  fg->push_back(factor_out);
401 
402  return *fg;
403 }
404 
405 /* ************************************************************************* */
407  using symbol_shorthand::X;
408  std::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
409  Point2 z(0.0, 0.0);
410  double sigma = 0.1;
411  auto gmNoise = noiseModel::Robust::Create(
413  std::shared_ptr<PriorFactor<Point2>> factor(
414  new PriorFactor<Point2>(X(1), z, gmNoise));
415  // 3 noiseless inliers
416  fg->push_back(factor);
417  fg->push_back(factor);
418  fg->push_back(factor);
419 
420  // 1 outlier
421  Point2 z_out(1.0, 0.0);
422  std::shared_ptr<PriorFactor<Point2>> factor_out(
423  new PriorFactor<Point2>(X(1), z_out, gmNoise));
424  fg->push_back(factor_out);
425 
426  return *fg;
427 }
428 
429 
430 /* ************************************************************************* */
431 inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
432  using namespace impl;
433  using symbol_shorthand::X;
434  using symbol_shorthand::L;
435 
436  // Create
438  Values poses;
439 
440  // prior on x1
441  Point2 x1(1.0, 0.0);
443  nlfg.push_back(prior);
444  poses.insert(X(1), x1);
445 
446  for (int t = 2; t <= T; t++) {
447  // odometry between x_t and x_{t-1}
448  Point2 odo(1.0, 0.0);
450  nlfg.push_back(odometry);
451 
452  // measurement on x_t is like perfect GPS
453  Point2 xt(t, 0);
455  nlfg.push_back(measurement);
456 
457  // initial estimate
458  poses.insert(X(t), xt);
459  }
460 
461  return std::make_pair(nlfg, poses);
462 }
463 
464 /* ************************************************************************* */
466  const auto [nlfg, poses] = createNonlinearSmoother(T);
467  return *nlfg.linearize(poses);
468 }
469 
470 /* ************************************************************************* */
472  using namespace impl;
473  // create unary factor
474  // prior on _x_, mean = [1,-1], sigma=0.1
475  Matrix Ax = I_2x2;
476  Vector b1(2);
477  b1(0) = 1.0;
478  b1(1) = -1.0;
480 
481  // create binary constraint factor
482  // between _x_ and _y_, that is going to be the only factor on _y_
483  // |1 0||x_1| + |-1 0||y_1| = |0|
484  // |0 1||x_2| | 0 -1||y_2| |0|
485  Matrix Ax1 = I_2x2;
486  Matrix Ay1 = I_2x2 * -1;
487  Vector b2 = Vector2(0.0, 0.0);
490 
491  // construct the graph
493  fg.push_back(f1);
494  fg.push_back(f2);
495 
496  return fg;
497 }
498 
499 /* ************************************************************************* */
501  using namespace impl;
502  using symbol_shorthand::X;
503  using symbol_shorthand::L;
504  VectorValues config;
505  Vector v = Vector2(1.0, -1.0);
506  config.insert(_x_, v);
507  config.insert(_y_, v);
508  return config;
509 }
510 
511 /* ************************************************************************* */
513  using namespace impl;
514  // create unary factor
515  // prior on _x_, mean = [1,-1], sigma=0.1
516  Matrix Ax = I_2x2;
517  Vector b1(2);
518  b1(0) = 1.0;
519  b1(1) = -1.0;
520  //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, kSigma0_1->Whiten(Ax), kSigma0_1->whiten(b1), kSigma0_1));
522 
523  // create binary constraint factor
524  // between _x_ and _y_, that is going to be the only factor on _y_
525  // |1 2||x_1| + |10 0||y_1| = |1|
526  // |2 1||x_2| |0 10||y_2| |2|
527  Matrix Ax1(2, 2);
528  Ax1(0, 0) = 1.0;
529  Ax1(0, 1) = 2.0;
530  Ax1(1, 0) = 2.0;
531  Ax1(1, 1) = 1.0;
532  Matrix Ay1 = I_2x2 * 10;
533  Vector b2 = Vector2(1.0, 2.0);
536 
537  // construct the graph
539  fg.push_back(f1);
540  fg.push_back(f2);
541 
542  return fg;
543 }
544 
545 /* ************************************************************************* */
547  using namespace impl;
548  VectorValues config{{_x_, Vector2(1.0, -1.0)}, {_y_, Vector2(0.2, 0.1)}};
549  return config;
550 }
551 
552 /* ************************************************************************* */
554  using namespace impl;
555  // unary factor 1
556  Matrix A = I_2x2;
557  Vector b = Vector2(-2.0, 2.0);
559 
560  // constraint 1
561  Matrix A11(2, 2);
562  A11(0, 0) = 1.0;
563  A11(0, 1) = 2.0;
564  A11(1, 0) = 2.0;
565  A11(1, 1) = 1.0;
566 
567  Matrix A12(2, 2);
568  A12(0, 0) = 10.0;
569  A12(0, 1) = 0.0;
570  A12(1, 0) = 0.0;
571  A12(1, 1) = 10.0;
572 
573  Vector b1(2);
574  b1(0) = 1.0;
575  b1(1) = 2.0;
578 
579  // constraint 2
580  Matrix A21(2, 2);
581  A21(0, 0) = 3.0;
582  A21(0, 1) = 4.0;
583  A21(1, 0) = -1.0;
584  A21(1, 1) = -2.0;
585 
586  Matrix A22(2, 2);
587  A22(0, 0) = 1.0;
588  A22(0, 1) = 1.0;
589  A22(1, 0) = 1.0;
590  A22(1, 1) = 2.0;
591 
592  Vector b2(2);
593  b2(0) = 3.0;
594  b2(1) = 4.0;
597 
598  // construct the graph
600  fg.push_back(lf1);
601  fg.push_back(lc1);
602  fg.push_back(lc2);
603 
604  return fg;
605 }
606 
607 /* ************************************************************************* */
609  using namespace impl;
610  VectorValues config{{_x_, Vector2(-2.0, 2.0)},
611  {_y_, Vector2(-0.1, 0.4)},
612  {_z_, Vector2(-4.0, 5.0)}};
613  return config;
614 }
615 
616 /* ************************************************************************* */
617 // Create key for simulated planar graph
618 namespace impl {
619 inline Symbol key(size_t x, size_t y) {
620  using symbol_shorthand::X;
621  return X(1000*x+y);
622 }
623 } // \namespace impl
624 
625 /* ************************************************************************* */
626 inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
627  using namespace impl;
628 
629  // create empty graph
631 
632  // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
633  shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1)));
634  nlfg.push_back(constraint);
635 
636  // Create horizontal constraints, 1...N*(N-1)
637  Point2 z1(1.0, 0.0); // move right
638  for (size_t x = 1; x < N; x++)
639  for (size_t y = 1; y <= N; y++) {
641  nlfg.push_back(f);
642  }
643 
644  // Create vertical constraints, N*(N-1)+1..2*N*(N-1)
645  Point2 z2(0.0, 1.0); // move up
646  for (size_t x = 1; x <= N; x++)
647  for (size_t y = 1; y < N; y++) {
649  nlfg.push_back(f);
650  }
651 
652  // Create linearization and ground xtrue config
653  Values zeros;
654  for (size_t x = 1; x <= N; x++)
655  for (size_t y = 1; y <= N; y++)
656  zeros.insert(key(x, y), Point2(0,0));
657  VectorValues xtrue;
658  for (size_t x = 1; x <= N; x++)
659  for (size_t y = 1; y <= N; y++)
660  xtrue.insert(key(x, y), Point2((double)x, (double)y));
661 
662  // linearize around zero
663  std::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);
664  return std::make_pair(*gfg, xtrue);
665 }
666 
667 /* ************************************************************************* */
668 inline Ordering planarOrdering(size_t N) {
670  for (size_t y = N; y >= 1; y--)
671  for (size_t x = N; x >= 1; x--)
672  ordering.push_back(impl::key(x, y));
673  return ordering;
674 }
675 
676 /* ************************************************************************* */
677 inline std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(
678  size_t N, const GaussianFactorGraph& original) {
680 
681  // Add the x11 constraint to the tree
682  T.push_back(original[0]);
683 
684  // Add all horizontal constraints to the tree
685  size_t i = 1;
686  for (size_t x = 1; x < N; x++)
687  for (size_t y = 1; y <= N; y++, i++) T.push_back(original[i]);
688 
689  // Add first vertical column of constraints to T, others to C
690  for (size_t x = 1; x <= N; x++)
691  for (size_t y = 1; y < N; y++, i++)
692  if (x == 1)
693  T.push_back(original[i]);
694  else
695  C.push_back(original[i]);
696 
697  return std::make_pair(T, C);
698 }
699 
700 /* ************************************************************************* */
701 
702 } // \namespace example
703 } // \namespace gtsam
gtsam::example::createNoisyValues
Values createNoisyValues()
Definition: smallExample.h:243
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::example::sharedNonlinearFactorGraph
std::shared_ptr< const NonlinearFactorGraph > sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:173
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::example::impl::shared_nlf
std::shared_ptr< NonlinearFactor > shared_nlf
Definition: smallExample.h:159
simple_graph::b1
Vector2 b1(2, -1)
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
gtsam::example::smallOptimize::H
Matrix H(const Point2 &v)
Definition: smallExample.h:324
gtsam::example::createMultiConstraintGraph
GaussianFactorGraph createMultiConstraintGraph()
Definition: smallExample.h:553
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
simulated2D::GenericOdometry
Definition: simulated2D.h:178
gtsam::example::nonlinearFactorGraphWithGivenSigma
NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma)
Definition: smallExample.h:354
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
z1
Point2 z1
Definition: testTriangulationFactor.cpp:45
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
gtsam::example::impl::_y_
static const Key _y_
Definition: smallExample.h:167
simple_graph::b2
Vector2 b2(4, -5)
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::example::impl::_x2_
static const Key _x2_
Definition: smallExample.h:166
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::example::sharedReallyNonlinearFactorGraph
std::shared_ptr< const NonlinearFactorGraph > sharedReallyNonlinearFactorGraph()
Definition: smallExample.h:366
gtsam::example::createMultiConstraintValues
VectorValues createMultiConstraintValues()
Definition: smallExample.h:608
shared_nlf
std::shared_ptr< NonlinearFactor > shared_nlf
Definition: testNonlinearFactor.cpp:44
gtsam::example::smallOptimize::UnaryFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: smallExample.h:346
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::example::createVectorValues
VectorValues createVectorValues()
Definition: smallExample.h:223
X
#define X
Definition: icosphere.cpp:20
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::example::smallOptimize::UnaryFactor::evaluateError
Vector evaluateError(const Point2 &x, OptionalMatrixType A) const override
Definition: smallExample.h:341
gtsam::example::splitOffPlanarTree
std::pair< GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N, const GaussianFactorGraph &original)
Definition: smallExample.h:677
gtsam::example::smallOptimize::UnaryFactor
Definition: smallExample.h:330
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::example::smallOptimize::UnaryFactor::z_
Point2 z_
Definition: smallExample.h:335
gtsam::example::impl::_x1_
static const Key _x1_
Definition: smallExample.h:166
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
gtsam::Measurement
This is the base class for all measurement types.
Definition: Measurement.h:11
gtsam::example::impl::_x_
static const Key _x_
Definition: smallExample.h:167
odometry
Pose2 odometry(2.0, 0.0, 0.0)
example
Definition: testOrdering.cpp:28
gtsam::example::impl::key
Symbol key(size_t x, size_t y)
Definition: smallExample.h:619
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
gtsam::example::createNonlinearFactorGraph
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Definition: smallExample.h:206
gtsam::example::impl::kSigma0_2
static SharedDiagonal kSigma0_2
Definition: smallExample.h:163
z3
static const Unit3 z3
Definition: testRotateFactor.cpp:44
A
Definition: test_numpy_dtypes.cpp:298
gtsam::VectorValues
Definition: VectorValues.h:74
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::example::createGaussianFactorGraph
GaussianFactorGraph createGaussianFactorGraph()
Definition: smallExample.h:270
gtsam::example::createSmallGaussianBayesNet
GaussianBayesNet createSmallGaussianBayesNet()
Definition: smallExample.h:297
gtsam::example::createSimpleConstraintGraph
GaussianFactorGraph createSimpleConstraintGraph()
Definition: smallExample.h:471
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::example::smallOptimize::h
Point2 h(const Point2 &v)
Definition: smallExample.h:320
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Definition: NoiseModel.h:480
gtsam::example::sharedNoisyValues
std::shared_ptr< const Values > sharedNoisyValues()
Definition: smallExample.h:232
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
gtsam::symbol_shorthand::L
Key L(std::uint64_t j)
Definition: inference/Symbol.h:159
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::example::planarOrdering
Ordering planarOrdering(size_t N)
Definition: smallExample.h:668
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Eigen::Triplet< double >
A12
static const double A12[]
Definition: expn.h:17
gtsam::example::createNonlinearSmoother
std::pair< NonlinearFactorGraph, Values > createNonlinearSmoother(int T)
Definition: smallExample.h:431
simple_graph::A21
Matrix A21
Definition: testJacobianFactor.cpp:200
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
ordering
static enum @1096 ordering
simulated2D.h
measurement functions and derivatives for simulated 2D robot
y
Scalar * y
Definition: level1_cplx_impl.h:124
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
test_docs.d2
d2
Definition: test_docs.py:29
gtsam::example::createSingleConstraintGraph
GaussianFactorGraph createSingleConstraintGraph()
Definition: smallExample.h:512
gtsam::b
const G & b
Definition: Group.h:79
gtsam::example::planarGraph
std::pair< GaussianFactorGraph, VectorValues > planarGraph(size_t N)
Definition: smallExample.h:626
gtsam::example::impl::kSigma0_1
static SharedDiagonal kSigma0_1
Definition: smallExample.h:162
simulated2D::GenericPrior
Definition: simulated2D.h:129
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
gtsam::example::impl::kConstrainedModel
static SharedDiagonal kConstrainedModel
Definition: smallExample.h:164
gtsam::example::impl::kSigma1_0
static SharedDiagonal kSigma1_0
Definition: smallExample.h:161
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
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::Values
Definition: Values.h:65
gtsam::example::sharedNonRobustFactorGraphWithOutliers
NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers()
Definition: smallExample.h:383
gtsam::example::createSimpleConstraintValues
VectorValues createSimpleConstraintValues()
Definition: smallExample.h:500
gtsam::example::createSingleConstraintValues
VectorValues createSingleConstraintValues()
Definition: smallExample.h:546
simulated2D::odo
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
Definition: simulated2D.h:99
gtsam::example::sharedRobustFactorGraphWithOutliers
NonlinearFactorGraph sharedRobustFactorGraphWithOutliers()
Definition: smallExample.h:406
simple_graph::A22
Matrix A22
Definition: testJacobianFactor.cpp:201
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::example::smallOptimize::UnaryFactor::UnaryFactor
UnaryFactor(const Point2 &z, const SharedNoiseModel &model, Key key)
Definition: smallExample.h:337
f4
double f4(double x, double y, double z)
Definition: testNumericalDerivative.cpp:105
A11
static const double A11[]
Definition: expn.h:16
gtsam::example::createValues
Values createValues()
Definition: smallExample.h:212
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::example::createReallyNonlinearFactorGraph
NonlinearFactorGraph createReallyNonlinearFactorGraph()
Definition: smallExample.h:378
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:238
gtsam::example::impl::_l1_
static const Key _l1_
Definition: smallExample.h:166
N
#define N
Definition: igam.h:9
gtsam::NoiseModelFactorN< Point2 >::key
Key key() const
Definition: NonlinearFactor.h:582
gtsam::example::createZeroDelta
VectorValues createZeroDelta()
Definition: smallExample.h:259
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::noiseModel::mEstimator::GemanMcClure::Create
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Definition: LossFunctions.cpp:343
gtsam::example::createCorrectDelta
VectorValues createCorrectDelta()
Definition: smallExample.h:248
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::example::impl::_z_
static const Key _z_
Definition: smallExample.h:167
measurement
static Point2 measurement(323.0, 240.0)
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
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:153
z2
static const Unit3 z2
Definition: testRotateFactor.cpp:43
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:21