1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file smallExample.h
14 * @brief Create small example with two poses and one landmark
15 * @brief smallExample
16 * @author Carlos Nieto
17 */
18
19// \callgraph
20
21
22#pragma once
23
24#include <tests/simulated2D.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/nonlinear/NonlinearFactorGraph.h>
27#include <gtsam/linear/GaussianBayesNet.h>
28#include <gtsam/linear/GaussianFactorGraph.h>
29
30namespace gtsam {
31namespace example {
32
33/**
34 * Create small example for non-linear factor graph
35 */
36// inline std::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph();
37// inline NonlinearFactorGraph createNonlinearFactorGraph();
38
39/**
40 * Create values structure to go with it
41 * The ground truth values structure for the example above
42 */
43// inline Values createValues();
44
45/** Vector Values equivalent */
46// inline VectorValues createVectorValues();
47
48/**
49 * create a noisy values structure for a nonlinear factor graph
50 */
51// inline std::shared_ptr<const Values> sharedNoisyValues();
52// inline Values createNoisyValues();
53
54/**
55 * Zero delta config
56 */
57// inline VectorValues createZeroDelta();
58
59/**
60 * Delta config that, when added to noisyValues, returns the ground truth
61 */
62// inline VectorValues createCorrectDelta();
63
64/**
65 * create a linear factor graph
66 * The non-linear graph above evaluated at NoisyValues
67 */
68// inline GaussianFactorGraph createGaussianFactorGraph();
69
70/**
71 * create small Chordal Bayes Net x <- y
72 */
73// inline GaussianBayesNet createSmallGaussianBayesNet();
74
75/**
76 * Create really non-linear factor graph (cos/sin)
77 */
78// inline std::shared_ptr<const NonlinearFactorGraph>
79//sharedReallyNonlinearFactorGraph();
80// inline NonlinearFactorGraph createReallyNonlinearFactorGraph();
81
82/**
83 * Create a full nonlinear smoother
84 * @param T number of time-steps
85 */
86// inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T);
87
88/**
89 * Create a Kalman smoother by linearizing a non-linear factor graph
90 * @param T number of time-steps
91 */
92// inline GaussianFactorGraph createSmoother(int T);
93
94/* ******************************************************* */
95// Linear Constrained Examples
96/* ******************************************************* */
97
98/**
99 * Creates a simple constrained graph with one linear factor and
100 * one binary equality constraint that sets x = y
101 */
102// inline GaussianFactorGraph createSimpleConstraintGraph();
103// inline VectorValues createSimpleConstraintValues();
104
105/**
106 * Creates a simple constrained graph with one linear factor and
107 * one binary constraint.
108 */
109// inline GaussianFactorGraph createSingleConstraintGraph();
110// inline VectorValues createSingleConstraintValues();
111
112/**
113 * Creates a constrained graph with a linear factor and two
114 * binary constraints that share a node
115 */
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
158namespace impl {
159typedef std::shared_ptr<NonlinearFactor> shared_nlf;
160
161static SharedDiagonal kSigma1_0 = noiseModel::Isotropic::Sigma(dim: 2,sigma: 1.0);
162static SharedDiagonal kSigma0_1 = noiseModel::Isotropic::Sigma(dim: 2,sigma: 0.1);
163static SharedDiagonal kSigma0_2 = noiseModel::Isotropic::Sigma(dim: 2,sigma: 0.2);
164static SharedDiagonal kConstrainedModel = noiseModel::Constrained::All(dim: 2);
165
166static const Key _l1_=0, _x1_=1, _x2_=2;
167static const Key _x_=0, _y_=1, _z_=2;
168} // \namespace impl
169
170
171/* ************************************************************************* */
172inline std::shared_ptr<const NonlinearFactorGraph>
173sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1,
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(j: 1)));
184 nlfg->push_back(factor: f1);
185
186 // odometry between x1 and x2
187 Point2 z2(1.5, 0);
188 shared_nlf f2(new simulated2D::Odometry(z2, noiseModel1, X(j: 1), X(j: 2)));
189 nlfg->push_back(factor: f2);
190
191 // measurement between x1 and l1
192 Point2 z3(0, -1);
193 shared_nlf f3(new simulated2D::Measurement(z3, noiseModel2, X(j: 1), L(j: 1)));
194 nlfg->push_back(factor: f3);
195
196 // measurement between x2 and l1
197 Point2 z4(-1.5, -1.);
198 shared_nlf f4(new simulated2D::Measurement(z4, noiseModel2, X(j: 2), L(j: 1)));
199 nlfg->push_back(factor: f4);
200
201 return nlfg;
202}
203
204/* ************************************************************************* */
205inline NonlinearFactorGraph
206createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1,
207 const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
208 return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2);
209}
210
211/* ************************************************************************* */
212inline Values createValues() {
213 using symbol_shorthand::X;
214 using symbol_shorthand::L;
215 Values c;
216 c.insert(j: X(j: 1), val: Point2(0.0, 0.0));
217 c.insert(j: X(j: 2), val: Point2(1.5, 0.0));
218 c.insert(j: L(j: 1), val: Point2(0.0, -1.0));
219 return c;
220}
221
222/* ************************************************************************* */
223inline VectorValues createVectorValues() {
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/* ************************************************************************* */
232inline 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(j: X(j: 1), val: Point2(0.1, 0.1));
237 c->insert(j: X(j: 2), val: Point2(1.4, 0.2));
238 c->insert(j: L(j: 1), val: Point2(0.1, -1.1));
239 return c;
240}
241
242/* ************************************************************************* */
243inline Values createNoisyValues() {
244 return *sharedNoisyValues();
245}
246
247/* ************************************************************************* */
248inline VectorValues createCorrectDelta() {
249 using symbol_shorthand::X;
250 using symbol_shorthand::L;
251 VectorValues c;
252 c.insert(j: L(j: 1), value: Vector2(-0.1, 0.1));
253 c.insert(j: X(j: 1), value: Vector2(-0.1, -0.1));
254 c.insert(j: X(j: 2), value: Vector2(0.1, -0.2));
255 return c;
256}
257
258/* ************************************************************************* */
259inline VectorValues createZeroDelta() {
260 using symbol_shorthand::X;
261 using symbol_shorthand::L;
262 VectorValues c;
263 c.insert(j: L(j: 1), value: Z_2x1);
264 c.insert(j: X(j: 1), value: Z_2x1);
265 c.insert(j: X(j: 2), value: Z_2x1);
266 return c;
267}
268
269/* ************************************************************************* */
270inline GaussianFactorGraph createGaussianFactorGraph() {
271 using symbol_shorthand::X;
272 using symbol_shorthand::L;
273 // Create empty graph
274 GaussianFactorGraph fg;
275
276 // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
277 fg.emplace_shared<JacobianFactor>(args: X(j: 1), args: 10*I_2x2, args: -1.0*Vector::Ones(newSize: 2));
278
279 // odometry between x1 and x2: x2-x1=[0.2;-0.1]
280 fg.emplace_shared<JacobianFactor>(args: X(j: 1), args: -10*I_2x2, args: X(j: 2), args: 10*I_2x2, args: Vector2(2.0, -1.0));
281
282 // measurement between x1 and l1: l1-x1=[0.0;0.2]
283 fg.emplace_shared<JacobianFactor>(args: X(j: 1), args: -5*I_2x2, args: L(j: 1), args: 5*I_2x2, args: Vector2(0.0, 1.0));
284
285 // measurement between x2 and l1: l1-x2=[-0.2;0.3]
286 fg.emplace_shared<JacobianFactor>(args: X(j: 2), args: -5*I_2x2, args: L(j: 1), args: 5*I_2x2, args: Vector2(-1.0, 1.5));
287
288 return fg;
289}
290
291/* ************************************************************************* */
292/** create small Chordal Bayes Net x <- y
293 * x y d
294 * 1 1 9
295 * 1 5
296 */
297inline GaussianBayesNet createSmallGaussianBayesNet() {
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)
306 GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12));
307 GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22));
308 GaussianBayesNet cbn;
309 cbn.push_back(factor: Px_y);
310 cbn.push_back(factor: Py);
311
312 return cbn;
313}
314
315/* ************************************************************************* */
316// Some nonlinear functions to optimize
317/* ************************************************************************* */
318namespace smallOptimize {
319
320inline Point2 h(const Point2& v) {
321 return Point2(cos(x: v.x()), sin(x: v.y()));
322}
323
324inline Matrix H(const Point2& v) {
325 return (Matrix(2, 2) <<
326 -sin(x: v.x()), 0.0,
327 0.0, cos(x: v.y())).finished();
328}
329
330struct UnaryFactor: public gtsam::NoiseModelFactorN<Point2> {
331
332 // Provide access to the Matrix& version of evaluateError:
333 using gtsam::NoiseModelFactor1<Point2>::evaluateError;
334
335 Point2 z_;
336
337 UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
338 gtsam::NoiseModelFactorN<Point2>(model, key), z_(z) {
339 }
340
341 Vector evaluateError(const Point2& x, OptionalMatrixType A) const override {
342 if (A) *A = H(v: x);
343 return (h(v: x) - z_);
344 }
345
346 gtsam::NonlinearFactor::shared_ptr clone() const override {
347 return std::static_pointer_cast<gtsam::NonlinearFactor>(
348 r: gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
349};
350
351}
352
353/* ************************************************************************* */
354inline NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma) {
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(
360 new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(dim: 2,sigma), X(j: 1)));
361 fg->push_back(factor);
362 return *fg;
363}
364
365/* ************************************************************************* */
366inline 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(
373 new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(dim: 2,sigma), X(j: 1)));
374 fg->push_back(factor);
375 return fg;
376}
377
378inline NonlinearFactorGraph createReallyNonlinearFactorGraph() {
379 return *sharedReallyNonlinearFactorGraph();
380}
381
382/* ************************************************************************* */
383inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() {
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(
390 new PriorFactor<Point2>(X(j: 1), z, noiseModel::Isotropic::Sigma(dim: 2,sigma)));
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(
399 new PriorFactor<Point2>(X(j: 1), z_out, noiseModel::Isotropic::Sigma(dim: 2,sigma)));
400 fg->push_back(factor: factor_out);
401
402 return *fg;
403}
404
405/* ************************************************************************* */
406inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() {
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(
412 robust: noiseModel::mEstimator::GemanMcClure::Create(k: 1.0), noise: noiseModel::Isotropic::Sigma(dim: 2,sigma));
413 std::shared_ptr<PriorFactor<Point2>> factor(
414 new PriorFactor<Point2>(X(j: 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(j: 1), z_out, gmNoise));
424 fg->push_back(factor: factor_out);
425
426 return *fg;
427}
428
429
430/* ************************************************************************* */
431inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
432 using namespace impl;
433 using symbol_shorthand::X;
434 using symbol_shorthand::L;
435
436 // Create
437 NonlinearFactorGraph nlfg;
438 Values poses;
439
440 // prior on x1
441 Point2 x1(1.0, 0.0);
442 shared_nlf prior(new simulated2D::Prior(x1, kSigma1_0, X(j: 1)));
443 nlfg.push_back(factor: prior);
444 poses.insert(j: X(j: 1), val: 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);
449 shared_nlf odometry(new simulated2D::Odometry(odo, kSigma1_0, X(j: t - 1), X(j: t)));
450 nlfg.push_back(factor: odometry);
451
452 // measurement on x_t is like perfect GPS
453 Point2 xt(t, 0);
454 shared_nlf measurement(new simulated2D::Prior(xt, kSigma1_0, X(j: t)));
455 nlfg.push_back(factor: measurement);
456
457 // initial estimate
458 poses.insert(j: X(j: t), val: xt);
459 }
460
461 return std::make_pair(x&: nlfg, y&: poses);
462}
463
464/* ************************************************************************* */
465inline GaussianFactorGraph createSmoother(int T) {
466 const auto [nlfg, poses] = createNonlinearSmoother(T);
467 return *nlfg.linearize(linearizationPoint: poses);
468}
469
470/* ************************************************************************* */
471inline GaussianFactorGraph createSimpleConstraintGraph() {
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;
479 JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1));
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);
488 JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
489 kConstrainedModel));
490
491 // construct the graph
492 GaussianFactorGraph fg;
493 fg.push_back(factor: f1);
494 fg.push_back(factor: f2);
495
496 return fg;
497}
498
499/* ************************************************************************* */
500inline VectorValues createSimpleConstraintValues() {
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(j: _x_, value: v);
507 config.insert(j: _y_, value: v);
508 return config;
509}
510
511/* ************************************************************************* */
512inline GaussianFactorGraph createSingleConstraintGraph() {
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));
521 JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, 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);
534 JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
535 kConstrainedModel));
536
537 // construct the graph
538 GaussianFactorGraph fg;
539 fg.push_back(factor: f1);
540 fg.push_back(factor: f2);
541
542 return fg;
543}
544
545/* ************************************************************************* */
546inline VectorValues createSingleConstraintValues() {
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/* ************************************************************************* */
553inline GaussianFactorGraph createMultiConstraintGraph() {
554 using namespace impl;
555 // unary factor 1
556 Matrix A = I_2x2;
557 Vector b = Vector2(-2.0, 2.0);
558 JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, kSigma0_1));
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;
576 JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
577 kConstrainedModel));
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;
595 JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
596 kConstrainedModel));
597
598 // construct the graph
599 GaussianFactorGraph fg;
600 fg.push_back(factor: lf1);
601 fg.push_back(factor: lc1);
602 fg.push_back(factor: lc2);
603
604 return fg;
605}
606
607/* ************************************************************************* */
608inline VectorValues createMultiConstraintValues() {
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
618namespace impl {
619inline Symbol key(size_t x, size_t y) {
620 using symbol_shorthand::X;
621 return X(j: 1000*x+y);
622}
623} // \namespace impl
624
625/* ************************************************************************* */
626inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
627 using namespace impl;
628
629 // create empty graph
630 NonlinearFactorGraph nlfg;
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(dim: 2,sigma: 1e-3), key(x: 1,y: 1)));
634 nlfg.push_back(factor: 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++) {
640 shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(dim: 2,sigma: 0.01), key(x, y), key(x: x + 1, y)));
641 nlfg.push_back(factor: 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++) {
648 shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(dim: 2,sigma: 0.01), key(x, y), key(x, y: y + 1)));
649 nlfg.push_back(factor: 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(j: key(x, y), val: 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(j: key(x, y), value: Point2((double)x, (double)y));
661
662 // linearize around zero
663 std::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(linearizationPoint: zeros);
664 return std::make_pair(x&: *gfg, y&: xtrue);
665}
666
667/* ************************************************************************* */
668inline Ordering planarOrdering(size_t N) {
669 Ordering ordering;
670 for (size_t y = N; y >= 1; y--)
671 for (size_t x = N; x >= 1; x--)
672 ordering.push_back(x: impl::key(x, y));
673 return ordering;
674}
675
676/* ************************************************************************* */
677inline std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(
678 size_t N, const GaussianFactorGraph& original) {
679 GaussianFactorGraph T, C;
680
681 // Add the x11 constraint to the tree
682 T.push_back(factor: 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(factor: 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(factor: original[i]);
694 else
695 C.push_back(factor: original[i]);
696
697 return std::make_pair(x&: T, y&: C);
698}
699
700/* ************************************************************************* */
701
702} // \namespace example
703} // \namespace gtsam
704