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


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