testGaussianISAM2.cpp
Go to the documentation of this file.
1 
7 #include <gtsam/nonlinear/ISAM2.h>
8 
9 #include <tests/smallExample.h>
10 #include <gtsam/slam/BetweenFactor.h>
11 #include <gtsam/sam/BearingRangeFactor.h>
12 #include <gtsam/geometry/Point2.h>
13 #include <gtsam/geometry/Pose2.h>
14 #include <gtsam/nonlinear/Values.h>
15 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
16 #include <gtsam/nonlinear/Marginals.h>
17 #include <gtsam/linear/GaussianBayesNet.h>
18 #include <gtsam/linear/GaussianBayesTree.h>
19 #include <gtsam/linear/GaussianFactorGraph.h>
20 #include <gtsam/inference/Ordering.h>
21 #include <gtsam/base/debug.h>
22 #include <gtsam/base/TestableAssertions.h>
23 #include <gtsam/base/treeTraversal-inst.h>
24 
25 #include <CppUnitLite/TestHarness.h>
26 
27 #include <boost/assign/list_of.hpp>
28 #include <boost/range/adaptor/map.hpp>
29 using namespace boost::assign;
30 namespace br { using namespace boost::adaptors; using namespace boost::range; }
31 
32 using namespace std;
33 using namespace gtsam;
34 using boost::shared_ptr;
35 
36 static const SharedNoiseModel model;
37 
38 // SETDEBUG("ISAM2 update", true);
39 // SETDEBUG("ISAM2 update verbose", true);
40 // SETDEBUG("ISAM2 recalculate", true);
41 
42 // Set up parameters
43 SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0).finished());
44 SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1).finished());
45 
46 ISAM2 createSlamlikeISAM2(
47  boost::optional<Values&> init_values = boost::none,
48  boost::optional<NonlinearFactorGraph&> full_graph = boost::none,
49  const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0,
50  0, false, true,
51  ISAM2Params::CHOLESKY, true,
52  DefaultKeyFormatter, true),
53  size_t maxPoses = 10) {
54  // These variables will be reused and accumulate factors and values
55  ISAM2 isam(params);
56  Values fullinit;
57  NonlinearFactorGraph fullgraph;
58 
59  // i keeps track of the time step
60  size_t i = 0;
61 
62  // Add a prior at time 0 and update isam
63  {
64  NonlinearFactorGraph newfactors;
65  newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
66  fullgraph.push_back(newfactors);
67 
68  Values init;
69  init.insert((0), Pose2(0.01, 0.01, 0.01));
70  fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
71 
72  isam.update(newfactors, init);
73  }
74 
75  if(i > maxPoses)
76  goto done;
77 
78  // Add odometry from time 0 to time 5
79  for( ; i<5; ++i) {
80  NonlinearFactorGraph newfactors;
81  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
82  fullgraph.push_back(newfactors);
83 
84  Values init;
85  init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
86  fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
87 
88  isam.update(newfactors, init);
89 
90  if(i > maxPoses)
91  goto done;
92  }
93 
94  if(i > maxPoses)
95  goto done;
96 
97  // Add odometry from time 5 to 6 and landmark measurement at time 5
98  {
99  NonlinearFactorGraph newfactors;
100  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
101  newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
102  newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
103  fullgraph.push_back(newfactors);
104 
105  Values init;
106  init.insert((i+1), Pose2(1.01, 0.01, 0.01));
107  init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
108  init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
109  fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
110  fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
111  fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
112 
113  isam.update(newfactors, init);
114  ++ i;
115  }
116 
117  if(i > maxPoses)
118  goto done;
119 
120  // Add odometry from time 6 to time 10
121  for( ; i<10; ++i) {
122  NonlinearFactorGraph newfactors;
123  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
124  fullgraph.push_back(newfactors);
125 
126  Values init;
127  init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
128  fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
129 
130  isam.update(newfactors, init);
131 
132  if(i > maxPoses)
133  goto done;
134  }
135 
136  if(i > maxPoses)
137  goto done;
138 
139  // Add odometry from time 10 to 11 and landmark measurement at time 10
140  {
141  NonlinearFactorGraph newfactors;
142  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
143  newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
144  newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
145  fullgraph.push_back(newfactors);
146 
147  Values init;
148  init.insert((i+1), Pose2(6.9, 0.1, 0.01));
149  fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
150 
151  isam.update(newfactors, init);
152  ++ i;
153  }
154 
155 done:
156 
157  if (full_graph)
158  *full_graph = fullgraph;
159 
160  if (init_values)
161  *init_values = fullinit;
162 
163  return isam;
164 }
165 
166 /* ************************************************************************* */
167 //TEST(ISAM2, CheckRelinearization) {
168 //
169 // typedef GaussianISAM2<Values>::Impl Impl;
170 //
171 // // Create values where indices 1 and 3 are above the threshold of 0.1
172 // VectorValues values;
173 // values.reserve(4, 10);
174 // values.push_back_preallocated(Vector2(0.09, 0.09));
175 // values.push_back_preallocated(Vector3(0.11, 0.11, 0.09));
176 // values.push_back_preallocated(Vector3(0.09, 0.09, 0.09));
177 // values.push_back_preallocated(Vector2(0.11, 0.11));
178 //
179 // // Create a permutation
180 // Permutation permutation(4);
181 // permutation[0] = 2;
182 // permutation[1] = 0;
183 // permutation[2] = 1;
184 // permutation[3] = 3;
185 //
186 // Permuted<VectorValues> permuted(permutation, values);
187 //
188 // // After permutation, the indices above the threshold are 2 and 2
189 // KeySet expected;
190 // expected.insert(2);
191 // expected.insert(3);
192 //
193 // // Indices checked by CheckRelinearization
194 // KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
195 //
196 // EXPECT(assert_equal(expected, actual));
197 //}
198 
199 /* ************************************************************************* */
200 struct ConsistencyVisitor
201 {
202  bool consistent;
203  const ISAM2& isam;
204  ConsistencyVisitor(const ISAM2& isam) :
205  consistent(true), isam(isam) {}
206  int operator()(const ISAM2::sharedClique& node, int& parentData)
207  {
208  if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
209  {
210  if(node->parent_.expired())
211  consistent = false;
212  if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
213  consistent = false;
214  }
215  for(Key j: node->conditional()->frontals())
216  {
217  if(isam.nodes().at(j).get() != node.get())
218  consistent = false;
219  }
220  return 0;
221  }
222 };
223 
224 /* ************************************************************************* */
225 bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
226 
227  TestResult& result_ = result;
228  const string name_ = test.getName();
229 
230  Values actual = isam.calculateEstimate();
231  Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize());
232 
233  bool isamEqual = assert_equal(expected, actual);
234 
235  // Check information
236  GaussianFactorGraph isamGraph(isam);
237  isamGraph += isam.roots().front()->cachedFactor_;
238  Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian();
239  Matrix actualHessian = isamGraph.augmentedHessian();
240  expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1);
241  bool isamTreeEqual = assert_equal(expectedHessian, actualHessian);
242 
243  // Check consistency
244  ConsistencyVisitor visitor(isam);
245  int data; // Unused
246  treeTraversal::DepthFirstForest(isam, data, visitor);
247  bool consistent = visitor.consistent;
248 
249  // The following two checks make sure that the cached gradients are maintained and used correctly
250 
251  // Check gradient at each node
252  bool nodeGradientsOk = true;
253  typedef ISAM2::sharedClique sharedClique;
254  for(const sharedClique& clique: isam.nodes() | br::map_values) {
255  // Compute expected gradient
256  GaussianFactorGraph jfg;
257  jfg += clique->conditional();
258  VectorValues expectedGradient = jfg.gradientAtZero();
259  // Compare with actual gradients
260  DenseIndex variablePosition = 0;
261  for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
262  const DenseIndex dim = clique->conditional()->getDim(jit);
263  Vector actual = clique->gradientContribution().segment(variablePosition, dim);
264  bool gradOk = assert_equal(expectedGradient[*jit], actual);
265  EXPECT(gradOk);
266  nodeGradientsOk = nodeGradientsOk && gradOk;
267  variablePosition += dim;
268  }
269  bool dimOk = clique->gradientContribution().rows() == variablePosition;
270  EXPECT(dimOk);
271  nodeGradientsOk = nodeGradientsOk && dimOk;
272  }
273 
274  // Check gradient
275  VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
276  VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
277  VectorValues actualGradient = isam.gradientAtZero();
278  bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient);
279  EXPECT(expectedGradOk);
280  bool totalGradOk = assert_equal(expectedGradient, actualGradient);
281  EXPECT(totalGradOk);
282 
283  return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
284 }
285 
286 /* ************************************************************************* */
287 TEST(ISAM2, simple)
288 {
289  for(size_t i = 0; i < 10; ++i) {
290  // These variables will be reused and accumulate factors and values
291  Values fullinit;
292  NonlinearFactorGraph fullgraph;
293  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i);
294 
295  // Compare solutions
296  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
297  }
298 }
299 
300 /* ************************************************************************* */
301 TEST(ISAM2, slamlike_solution_gaussnewton)
302 {
303  // These variables will be reused and accumulate factors and values
304  Values fullinit;
305  NonlinearFactorGraph fullgraph;
306  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
307 
308  // Compare solutions
309  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
310 }
311 
312 /* ************************************************************************* */
313 TEST(ISAM2, slamlike_solution_dogleg)
314 {
315  // These variables will be reused and accumulate factors and values
316  Values fullinit;
317  NonlinearFactorGraph fullgraph;
318  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
319 
320  // Compare solutions
321  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
322 }
323 
324 /* ************************************************************************* */
325 TEST(ISAM2, slamlike_solution_gaussnewton_qr)
326 {
327  // These variables will be reused and accumulate factors and values
328  Values fullinit;
329  NonlinearFactorGraph fullgraph;
330  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
331 
332  // Compare solutions
333  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
334 }
335 
336 /* ************************************************************************* */
337 TEST(ISAM2, slamlike_solution_dogleg_qr)
338 {
339  // These variables will be reused and accumulate factors and values
340  Values fullinit;
341  NonlinearFactorGraph fullgraph;
342  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
343 
344  // Compare solutions
345  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
346 }
347 
348 /* ************************************************************************* */
349 TEST(ISAM2, clone) {
350 
351  ISAM2 clone1;
352 
353  {
354  ISAM2 isam = createSlamlikeISAM2();
355  clone1 = isam;
356 
357  ISAM2 clone2(isam);
358 
359  // Modify original isam
360  NonlinearFactorGraph factors;
361  factors += BetweenFactor<Pose2>(0, 10,
362  isam.calculateEstimate<Pose2>(0).between(isam.calculateEstimate<Pose2>(10)), noiseModel::Unit::Create(3));
363  isam.update(factors);
364 
365  CHECK(assert_equal(createSlamlikeISAM2(), clone2));
366  }
367 
368  // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced
369  // if the references in the iSAM2 copy point to the old instance which deleted at
370  // the end of the {...} section above.
371  ISAM2 temp = createSlamlikeISAM2();
372 
373  CHECK(assert_equal(createSlamlikeISAM2(), clone1));
374  CHECK(assert_equal(clone1, temp));
375 
376  // Check clone empty
377  ISAM2 isam;
378  clone1 = isam;
379  CHECK(assert_equal(ISAM2(), clone1));
380 }
381 
382 /* ************************************************************************* */
383 TEST(ISAM2, removeFactors)
384 {
385  // This test builds a graph in the same way as the "slamlike" test above, but
386  // then removes the 2nd-to-last landmark measurement
387 
388  // These variables will be reused and accumulate factors and values
389  Values fullinit;
390  NonlinearFactorGraph fullgraph;
391  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
392 
393  // Remove the 2nd measurement on landmark 0 (Key 100)
394  FactorIndices toRemove;
395  toRemove.push_back(12);
396  isam.update(NonlinearFactorGraph(), Values(), toRemove);
397 
398  // Remove the factor from the full system
399  fullgraph.remove(12);
400 
401  // Compare solutions
402  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
403 }
404 
405 /* ************************************************************************* */
406 TEST(ISAM2, removeVariables)
407 {
408  // These variables will be reused and accumulate factors and values
409  Values fullinit;
410  NonlinearFactorGraph fullgraph;
411  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
412 
413  // Remove the measurement on landmark 0 (Key 100)
414  FactorIndices toRemove;
415  toRemove.push_back(7);
416  toRemove.push_back(14);
417  isam.update(NonlinearFactorGraph(), Values(), toRemove);
418 
419  // Remove the factors and variable from the full system
420  fullgraph.remove(7);
421  fullgraph.remove(14);
422  fullinit.erase(100);
423 
424  // Compare solutions
425  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
426 }
427 
428 /* ************************************************************************* */
429 TEST(ISAM2, swapFactors)
430 {
431  // This test builds a graph in the same way as the "slamlike" test above, but
432  // then swaps the 2nd-to-last landmark measurement with a different one
433 
434  Values fullinit;
435  NonlinearFactorGraph fullgraph;
436  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph);
437 
438  // Remove the measurement on landmark 0 and replace with a different one
439  {
440  size_t swap_idx = isam.getFactorsUnsafe().size()-2;
441  FactorIndices toRemove;
442  toRemove.push_back(swap_idx);
443  fullgraph.remove(swap_idx);
444 
445  NonlinearFactorGraph swapfactors;
446 // swapfactors += BearingRange<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor
447  swapfactors += BearingRangeFactor<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
448  fullgraph.push_back(swapfactors);
449  isam.update(swapfactors, Values(), toRemove);
450  }
451 
452  // Compare solutions
453  EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe())));
454  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
455 
456  // Check gradient at each node
457  typedef ISAM2::sharedClique sharedClique;
458  for(const sharedClique& clique: isam.nodes() | br::map_values) {
459  // Compute expected gradient
460  GaussianFactorGraph jfg;
461  jfg += clique->conditional();
462  VectorValues expectedGradient = jfg.gradientAtZero();
463  // Compare with actual gradients
464  DenseIndex variablePosition = 0;
465  for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
466  const DenseIndex dim = clique->conditional()->getDim(jit);
467  Vector actual = clique->gradientContribution().segment(variablePosition, dim);
468  EXPECT(assert_equal(expectedGradient[*jit], actual));
469  variablePosition += dim;
470  }
471  EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition);
472  }
473 
474  // Check gradient
475  VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
476  VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
477  VectorValues actualGradient = isam.gradientAtZero();
478  EXPECT(assert_equal(expectedGradient2, expectedGradient));
479  EXPECT(assert_equal(expectedGradient, actualGradient));
480 }
481 
482 /* ************************************************************************* */
483 TEST(ISAM2, constrained_ordering)
484 {
485  // These variables will be reused and accumulate factors and values
486  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
487  Values fullinit;
488  NonlinearFactorGraph fullgraph;
489 
490  // We will constrain x3 and x4 to the end
491  FastMap<Key, int> constrained;
492  constrained.insert(make_pair((3), 1));
493  constrained.insert(make_pair((4), 2));
494 
495  // i keeps track of the time step
496  size_t i = 0;
497 
498  // Add a prior at time 0 and update isam
499  {
500  NonlinearFactorGraph newfactors;
501  newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
502  fullgraph.push_back(newfactors);
503 
504  Values init;
505  init.insert((0), Pose2(0.01, 0.01, 0.01));
506  fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
507 
508  isam.update(newfactors, init);
509  }
510 
511  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
512 
513  // Add odometry from time 0 to time 5
514  for( ; i<5; ++i) {
515  NonlinearFactorGraph newfactors;
516  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
517  fullgraph.push_back(newfactors);
518 
519  Values init;
520  init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
521  fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
522 
523  if(i >= 3)
524  isam.update(newfactors, init, FactorIndices(), constrained);
525  else
526  isam.update(newfactors, init);
527  }
528 
529  // Add odometry from time 5 to 6 and landmark measurement at time 5
530  {
531  NonlinearFactorGraph newfactors;
532  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
533  newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
534  newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
535  fullgraph.push_back(newfactors);
536 
537  Values init;
538  init.insert((i+1), Pose2(1.01, 0.01, 0.01));
539  init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
540  init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
541  fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
542  fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
543  fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
544 
545  isam.update(newfactors, init, FactorIndices(), constrained);
546  ++ i;
547  }
548 
549  // Add odometry from time 6 to time 10
550  for( ; i<10; ++i) {
551  NonlinearFactorGraph newfactors;
552  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
553  fullgraph.push_back(newfactors);
554 
555  Values init;
556  init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
557  fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
558 
559  isam.update(newfactors, init, FactorIndices(), constrained);
560  }
561 
562  // Add odometry from time 10 to 11 and landmark measurement at time 10
563  {
564  NonlinearFactorGraph newfactors;
565  newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
566  newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
567  newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
568  fullgraph.push_back(newfactors);
569 
570  Values init;
571  init.insert((i+1), Pose2(6.9, 0.1, 0.01));
572  fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
573 
574  isam.update(newfactors, init, FactorIndices(), constrained);
575  ++ i;
576  }
577 
578  // Compare solutions
579  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
580 
581  // Check gradient at each node
582  typedef ISAM2::sharedClique sharedClique;
583  for(const sharedClique& clique: isam.nodes() | br::map_values) {
584  // Compute expected gradient
585  GaussianFactorGraph jfg;
586  jfg += clique->conditional();
587  VectorValues expectedGradient = jfg.gradientAtZero();
588  // Compare with actual gradients
589  DenseIndex variablePosition = 0;
590  for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
591  const DenseIndex dim = clique->conditional()->getDim(jit);
592  Vector actual = clique->gradientContribution().segment(variablePosition, dim);
593  EXPECT(assert_equal(expectedGradient[*jit], actual));
594  variablePosition += dim;
595  }
596  LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition);
597  }
598 
599  // Check gradient
600  VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
601  VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
602  VectorValues actualGradient = isam.gradientAtZero();
603  EXPECT(assert_equal(expectedGradient2, expectedGradient));
604  EXPECT(assert_equal(expectedGradient, actualGradient));
605 }
606 
607 /* ************************************************************************* */
608 TEST(ISAM2, slamlike_solution_partial_relinearization_check)
609 {
610  // These variables will be reused and accumulate factors and values
611  Values fullinit;
612  NonlinearFactorGraph fullgraph;
613  ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
614  params.enablePartialRelinearizationCheck = true;
615  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params);
616 
617  // Compare solutions
618  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
619 }
620 
621 namespace {
622  bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
623  Matrix expectedAugmentedHessian, expected3AugmentedHessian;
624  KeyVector toKeep;
625  for(Key j: isam.getDelta() | br::map_keys)
626  if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
627  toKeep.push_back(j);
628 
629  // Calculate expected marginal from iSAM2 tree
630  expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian();
631 
632  // Calculate expected marginal from cached linear factors
633  //assert(isam.params().cacheLinearizedFactors);
634  //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian();
635 
636  // Calculate expected marginal from original nonlinear factors
637  expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint())
638  ->marginal(toKeep, EliminateQR)->augmentedHessian();
639 
640  // Do marginalization
641  isam.marginalizeLeaves(leafKeys);
642 
643  // Check
644  GaussianFactorGraph actualMarginalGraph(isam);
645  Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
646  //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian();
647  Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize(
648  isam.getLinearizationPoint())->augmentedHessian();
649  assert(actualAugmentedHessian.allFinite());
650 
651  // Check full marginalization
652  //cout << "treeEqual" << endl;
653  bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6);
654  //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
655  //cout << "nonlinEqual" << endl;
656  actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6);
657  //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6);
658  //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6);
659  //cout << "nonlinCorrect" << endl;
660  bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6);
661 
662  bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
663  return ok;
664  }
665 }
666 
667 /* ************************************************************************* */
668 TEST(ISAM2, marginalizeLeaves1) {
669  ISAM2 isam;
670  NonlinearFactorGraph factors;
671  factors.addPrior(0, 0.0, model);
672 
673  factors += BetweenFactor<double>(0, 1, 0.0, model);
674  factors += BetweenFactor<double>(1, 2, 0.0, model);
675  factors += BetweenFactor<double>(0, 2, 0.0, model);
676 
677  Values values;
678  values.insert(0, 0.0);
679  values.insert(1, 0.0);
680  values.insert(2, 0.0);
681 
682  FastMap<Key, int> constrainedKeys;
683  constrainedKeys.insert(make_pair(0, 0));
684  constrainedKeys.insert(make_pair(1, 1));
685  constrainedKeys.insert(make_pair(2, 2));
686 
687  isam.update(factors, values, FactorIndices(), constrainedKeys);
688 
689  FastList<Key> leafKeys = list_of(0);
690  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
691 }
692 
693 /* ************************************************************************* */
694 TEST(ISAM2, marginalizeLeaves2) {
695  ISAM2 isam;
696 
697  NonlinearFactorGraph factors;
698  factors.addPrior(0, 0.0, model);
699 
700  factors += BetweenFactor<double>(0, 1, 0.0, model);
701  factors += BetweenFactor<double>(1, 2, 0.0, model);
702  factors += BetweenFactor<double>(0, 2, 0.0, model);
703  factors += BetweenFactor<double>(2, 3, 0.0, model);
704 
705  Values values;
706  values.insert(0, 0.0);
707  values.insert(1, 0.0);
708  values.insert(2, 0.0);
709  values.insert(3, 0.0);
710 
711  FastMap<Key, int> constrainedKeys;
712  constrainedKeys.insert(make_pair(0, 0));
713  constrainedKeys.insert(make_pair(1, 1));
714  constrainedKeys.insert(make_pair(2, 2));
715  constrainedKeys.insert(make_pair(3, 3));
716 
717  isam.update(factors, values, FactorIndices(), constrainedKeys);
718 
719  FastList<Key> leafKeys = list_of(0);
720  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
721 }
722 
723 /* ************************************************************************* */
724 TEST(ISAM2, marginalizeLeaves3) {
725  ISAM2 isam;
726 
727  NonlinearFactorGraph factors;
728  factors.addPrior(0, 0.0, model);
729 
730  factors += BetweenFactor<double>(0, 1, 0.0, model);
731  factors += BetweenFactor<double>(1, 2, 0.0, model);
732  factors += BetweenFactor<double>(0, 2, 0.0, model);
733 
734  factors += BetweenFactor<double>(2, 3, 0.0, model);
735 
736  factors += BetweenFactor<double>(3, 4, 0.0, model);
737  factors += BetweenFactor<double>(4, 5, 0.0, model);
738  factors += BetweenFactor<double>(3, 5, 0.0, model);
739 
740  Values values;
741  values.insert(0, 0.0);
742  values.insert(1, 0.0);
743  values.insert(2, 0.0);
744  values.insert(3, 0.0);
745  values.insert(4, 0.0);
746  values.insert(5, 0.0);
747 
748  FastMap<Key, int> constrainedKeys;
749  constrainedKeys.insert(make_pair(0, 0));
750  constrainedKeys.insert(make_pair(1, 1));
751  constrainedKeys.insert(make_pair(2, 2));
752  constrainedKeys.insert(make_pair(3, 3));
753  constrainedKeys.insert(make_pair(4, 4));
754  constrainedKeys.insert(make_pair(5, 5));
755 
756  isam.update(factors, values, FactorIndices(), constrainedKeys);
757 
758  FastList<Key> leafKeys = list_of(0);
759  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
760 }
761 
762 /* ************************************************************************* */
763 TEST(ISAM2, marginalizeLeaves4) {
764  ISAM2 isam;
765 
766  NonlinearFactorGraph factors;
767  factors.addPrior(0, 0.0, model);
768  factors += BetweenFactor<double>(0, 2, 0.0, model);
769  factors += BetweenFactor<double>(1, 2, 0.0, model);
770 
771  Values values;
772  values.insert(0, 0.0);
773  values.insert(1, 0.0);
774  values.insert(2, 0.0);
775 
776  FastMap<Key, int> constrainedKeys;
777  constrainedKeys.insert(make_pair(0, 0));
778  constrainedKeys.insert(make_pair(1, 1));
779  constrainedKeys.insert(make_pair(2, 2));
780 
781  isam.update(factors, values, FactorIndices(), constrainedKeys);
782 
783  FastList<Key> leafKeys = list_of(1);
784  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
785 }
786 
787 /* ************************************************************************* */
788 TEST(ISAM2, marginalizeLeaves5)
789 {
790  // Create isam2
791  ISAM2 isam = createSlamlikeISAM2();
792 
793  // Marginalize
794  FastList<Key> marginalizeKeys = list_of(0);
795  EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
796 }
797 
798 /* ************************************************************************* */
799 TEST(ISAM2, marginalCovariance)
800 {
801  // Create isam2
802  ISAM2 isam = createSlamlikeISAM2();
803 
804  // Check marginal
805  Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5);
806  Matrix actual = isam.marginalCovariance(5);
807  EXPECT(assert_equal(expected, actual));
808 }
809 
810 /* ************************************************************************* */
811 TEST(ISAM2, calculate_nnz)
812 {
813  ISAM2 isam = createSlamlikeISAM2();
814  int expected = 241;
815  int actual = isam.roots().front()->calculate_nnz();
816 
817  EXPECT_LONGS_EQUAL(expected, actual);
818 }
819 
820 /* ************************************************************************* */
821 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
822 /* ************************************************************************* */
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:306
CHECK
#define CHECK(condition)
Definition: Test.h:109
ConsistencyVisitor::ConsistencyVisitor
ConsistencyVisitor(const ISAM2 &isam)
Definition: testGaussianISAM2.cpp:204
model
static const SharedNoiseModel model
Definition: testGaussianISAM2.cpp:36
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
createSlamlikeISAM2
ISAM2 createSlamlikeISAM2(boost::optional< Values & > init_values=boost::none, boost::optional< NonlinearFactorGraph & > full_graph=boost::none, const ISAM2Params &params=ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true, ISAM2Params::CHOLESKY, true, DefaultKeyFormatter, true), size_t maxPoses=10)
Definition: testGaussianISAM2.cpp:46
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
Values.h
A non-templated config holding any types of Manifold-group elements.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::ISAM2::marginalCovariance
Matrix marginalCovariance(Key key) const
Definition: ISAM2.cpp:784
debug.h
Global debugging flags.
gtsam::EliminateableFactorGraph::marginal
boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Definition: EliminateableFactorGraph-inst.h:351
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
test
Definition: test.py:1
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
M_PI
#define M_PI
Definition: main.h:78
gtsam::BayesTree::nodes
const Nodes & nodes() const
Definition: BayesTree.h:141
leaf::values
leaf::MyValues values
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
std
Definition: Half.h:150
simple_graph::factors
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
gtsam::FactorGraph::remove
void remove(size_t i)
Definition: FactorGraph.h:362
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:109
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::DefaultKeyFormatter
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Definition: treeTraversal-inst.h:77
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:229
brNoise
SharedDiagonal brNoise
Definition: testGaussianISAM2.cpp:44
gtsam::ISAM2::calculateEstimate
Values calculateEstimate() const
Definition: ISAM2.cpp:763
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
odoNoise
SharedDiagonal odoNoise
Definition: testGaussianISAM2.cpp:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
ConsistencyVisitor::operator()
int operator()(const ISAM2::sharedClique &node, int &parentData)
Definition: testGaussianISAM2.cpp:206
result
Values result
Definition: OdometryOptimize.cpp:8
parentData
DATA & parentData
Definition: treeTraversal-inst.h:45
gtsam::ISAM2::marginalizeLeaves
void marginalizeLeaves(const FastList< Key > &leafKeys, boost::optional< FactorIndices & > marginalFactorsIndices=boost::none, boost::optional< FactorIndices & > deletedFactorsIndices=boost::none)
Definition: ISAM2.cpp:479
gtsam::GaussianFactorGraph::gradient
VectorValues gradient(const VectorValues &x0) const
Definition: GaussianFactorGraph.cpp:341
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
data
int data[]
Definition: Map_placement_new.cpp:1
gtsam::GaussianFactorGraph::augmentedHessian
Matrix augmentedHessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:232
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Values::erase
void erase(Key j)
Definition: Values.cpp:183
isam
NonlinearISAM isam(relinearizeInterval)
isam_check
bool isam_check(const NonlinearFactorGraph &fullgraph, const Values &fullinit, const ISAM2 &isam, Test &test, TestResult &result)
Definition: testGaussianISAM2.cpp:225
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
params
static SmartStereoProjectionParams params
Definition: testSmartStereoProjectionPoseFactor.cpp:43
gtsam::ISAM2::gradientAtZero
VectorValues gradientAtZero() const
Definition: ISAM2.cpp:802
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
mpfr::dim
const mpreal dim(const mpreal &a, const mpreal &b, mp_rnd_t r=mpreal::get_default_rnd())
Definition: mpreal.h:2201
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
main
int main()
Definition: testGaussianISAM2.cpp:821
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::ISAM2::getDelta
const VectorValues & getDelta() const
Definition: ISAM2.cpp:791
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1460
gtsam::ISAM2Params::enablePartialRelinearizationCheck
bool enablePartialRelinearizationCheck
Definition: ISAM2Params.h:218
TEST
TEST(ISAM2, simple)
Definition: testGaussianISAM2.cpp:287
smallExample.h
Create small example with two poses and one landmark.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::ISAM2::update
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const boost::optional< FastMap< Key, int > > &constrainedKeys=boost::none, const boost::optional< FastList< Key > > &noRelinKeys=boost::none, const boost::optional< FastList< Key > > &extraReelimKeys=boost::none, bool force_relinearize=false)
Definition: ISAM2.cpp:396
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:257
Pose2.h
2D Pose
gtsam::NonlinearFactorGraph::linearize
boost::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:340
Point2.h
2D Point
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
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::BayesTree::roots
const Roots & roots() const
Definition: BayesTree.h:147
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:205
gtsam::GaussianFactorGraph::gradientAtZero
virtual VectorValues gradientAtZero() const
Definition: GaussianFactorGraph.cpp:353

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