testHybridGaussianISAM.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
22 #include <gtsam/geometry/Pose2.h>
29 
30 #include <numeric>
31 
32 #include "Switching.h"
33 
34 // Include for test suite
36 
37 using namespace std;
38 using namespace gtsam;
46 
47 /* ****************************************************************************/
48 // Test if we can perform elimination incrementally.
49 TEST(HybridGaussianElimination, IncrementalElimination) {
50  Switching switching(3);
53 
54  // Create initial factor graph
55  // * * *
56  // | | |
57  // X0 -*- X1 -*- X2
58  // \*-M0-*/
59  graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X0)
60  graph1.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0)
61  graph1.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1)
62  graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M0)
63 
64  // Run update step
65  isam.update(graph1);
66 
67  // Check that after update we have 2 hybrid Bayes net nodes:
68  // P(X0 | X1, M0) and P(X1, X2 | M0, M1), P(M0, M1)
69  EXPECT_LONGS_EQUAL(3, isam.size());
70  EXPECT(isam[X(0)]->conditional()->frontals() == KeyVector{X(0)});
71  EXPECT(isam[X(0)]->conditional()->parents() == KeyVector({X(1), M(0)}));
72  EXPECT(isam[X(1)]->conditional()->frontals() == KeyVector({X(1), X(2)}));
73  EXPECT(isam[X(1)]->conditional()->parents() == KeyVector({M(0), M(1)}));
74 
75  /********************************************************/
76  // New factor graph for incremental update.
78 
79  graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X1)
80  graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X2)
81  graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M0, M1)
82 
84 
85  // Check that after the second update we have
86  // 1 additional hybrid Bayes net node:
87  // P(X1, X2 | M0, M1)
88  EXPECT_LONGS_EQUAL(3, isam.size());
89  EXPECT(isam[X(2)]->conditional()->frontals() == KeyVector({X(1), X(2)}));
90  EXPECT(isam[X(2)]->conditional()->parents() == KeyVector({M(0), M(1)}));
91 }
92 
93 /* ****************************************************************************/
94 // Test if we can incrementally do the inference
95 TEST(HybridGaussianElimination, IncrementalInference) {
96  Switching switching(3);
99 
100  // Create initial factor graph
101  // * * *
102  // | | |
103  // X0 -*- X1 -*- X2
104  // | |
105  // *-M0 - * - M1
106  graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X0)
107  graph1.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0)
108  graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X1)
109  graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M0)
110 
111  // Run update step
112  isam.update(graph1);
113 
114  auto discreteConditional_m0 = isam[M(0)]->conditional()->asDiscrete();
115  EXPECT(discreteConditional_m0->keys() == KeyVector({M(0)}));
116 
117  /********************************************************/
118  // New factor graph for incremental update.
120 
121  graph2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1)
122  graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X2)
123  graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M0, M1)
124 
125  isam.update(graph2);
126 
127  /********************************************************/
128  // Run batch elimination so we can compare results.
129  const Ordering ordering{X(0), X(1), X(2)};
130 
131  // Now we calculate the expected factors using full elimination
132  const auto [expectedHybridBayesTree, expectedRemainingGraph] =
134 
135  // The densities on X(0) should be the same
136  auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
137  isam[X(0)]->conditional()->inner());
138  auto expected_x0_conditional =
139  dynamic_pointer_cast<HybridGaussianConditional>(
140  (*expectedHybridBayesTree)[X(0)]->conditional()->inner());
141  EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional));
142 
143  // The densities on X(1) should be the same
144  auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
145  isam[X(1)]->conditional()->inner());
146  auto expected_x1_conditional =
147  dynamic_pointer_cast<HybridGaussianConditional>(
148  (*expectedHybridBayesTree)[X(1)]->conditional()->inner());
149  EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional));
150 
151  // The densities on X(2) should be the same
152  auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
153  isam[X(2)]->conditional()->inner());
154  auto expected_x2_conditional =
155  dynamic_pointer_cast<HybridGaussianConditional>(
156  (*expectedHybridBayesTree)[X(2)]->conditional()->inner());
157  EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));
158 
159  // We only perform manual continuous elimination for 0,0.
160  // The other discrete probabilities on M(2) are calculated the same way
161  const Ordering discreteOrdering{M(0), M(1)};
162  HybridBayesTree::shared_ptr discreteBayesTree =
163  expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(
164  discreteOrdering);
165 
166  DiscreteValues m00;
167  m00[M(0)] = 0, m00[M(1)] = 0;
168  DiscreteConditional decisionTree =
169  *(*discreteBayesTree)[M(1)]->conditional()->asDiscrete();
170  double m00_prob = decisionTree(m00);
171 
172  auto discreteConditional = isam[M(1)]->conditional()->asDiscrete();
173 
174  // Test the probability values with regression tests.
175  DiscreteValues assignment;
176  EXPECT(assert_equal(0.0952922, m00_prob, 1e-5));
177  assignment[M(0)] = 0;
178  assignment[M(1)] = 0;
179  EXPECT(assert_equal(0.0952922, (*discreteConditional)(assignment), 1e-5));
180  assignment[M(0)] = 1;
181  assignment[M(1)] = 0;
182  EXPECT(assert_equal(0.282758, (*discreteConditional)(assignment), 1e-5));
183  assignment[M(0)] = 0;
184  assignment[M(1)] = 1;
185  EXPECT(assert_equal(0.314175, (*discreteConditional)(assignment), 1e-5));
186  assignment[M(0)] = 1;
187  assignment[M(1)] = 1;
188  EXPECT(assert_equal(0.307775, (*discreteConditional)(assignment), 1e-5));
189 
190  // Check if the clique conditional generated from incremental elimination
191  // matches that of batch elimination.
192  auto expectedChordal =
193  expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal();
194  auto actualConditional = dynamic_pointer_cast<DecisionTreeFactor>(
195  isam[M(1)]->conditional()->inner());
196  // Account for the probability terms from evaluating continuous FGs
197  DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}};
198  vector<double> probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485};
199  auto expectedConditional =
200  std::make_shared<DecisionTreeFactor>(discrete_keys, probs);
201  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
202 }
203 
204 /* ****************************************************************************/
205 // Test if we can approximately do the inference
206 TEST(HybridGaussianElimination, Approx_inference) {
207  Switching switching(4);
208  HybridGaussianISAM incrementalHybrid;
210 
211  // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
212  for (size_t i = 1; i < 4; i++) {
213  graph1.push_back(switching.linearizedFactorGraph.at(i));
214  }
215 
216  // Add the Gaussian factors, 1 prior on X(0),
217  // 3 measurements on X(1), X(2), X(3)
218  graph1.push_back(switching.linearizedFactorGraph.at(0));
219  for (size_t i = 4; i <= 7; i++) {
220  graph1.push_back(switching.linearizedFactorGraph.at(i));
221  }
222 
223  // Create ordering.
225  for (size_t j = 0; j < 4; j++) {
226  ordering.push_back(X(j));
227  }
228 
229  // Now we calculate the actual factors using full elimination
230  const auto [unprunedHybridBayesTree, unprunedRemainingGraph] =
232 
233  size_t maxNrLeaves = 5;
234  incrementalHybrid.update(graph1);
235 
236  incrementalHybrid.prune(maxNrLeaves);
237 
238  /*
239  unpruned factor is:
240  Choice(m3)
241  0 Choice(m2)
242  0 0 Choice(m1)
243  0 0 0 Leaf 0.11267528
244  0 0 1 Leaf 0.18576102
245  0 1 Choice(m1)
246  0 1 0 Leaf 0.18754662
247  0 1 1 Leaf 0.30623871
248  1 Choice(m2)
249  1 0 Choice(m1)
250  1 0 0 Leaf 0.18576102
251  1 0 1 Leaf 0.30622428
252  1 1 Choice(m1)
253  1 1 0 Leaf 0.30623871
254  1 1 1 Leaf 0.5
255 
256  pruned factors is:
257  Choice(m3)
258  0 Choice(m2)
259  0 0 Leaf 0
260  0 1 Choice(m1)
261  0 1 0 Leaf 0.18754662
262  0 1 1 Leaf 0.30623871
263  1 Choice(m2)
264  1 0 Choice(m1)
265  1 0 0 Leaf 0
266  1 0 1 Leaf 0.30622428
267  1 1 Choice(m1)
268  1 1 0 Leaf 0.30623871
269  1 1 1 Leaf 0.5
270  */
271 
272  auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteConditional>(
273  incrementalHybrid[M(0)]->conditional()->inner());
274  EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)}));
275 
276  // Get the number of elements which are greater than 0.
277  auto count = [](const double &value, int count) {
278  return value > 0 ? count + 1 : count;
279  };
280  // Check that the number of leaves after pruning is 5.
281  EXPECT_LONGS_EQUAL(5, discreteConditional_m0.fold(count, 0));
282 
283  // Check that the hybrid nodes of the bayes net match those of the pre-pruning
284  // bayes net, at the same positions.
285  auto &unprunedLastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
286  unprunedHybridBayesTree->clique(X(3))->conditional()->inner());
287  auto &lastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
288  incrementalHybrid[X(3)]->conditional()->inner());
289 
290  std::vector<std::pair<DiscreteValues, double>> assignments =
291  discreteConditional_m0.enumerate();
292  // Loop over all assignments and check the pruned components
293  for (auto &&av : assignments) {
294  const DiscreteValues &assignment = av.first;
295  const double value = av.second;
296 
297  if (value == 0.0) {
298  EXPECT(lastDensity(assignment) == nullptr);
299  } else {
300  CHECK(lastDensity(assignment));
301  EXPECT(assert_equal(*unprunedLastDensity(assignment),
302  *lastDensity(assignment)));
303  }
304  }
305 }
306 
307 /* ****************************************************************************/
308 // Test approximate inference with an additional pruning step.
309 TEST(HybridGaussianElimination, Incremental_approximate) {
310  Switching switching(5);
311  HybridGaussianISAM incrementalHybrid;
313 
314  /***** Run Round 1 *****/
315  // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
316  for (size_t i = 1; i < 4; i++) {
317  graph1.push_back(switching.linearizedFactorGraph.at(i));
318  }
319 
320  // Add the Gaussian factors, 1 prior on X(0),
321  // 3 measurements on X(1), X(2), X(3)
322  graph1.push_back(switching.linearizedFactorGraph.at(0));
323  for (size_t i = 5; i <= 7; i++) {
324  graph1.push_back(switching.linearizedFactorGraph.at(i));
325  }
326 
327  // Run update with pruning
328  size_t maxComponents = 5;
329  incrementalHybrid.update(graph1);
330  incrementalHybrid.prune(maxComponents);
331 
332  // Check if we have a bayes tree with 4 hybrid nodes,
333  // each with 2, 4, 8, and 5 (pruned) leaves respetively.
334  EXPECT_LONGS_EQUAL(4, incrementalHybrid.size());
336  2, incrementalHybrid[X(0)]->conditional()->asHybrid()->nrComponents());
338  3, incrementalHybrid[X(1)]->conditional()->asHybrid()->nrComponents());
340  5, incrementalHybrid[X(2)]->conditional()->asHybrid()->nrComponents());
342  5, incrementalHybrid[X(3)]->conditional()->asHybrid()->nrComponents());
343 
344  /***** Run Round 2 *****/
348 
349  // Run update with pruning a second time.
350  incrementalHybrid.update(graph2);
351  incrementalHybrid.prune(maxComponents);
352 
353  // Check if we have a bayes tree with pruned hybrid nodes,
354  // with 5 (pruned) leaves.
355  CHECK_EQUAL(5, incrementalHybrid.size());
357  5, incrementalHybrid[X(3)]->conditional()->asHybrid()->nrComponents());
359  5, incrementalHybrid[X(4)]->conditional()->asHybrid()->nrComponents());
360 }
361 
362 /* ************************************************************************/
363 // A GTSAM-only test for running inference on a single-legged robot.
364 // The leg links are represented by the chain X-Y-Z-W, where X is the base and
365 // W is the foot.
366 // We use BetweenFactor<Pose2> as constraints between each of the poses.
367 TEST(HybridGaussianISAM, NonTrivial) {
368  /*************** Run Round 1 ***************/
370 
371  // Add a prior on pose x0 at the origin.
372  // A prior factor consists of a mean and
373  // a noise model (covariance matrix)
374  Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
375  auto priorNoise = noiseModel::Diagonal::Sigmas(
376  Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
378 
379  // create a noise model for the landmark measurements
380  auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
381 
382  // We model a robot's single leg as X - Y - Z - W
383  // where X is the base link and W is the foot link.
384 
385  // Add connecting poses similar to PoseFactors in GTD
386  fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
387  poseNoise);
388  fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
389  poseNoise);
390  fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
391  poseNoise);
392 
393  // Create initial estimate
394  Values initial;
395  initial.insert(X(0), Pose2(0.0, 0.0, 0.0));
396  initial.insert(Y(0), Pose2(0.0, 1.0, 0.0));
397  initial.insert(Z(0), Pose2(0.0, 2.0, 0.0));
398  initial.insert(W(0), Pose2(0.0, 3.0, 0.0));
399 
402 
403  HybridGaussianISAM inc;
404 
405  // Update without pruning
406  // The result is a HybridBayesNet with no discrete variables
407  // (equivalent to a GaussianBayesNet).
408  // Factorization is:
409  // `P(X | measurements) = P(W0|Z0) P(Z0|Y0) P(Y0|X0) P(X0)`
410  inc.update(gfg);
411 
412  /*************** Run Round 2 ***************/
413  using PlanarMotionModel = BetweenFactor<Pose2>;
414 
415  // Add odometry factor with discrete modes.
416  Pose2 odometry(1.0, 0.0, 0.0);
417  auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
418  std::vector<NoiseModelFactor::shared_ptr> components;
419  components.emplace_back(
420  new PlanarMotionModel(W(0), W(1), odometry, noise_model)); // moving
421  components.emplace_back(
422  new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model)); // still
424 
425  // Add equivalent of ImuFactor
426  fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
427  poseNoise);
428  // PoseFactors-like at k=1
429  fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
430  poseNoise);
431  fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
432  poseNoise);
433  fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
434  poseNoise);
435 
436  initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
437  initial.insert(Y(1), Pose2(1.0, 1.0, 0.0));
438  initial.insert(Z(1), Pose2(1.0, 2.0, 0.0));
439  // The leg link did not move so we set the expected pose accordingly.
440  initial.insert(W(1), Pose2(0.0, 3.0, 0.0));
441 
442  gfg = *fg.linearize(initial);
444 
445  // Update without pruning
446  // The result is a HybridBayesNet with 1 discrete variable M(1).
447  // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1)
448  // P(X0 | X1, W1, M1) P(W1|Z1, X1, M1) P(Z1|Y1, X1, M1)
449  // P(Y1 | X1, M1)P(X1 | M1)P(M1)
450  // The MHS tree is a 1 level tree for time indices (1,) with 2 leaves.
451  inc.update(gfg);
452 
453  /*************** Run Round 3 ***************/
454  // Add odometry factor with discrete modes.
455  components.clear();
456  components.emplace_back(
457  new PlanarMotionModel(W(1), W(2), odometry, noise_model)); // moving
458  components.emplace_back(
459  new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model)); // still
461 
462  // Add equivalent of ImuFactor
463  fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
464  poseNoise);
465  // PoseFactors-like at k=1
466  fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
467  poseNoise);
468  fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
469  poseNoise);
470  fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
471  poseNoise);
472 
473  initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
474  initial.insert(Y(2), Pose2(2.0, 1.0, 0.0));
475  initial.insert(Z(2), Pose2(2.0, 2.0, 0.0));
476  initial.insert(W(2), Pose2(0.0, 3.0, 0.0));
477 
478  gfg = *fg.linearize(initial);
480 
481  // Now we prune!
482  // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1)
483  // P(X0 | X1, W1, M1) P(W1|W2, Z1, X1, M1, M2)
484  // P(Z1| W2, Y1, X1, M1, M2) P(Y1 | W2, X1, M1, M2)
485  // P(X1 | W2, X2, M1, M2) P(W2|Z2, X2, M1, M2)
486  // P(Z2|Y2, X2, M1, M2) P(Y2 | X2, M1, M2)
487  // P(X2 | M1, M2) P(M1, M2)
488  // The MHS at this point should be a 2 level tree on (1, 2).
489  // 1 has 2 choices, and 2 has 4 choices.
490  inc.update(gfg);
491  inc.prune(2);
492 
493  /*************** Run Round 4 ***************/
494  // Add odometry factor with discrete modes.
495  components.clear();
496  components.emplace_back(
497  new PlanarMotionModel(W(2), W(3), odometry, noise_model)); // moving
498  components.emplace_back(
499  new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model)); // still
501 
502  // Add equivalent of ImuFactor
503  fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
504  poseNoise);
505  // PoseFactors-like at k=3
506  fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
507  poseNoise);
508  fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
509  poseNoise);
510  fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
511  poseNoise);
512 
513  initial.insert(X(3), Pose2(3.0, 0.0, 0.0));
514  initial.insert(Y(3), Pose2(3.0, 1.0, 0.0));
515  initial.insert(Z(3), Pose2(3.0, 2.0, 0.0));
516  initial.insert(W(3), Pose2(0.0, 3.0, 0.0));
517 
518  gfg = *fg.linearize(initial);
520 
521  // Keep pruning!
522  inc.update(gfg);
523  inc.prune(3);
524 
525  // The final discrete graph should not be empty since we have eliminated
526  // all continuous variables.
527  auto discreteTree = inc[M(3)]->conditional()->asDiscrete();
528  EXPECT_LONGS_EQUAL(3, discreteTree->size());
529 
530  // Test if the optimal discrete mode assignment is (1, 1, 1).
531  DiscreteFactorGraph discreteGraph;
532  discreteGraph.push_back(discreteTree);
533  DiscreteValues optimal_assignment = discreteGraph.optimize();
534 
535  DiscreteValues expected_assignment;
536  expected_assignment[M(1)] = 1;
537  expected_assignment[M(2)] = 1;
538  expected_assignment[M(3)] = 1;
539 
540  EXPECT(assert_equal(expected_assignment, optimal_assignment));
541 
542  // Test if pruning worked correctly by checking that we only have 3 leaves in
543  // the last node.
544  auto lastConditional = inc[X(3)]->conditional()->asHybrid();
545  EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents());
546 }
547 
548 /* ************************************************************************* */
549 int main() {
550  TestResult tr;
551  return TestRegistry::runAllTests(tr);
552 }
gtsam::EliminateableFactorGraph::eliminatePartialMultifrontal
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:190
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::HybridGaussianISAM
Incremental Smoothing and Mapping (ISAM) algorithm for hybrid factor graphs.
Definition: HybridGaussianISAM.h:37
Pose2.h
2D Pose
DiscreteBayesNet.h
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
Y
const char Y
Definition: test/EulerAngles.cpp:31
gtsam::DiscreteFactorGraph
Definition: DiscreteFactorGraph.h:98
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
DiscreteFactorGraph.h
gtsam::HybridNonlinearFactorGraph
Definition: HybridNonlinearFactorGraph.h:33
TEST
TEST(HybridGaussianElimination, IncrementalElimination)
Definition: testHybridGaussianISAM.cpp:49
initial
Values initial
Definition: OdometryOptimize.cpp:2
Switching.h
simple::graph2
NonlinearFactorGraph graph2()
Definition: testInitializePose3.cpp:72
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
X
#define X
Definition: icosphere.cpp:20
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
test_motion::noise_model
auto noise_model
Definition: testHybridNonlinearFactorGraph.cpp:119
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
gtsam::Switching
ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(k),M(k+1))
Definition: Switching.h:122
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
odometry
Pose2 odometry(2.0, 0.0, 0.0)
CHECK_EQUAL
#define CHECK_EQUAL(expected, actual)
Definition: Test.h:131
PriorFactor.h
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
isam
NonlinearISAM isam(relinearizeInterval)
HybridGaussianISAM.h
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:104
L
MatrixXd L
Definition: LLT_example.cpp:6
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
gtsam::DiscreteConditional
Definition: DiscreteConditional.h:37
gtsam
traits
Definition: chartTesting.h:28
gtsam::BayesTree::size
size_t size() const
Definition: BayesTree-inst.h:133
gtsam::DiscreteValues
Definition: DiscreteValues.h:34
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
DiscreteDistribution.h
gtsam::Values
Definition: Values.h:65
gtsam::symbol_shorthand::W
Key W(std::uint64_t j)
Definition: inference/Symbol.h:170
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
std
Definition: BFloat16.h:88
gtsam::HybridNonlinearFactorGraph::linearize
std::shared_ptr< HybridGaussianFactorGraph > linearize(const Values &continuousValues) const
Linearize all the continuous factors in the HybridNonlinearFactorGraph.
Definition: HybridNonlinearFactorGraph.cpp:139
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Switching::linearizedFactorGraph
HybridGaussianFactorGraph linearizedFactorGraph
Definition: Switching.h:126
initial
Definition: testScenarioRunner.cpp:148
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:188
gtsam::HybridBayesTree::prune
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition: HybridBayesTree.cpp:178
test_motion::components
std::vector< NoiseModelFactor::shared_ptr > components
Definition: testHybridNonlinearFactorGraph.cpp:120
gtsam::DiscreteFactorGraph::optimize
DiscreteValues optimize(OptionalOrderingType orderingType={}) const
Find the maximum probable explanation (MPE) by doing max-product.
Definition: DiscreteFactorGraph.cpp:189
Z
#define Z
Definition: icosphere.cpp:21
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::HybridGaussianISAM::update
void update(const HybridGaussianFactorGraph &newFactors, const std::optional< size_t > &maxNrLeaves={}, const std::optional< Ordering > &ordering={}, const HybridBayesTree::Eliminate &function=HybridBayesTree::EliminationTraitsType::DefaultEliminate)
Perform update step with new factors.
Definition: HybridGaussianISAM.cpp:117
main
int main()
Definition: testHybridGaussianISAM.cpp:549
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::HybridNonlinearFactor
Implementation of a discrete-conditioned hybrid factor.
Definition: HybridNonlinearFactor.h:58
gtsam::HybridBayesTree::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: HybridBayesTree.h:68
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
HybridConditional.h
BearingRangeFactor.h
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Oct 4 2024 03:08:41