testHybridNonlinearFactorGraph.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6  * See LICENSE for the license information
7  * -------------------------------------------------------------------------- */
8 
19 #include <gtsam/base/utilities.h>
23 #include <gtsam/geometry/Pose2.h>
35 
36 #include "Switching.h"
37 
38 // Include for test suite
40 
41 using namespace std;
42 using namespace gtsam;
47 
48 /* ****************************************************************************
49  * Test that any linearizedFactorGraph gaussian factors are appended to the
50  * existing gaussian factor graph in the hybrid factor graph.
51  */
54 
55  // Add a simple prior factor to the nonlinear factor graph
56  fg.emplace_shared<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1));
57 
58  // Linearization point
59  Values linearizationPoint;
60  linearizationPoint.insert<double>(X(0), 0);
61 
62  // Linearize the factor graph.
63  HybridGaussianFactorGraph ghfg = *fg.linearize(linearizationPoint);
64  EXPECT_LONGS_EQUAL(1, ghfg.size());
65 
66  // Check that the error is the same for the nonlinear values.
67  const VectorValues zero{{X(0), Vector1(0)}};
68  const HybridValues hybridValues{zero, {}, linearizationPoint};
69  EXPECT_DOUBLES_EQUAL(fg.error(hybridValues), ghfg.error(hybridValues), 1e-9);
70 }
71 
72 /***************************************************************************
73  * Test equality for Hybrid Nonlinear Factor Graph.
74  */
78 
79  // Test empty factor graphs
81 
82  auto f0 = std::make_shared<PriorFactor<Pose2>>(
83  1, Pose2(), noiseModel::Isotropic::Sigma(3, 0.001));
86 
87  auto f1 = std::make_shared<BetweenFactor<Pose2>>(
88  1, 2, Pose2(), noiseModel::Isotropic::Sigma(3, 0.1));
91 
92  // Test non-empty graphs
94 }
95 
96 /***************************************************************************
97  * Test that the resize method works correctly for a HybridNonlinearFactorGraph.
98  */
101  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>();
102  fg.push_back(nonlinearFactor);
103 
104  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
105  fg.push_back(discreteFactor);
106 
107  auto dcFactor = std::make_shared<HybridNonlinearFactor>();
108  fg.push_back(dcFactor);
109 
110  EXPECT_LONGS_EQUAL(fg.size(), 3);
111 
112  fg.resize(0);
113  EXPECT_LONGS_EQUAL(fg.size(), 0);
114 }
115 
116 /***************************************************************************/
117 namespace test_motion {
118 gtsam::DiscreteKey m1(M(1), 2);
119 auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
120 std::vector<NoiseModelFactor::shared_ptr> components = {
121  std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
122  std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model)};
123 } // namespace test_motion
124 
125 /***************************************************************************
126  * Test that the resize method works correctly for a
127  * HybridGaussianFactorGraph.
128  */
130  using namespace test_motion;
131 
133  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
134  X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
135  hnfg.push_back(nonlinearFactor);
136  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
137  hnfg.push_back(discreteFactor);
138 
139  auto dcFactor = std::make_shared<HybridNonlinearFactor>(m1, components);
140  hnfg.push_back(dcFactor);
141 
142  Values linearizationPoint;
143  linearizationPoint.insert<double>(X(0), 0);
144  linearizationPoint.insert<double>(X(1), 1);
145 
146  // Generate `HybridGaussianFactorGraph` by linearizing
147  HybridGaussianFactorGraph gfg = *hnfg.linearize(linearizationPoint);
148 
149  EXPECT_LONGS_EQUAL(gfg.size(), 3);
150 
151  gfg.resize(0);
152  EXPECT_LONGS_EQUAL(gfg.size(), 0);
153 }
154 
155 /*****************************************************************************
156  * Test push_back on HFG makes the correct distinction.
157  */
160 
161  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>();
162  fg.push_back(nonlinearFactor);
163 
164  EXPECT_LONGS_EQUAL(fg.size(), 1);
165 
167 
168  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
169  fg.push_back(discreteFactor);
170 
171  EXPECT_LONGS_EQUAL(fg.size(), 1);
172 
174 
175  auto dcFactor = std::make_shared<HybridNonlinearFactor>();
176  fg.push_back(dcFactor);
177 
178  EXPECT_LONGS_EQUAL(fg.size(), 1);
179 
180  // Now do the same with HybridGaussianFactorGraph
182 
183  auto gaussianFactor = std::make_shared<JacobianFactor>();
184  ghfg.push_back(gaussianFactor);
185 
186  EXPECT_LONGS_EQUAL(ghfg.size(), 1);
187 
188  ghfg = HybridGaussianFactorGraph();
189  ghfg.push_back(discreteFactor);
190 
191  EXPECT_LONGS_EQUAL(ghfg.size(), 1);
192 
193  ghfg = HybridGaussianFactorGraph();
194  ghfg.push_back(dcFactor);
195 
197  hgfg2.push_back(ghfg.begin(), ghfg.end());
198 
199  EXPECT_LONGS_EQUAL(ghfg.size(), 1);
200 
203  auto noise = noiseModel::Isotropic::Sigma(3, 1.0);
204  factors.emplace_shared<PriorFactor<Pose2>>(0, Pose2(0, 0, 0), noise);
205  factors.emplace_shared<PriorFactor<Pose2>>(1, Pose2(1, 0, 0), noise);
206  factors.emplace_shared<PriorFactor<Pose2>>(2, Pose2(2, 0, 0), noise);
207  // TODO(Varun) This does not currently work. It should work once HybridFactor
208  // becomes a base class of NoiseModelFactor.
209  // hnfg.push_back(factors.begin(), factors.end());
210 
211  // EXPECT_LONGS_EQUAL(3, hnfg.size());
212 }
213 
214 /* ****************************************************************************/
215 // Test hybrid nonlinear factor graph errorTree
217  Switching s(3);
218 
219  const HybridNonlinearFactorGraph &graph = s.nonlinearFactorGraph();
220  const Values &values = s.linearizationPoint;
221 
222  auto error_tree = graph.errorTree(s.linearizationPoint);
223 
224  auto dkeys = graph.discreteKeys();
225  DiscreteKeys discrete_keys(dkeys.begin(), dkeys.end());
226 
227  // Compute the sum of errors for each factor.
228  auto assignments = DiscreteValues::CartesianProduct(discrete_keys);
229  std::vector<double> leaves(assignments.size());
230  for (auto &&factor : graph) {
231  for (size_t i = 0; i < assignments.size(); ++i) {
232  leaves[i] +=
233  factor->error(HybridValues(VectorValues(), assignments[i], values));
234  }
235  }
236  // Swap i=1 and i=2 to give correct ordering.
237  double temp = leaves[1];
238  leaves[1] = leaves[2];
239  leaves[2] = temp;
240  AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
241 
242  EXPECT(assert_equal(expected_error, error_tree, 1e-7));
243 }
244 
245 /****************************************************************************
246  * Test construction of switching-like hybrid factor graph.
247  */
249  Switching self(3);
250 
251  EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph().size());
252  EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph().size());
253 }
254 
255 /****************************************************************************
256  * Test linearization on a switching-like hybrid factor graph.
257  */
259  Switching self(3);
260 
261  // Linearize here:
262  HybridGaussianFactorGraph actualLinearized =
263  *self.nonlinearFactorGraph().linearize(self.linearizationPoint);
264 
265  EXPECT_LONGS_EQUAL(7, actualLinearized.size());
266 }
267 
268 /****************************************************************************
269  * Test elimination tree construction
270  */
272  Switching self(3);
273 
274  // Create ordering.
276  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
277 
278  // Create elimination tree.
279  HybridEliminationTree etree(self.linearizedFactorGraph(), ordering);
280  EXPECT_LONGS_EQUAL(1, etree.roots().size())
281 }
282 
283 /****************************************************************************
284  *Test elimination function by eliminating x0 in *-x0-*-x1 graph.
285  */
286 TEST(GaussianElimination, Eliminate_x0) {
287  Switching self(3);
288 
289  // Gather factors on x0, has a simple Gaussian and a hybrid factor.
291  // Add gaussian prior
292  factors.push_back(self.linearUnaryFactors[0]);
293  // Add first hybrid factor
294  factors.push_back(self.linearBinaryFactors[0]);
295 
296  // Eliminate x0
297  const Ordering ordering{X(0)};
298 
300  CHECK(result.first);
301  EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
302  CHECK(result.second);
303  // Has two keys, x2 and m1
304  EXPECT_LONGS_EQUAL(2, result.second->size());
305 }
306 
307 /****************************************************************************
308  * Test elimination function by eliminating x1 in x0-*-x1-*-x2 chain.
309  * m0/ \m1
310  */
311 TEST(HybridsGaussianElimination, Eliminate_x1) {
312  Switching self(3);
313 
314  // Gather factors on x1, will be two hybrid factors (with x0 and x2, resp.).
316  factors.push_back(self.linearBinaryFactors[0]); // involves m0
317  factors.push_back(self.linearBinaryFactors[1]); // involves m1
318 
319  // Eliminate x1
320  const Ordering ordering{X(1)};
321 
323  CHECK(result.first);
324  EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
325  CHECK(result.second);
326  // Note: separator keys should include m1, m2.
327  EXPECT_LONGS_EQUAL(4, result.second->size());
328 }
329 
330 /****************************************************************************
331  * Helper method to generate gaussian factor graphs with a specific mode.
332  */
334  Values linearizationPoint) {
336  graph.addPrior<double>(X(0), 0, Isotropic::Sigma(1, 0.1));
337 
338  auto between_x0_x1 = std::make_shared<MotionModel>(X(0), X(1), between,
339  Isotropic::Sigma(1, 1.0));
340 
341  graph.push_back(between_x0_x1);
342 
343  return graph.linearize(linearizationPoint);
344 }
345 
346 /****************************************************************************
347  * Test elimination function by eliminating x0 and x1 in graph.
348  */
349 TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
350  Switching self(2, 1.0, 0.1);
351 
352  auto factors = self.linearizedFactorGraph();
353 
354  // Eliminate x0
355  const Ordering ordering{X(0), X(1)};
356 
357  const auto [hybridConditional, factorOnModes] =
359 
360  auto hybridGaussianConditional =
361  dynamic_pointer_cast<HybridGaussianConditional>(
362  hybridConditional->inner());
363 
364  CHECK(hybridGaussianConditional);
365  // Frontals = [x0, x1]
366  EXPECT_LONGS_EQUAL(2, hybridGaussianConditional->nrFrontals());
367  // 1 parent, which is the mode
368  EXPECT_LONGS_EQUAL(1, hybridGaussianConditional->nrParents());
369 
370  // This is now a discreteFactor
371  auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
372  CHECK(discreteFactor);
373  EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
374  EXPECT(discreteFactor->root_->isLeaf() == false);
375 }
376 
377 /****************************************************************************
378  * Test partial elimination
379  */
380 TEST(HybridNonlinearFactorGraph, Partial_Elimination) {
381  Switching self(3);
382 
383  // Create ordering of only continuous variables.
385  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
386 
387  // Eliminate partially i.e. only continuous part.
388  const auto [hybridBayesNet, remainingFactorGraph] =
389  self.linearizedFactorGraph().eliminatePartialSequential(ordering);
390 
391  CHECK(hybridBayesNet);
392  EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
393  EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(0)});
394  EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(1), M(0)}));
395  EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(1)});
396  EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(2), M(0), M(1)}));
397  EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(2)});
398  EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(0), M(1)}));
399 
400  CHECK(remainingFactorGraph);
401  EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size());
402  EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(0)}));
403  EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(1), M(0)}));
404  EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)}));
405 }
406 
407 /* ****************************************************************************/
409  Switching self(3);
410  HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph();
411 
412  {
413  HybridValues values(VectorValues(), DiscreteValues{{M(0), 0}, {M(1), 0}},
414  self.linearizationPoint);
415  // regression
416  EXPECT_DOUBLES_EQUAL(152.791759469, fg.error(values), 1e-9);
417  }
418  {
419  HybridValues values(VectorValues(), DiscreteValues{{M(0), 0}, {M(1), 1}},
420  self.linearizationPoint);
421  // regression
422  EXPECT_DOUBLES_EQUAL(151.598612289, fg.error(values), 1e-9);
423  }
424  {
425  HybridValues values(VectorValues(), DiscreteValues{{M(0), 1}, {M(1), 0}},
426  self.linearizationPoint);
427  // regression
428  EXPECT_DOUBLES_EQUAL(151.703972804, fg.error(values), 1e-9);
429  }
430  {
431  HybridValues values(VectorValues(), DiscreteValues{{M(0), 1}, {M(1), 1}},
432  self.linearizationPoint);
433  // regression
434  EXPECT_DOUBLES_EQUAL(151.609437912, fg.error(values), 1e-9);
435  }
436 }
437 
438 /* ****************************************************************************/
440  Switching self(3);
441 
442  // Get nonlinear factor graph and add linear factors to be holistic.
443  // TODO(Frank): ???
444  HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph();
445  fg.add(self.linearizedFactorGraph());
446 
447  // Optimize to get HybridValues
449  self.linearizedFactorGraph().eliminateSequential();
450  HybridValues hv = bn->optimize();
451 
452  // Print and verify
453  // fg.print();
454  // std::cout << "\n\n\n" << std::endl;
455  // fg.printErrors(
456  // HybridValues(hv.continuous(), DiscreteValues(),
457  // self.linearizationPoint));
458 }
459 
460 /****************************************************************************
461  * Test full elimination
462  */
463 TEST(HybridNonlinearFactorGraph, Full_Elimination) {
464  Switching self(3);
465 
466  // We first do a partial elimination
467  DiscreteBayesNet discreteBayesNet;
468 
469  {
470  // Create ordering.
472  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
473 
474  // Eliminate partially.
475  const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
476  self.linearizedFactorGraph().eliminatePartialSequential(ordering);
477 
478  DiscreteFactorGraph discrete_fg;
479  // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
480  for (auto &factor : (*remainingFactorGraph_partial)) {
481  auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
482  assert(df);
483  discrete_fg.push_back(df);
484  }
485 
486  ordering.clear();
487  for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k));
488  discreteBayesNet =
490  }
491 
492  // Create ordering.
494  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
495  for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k));
496 
497  // Eliminate partially.
498  HybridBayesNet::shared_ptr hybridBayesNet =
499  self.linearizedFactorGraph().eliminateSequential(ordering);
500 
501  CHECK(hybridBayesNet);
502  EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
503  // p(x0 | x1, m0)
504  EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(0)});
505  EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(1), M(0)}));
506  // p(x1 | x2, m0, m1)
507  EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(1)});
508  EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(2), M(0), M(1)}));
509  // p(x2 | m0, m1)
510  EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(2)});
511  EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(0), M(1)}));
512  // P(m0 | m1)
513  EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(0)});
514  EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(1)}));
515  EXPECT(
516  dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
517  ->equals(*discreteBayesNet.at(0)));
518  // P(m1)
519  EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)});
520  EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
521  EXPECT(
522  dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
523  ->equals(*discreteBayesNet.at(1)));
524 }
525 
526 /****************************************************************************
527  * Test printing
528  */
530  Switching self(3);
531 
532  auto linearizedFactorGraph = self.linearizedFactorGraph();
533 
534  // Create ordering.
536  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
537 
538  // Eliminate partially.
539  const auto [hybridBayesNet, remainingFactorGraph] =
540  linearizedFactorGraph.eliminatePartialSequential(ordering);
541 
542 #ifdef GTSAM_DT_MERGING
543  string expected_hybridFactorGraph = R"(
544 size: 7
545 Factor 0
546 GaussianFactor:
547 
548  A[x0] = [
549  10
550 ]
551  b = [ -10 ]
552  No noise model
553 
554 Factor 1
555 GaussianFactor:
556 
557  A[x1] = [
558  10
559 ]
560  b = [ -10 ]
561  No noise model
562 
563 Factor 2
564 GaussianFactor:
565 
566  A[x2] = [
567  10
568 ]
569  b = [ -10 ]
570  No noise model
571 
572 Factor 3
573 HybridGaussianFactor:
574 Hybrid [x0 x1; m0]{
575  Choice(m0)
576  0 Leaf :
577  A[x0] = [
578  -1
579 ]
580  A[x1] = [
581  1
582 ]
583  b = [ -1 ]
584  No noise model
585 scalar: 0.918939
586 
587  1 Leaf :
588  A[x0] = [
589  -1
590 ]
591  A[x1] = [
592  1
593 ]
594  b = [ -0 ]
595  No noise model
596 scalar: 0.918939
597 
598 }
599 
600 Factor 4
601 HybridGaussianFactor:
602 Hybrid [x1 x2; m1]{
603  Choice(m1)
604  0 Leaf :
605  A[x1] = [
606  -1
607 ]
608  A[x2] = [
609  1
610 ]
611  b = [ -1 ]
612  No noise model
613 scalar: 0.918939
614 
615  1 Leaf :
616  A[x1] = [
617  -1
618 ]
619  A[x2] = [
620  1
621 ]
622  b = [ -0 ]
623  No noise model
624 scalar: 0.918939
625 
626 }
627 
628 Factor 5
629 DiscreteFactor:
630  P( m0 ):
631  Leaf 0.5
632 
633 
634 Factor 6
635 DiscreteFactor:
636  P( m1 | m0 ):
637  Choice(m1)
638  0 Choice(m0)
639  0 0 Leaf 0.33333333
640  0 1 Leaf 0.6
641  1 Choice(m0)
642  1 0 Leaf 0.66666667
643  1 1 Leaf 0.4
644 
645 
646 )";
647 #else
648  string expected_hybridFactorGraph = R"(
649 size: 7
650 Factor 0
651 GaussianFactor:
652 
653  A[x0] = [
654  10
655 ]
656  b = [ -10 ]
657  No noise model
658 
659 Factor 1
660 GaussianFactor:
661 
662  A[x1] = [
663  10
664 ]
665  b = [ -10 ]
666  No noise model
667 
668 Factor 2
669 GaussianFactor:
670 
671  A[x2] = [
672  10
673 ]
674  b = [ -10 ]
675  No noise model
676 
677 Factor 3
678 HybridGaussianFactor:
679 Hybrid [x0 x1; m0]{
680  Choice(m0)
681  0 Leaf :
682  A[x0] = [
683  -1
684 ]
685  A[x1] = [
686  1
687 ]
688  b = [ -1 ]
689  No noise model
690 scalar: 0.918939
691 
692  1 Leaf :
693  A[x0] = [
694  -1
695 ]
696  A[x1] = [
697  1
698 ]
699  b = [ -0 ]
700  No noise model
701 scalar: 0.918939
702 
703 }
704 
705 Factor 4
706 HybridGaussianFactor:
707 Hybrid [x1 x2; m1]{
708  Choice(m1)
709  0 Leaf :
710  A[x1] = [
711  -1
712 ]
713  A[x2] = [
714  1
715 ]
716  b = [ -1 ]
717  No noise model
718 scalar: 0.918939
719 
720  1 Leaf :
721  A[x1] = [
722  -1
723 ]
724  A[x2] = [
725  1
726 ]
727  b = [ -0 ]
728  No noise model
729 scalar: 0.918939
730 
731 }
732 
733 Factor 5
734 DiscreteFactor:
735  P( m0 ):
736  Choice(m0)
737  0 Leaf 0.5
738  1 Leaf 0.5
739 
740 
741 Factor 6
742 DiscreteFactor:
743  P( m1 | m0 ):
744  Choice(m1)
745  0 Choice(m0)
746  0 0 Leaf 0.33333333
747  0 1 Leaf 0.6
748  1 Choice(m0)
749  1 0 Leaf 0.66666667
750  1 1 Leaf 0.4
751 
752 
753 )";
754 #endif
755 
756  EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph));
757 
758  // Expected output for hybridBayesNet.
759  string expected_hybridBayesNet = R"(
760 size: 3
761 conditional 0: P( x0 | x1 m0)
762  Discrete Keys = (m0, 2),
763  logNormalizationConstant: 1.38862
764 
765  Choice(m0)
766  0 Leaf p(x0 | x1)
767  R = [ 10.0499 ]
768  S[x1] = [ -0.0995037 ]
769  d = [ -9.85087 ]
770  logNormalizationConstant: 1.38862
771  No noise model
772 
773  1 Leaf p(x0 | x1)
774  R = [ 10.0499 ]
775  S[x1] = [ -0.0995037 ]
776  d = [ -9.95037 ]
777  logNormalizationConstant: 1.38862
778  No noise model
779 
780 conditional 1: P( x1 | x2 m0 m1)
781  Discrete Keys = (m0, 2), (m1, 2),
782  logNormalizationConstant: 1.3935
783 
784  Choice(m1)
785  0 Choice(m0)
786  0 0 Leaf p(x1 | x2)
787  R = [ 10.099 ]
788  S[x2] = [ -0.0990196 ]
789  d = [ -9.99901 ]
790  logNormalizationConstant: 1.3935
791  No noise model
792 
793  0 1 Leaf p(x1 | x2)
794  R = [ 10.099 ]
795  S[x2] = [ -0.0990196 ]
796  d = [ -9.90098 ]
797  logNormalizationConstant: 1.3935
798  No noise model
799 
800  1 Choice(m0)
801  1 0 Leaf p(x1 | x2)
802  R = [ 10.099 ]
803  S[x2] = [ -0.0990196 ]
804  d = [ -10.098 ]
805  logNormalizationConstant: 1.3935
806  No noise model
807 
808  1 1 Leaf p(x1 | x2)
809  R = [ 10.099 ]
810  S[x2] = [ -0.0990196 ]
811  d = [ -10 ]
812  logNormalizationConstant: 1.3935
813  No noise model
814 
815 conditional 2: P( x2 | m0 m1)
816  Discrete Keys = (m0, 2), (m1, 2),
817  logNormalizationConstant: 1.38857
818 
819  Choice(m1)
820  0 Choice(m0)
821  0 0 Leaf p(x2)
822  R = [ 10.0494 ]
823  d = [ -10.1489 ]
824  mean: 1 elements
825  x2: -1.0099
826  logNormalizationConstant: 1.38857
827  No noise model
828 
829  0 1 Leaf p(x2)
830  R = [ 10.0494 ]
831  d = [ -10.1479 ]
832  mean: 1 elements
833  x2: -1.0098
834  logNormalizationConstant: 1.38857
835  No noise model
836 
837  1 Choice(m0)
838  1 0 Leaf p(x2)
839  R = [ 10.0494 ]
840  d = [ -10.0504 ]
841  mean: 1 elements
842  x2: -1.0001
843  logNormalizationConstant: 1.38857
844  No noise model
845 
846  1 1 Leaf p(x2)
847  R = [ 10.0494 ]
848  d = [ -10.0494 ]
849  mean: 1 elements
850  x2: -1
851  logNormalizationConstant: 1.38857
852  No noise model
853 
854 )";
855  EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet));
856 }
857 
858 /****************************************************************************
859  * Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose
860  * connects to 1 landmark) to expose issue with default decision tree creation
861  * in hybrid elimination. The hybrid factor is between the poses X0 and X1.
862  * The issue arises if we eliminate a landmark variable first since it is not
863  * connected to a HybridFactor.
864  */
865 TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
867 
868  // Add a prior on pose x0 at the origin.
869  // A prior factor consists of a mean and a noise model (covariance matrix)
870  Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
871  auto priorNoise = noiseModel::Diagonal::Sigmas(
872  Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
874 
875  using PlanarMotionModel = BetweenFactor<Pose2>;
876 
877  // Add odometry factor
878  Pose2 odometry(2.0, 0.0, 0.0);
879  auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
880  std::vector<NoiseModelFactor::shared_ptr> motion_models = {
881  std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
882  noise_model),
883  std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
884  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey{M(1), 2}, motion_models);
885 
886  // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
887  // create a noise model for the landmark measurements
888  auto measurementNoise = noiseModel::Diagonal::Sigmas(
889  Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
890  // create the measurement values - indices are (pose id, landmark id)
891  Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
892  double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
893 
894  // Add Bearing-Range factors
896  X(0), L(0), bearing11, range11, measurementNoise);
898  X(1), L(1), bearing22, range22, measurementNoise);
899 
900  // Create (deliberately inaccurate) initial estimate
901  Values initialEstimate;
902  initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2));
903  initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2));
904  initialEstimate.insert(L(0), Point2(1.8, 2.1));
905  initialEstimate.insert(L(1), Point2(4.1, 1.8));
906 
907  // We want to eliminate variables not connected to DCFactors first.
908  const Ordering ordering{L(0), L(1), X(0), X(1)};
909 
910  HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate);
911 
912  // This should NOT fail
913  const auto [hybridBayesNet, remainingFactorGraph] =
915  EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
916  EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());
917 }
918 
933  const std::vector<double> &means, const std::vector<double> &sigmas,
934  DiscreteKey &m1, double x0_measurement, double measurement_noise = 1e-3) {
935  auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]);
936  auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]);
937  auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
938 
939  auto f0 =
940  std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0);
941  auto f1 =
942  std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1);
943 
944  // Create HybridNonlinearFactor
945  // We take negative since we want
946  // the underlying scalar to be log(\sqrt(|2πΣ|))
947  std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
948 
949  HybridNonlinearFactor mixtureFactor(m1, factors);
950 
952  hfg.push_back(mixtureFactor);
953 
954  hfg.push_back(PriorFactor<double>(X(0), x0_measurement, prior_noise));
955 
956  return hfg;
957 }
958 } // namespace test_relinearization
959 
960 /* ************************************************************************* */
969  using namespace test_relinearization;
970 
971  DiscreteKey m1(M(1), 2);
972 
973  Values values;
974  double x0 = 0.0, x1 = 1.75;
975  values.insert(X(0), x0);
976  values.insert(X(1), x1);
977 
978  std::vector<double> means = {0.0, 2.0}, sigmas = {1e-0, 1e-0};
979 
981 
982  {
983  auto bn = hfg.linearize(values)->eliminateSequential();
984  HybridValues actual = bn->optimize();
985 
987  VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}},
988  DiscreteValues{{M(1), 0}});
989 
990  EXPECT(assert_equal(expected, actual));
991 
992  DiscreteValues dv0{{M(1), 0}};
993  VectorValues cont0 = bn->optimize(dv0);
994  double error0 = bn->error(HybridValues(cont0, dv0));
995 
996  // regression
997  EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9);
998 
999  DiscreteValues dv1{{M(1), 1}};
1000  VectorValues cont1 = bn->optimize(dv1);
1001  double error1 = bn->error(HybridValues(cont1, dv1));
1002  EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9);
1003  }
1004 
1005  {
1006  // Add measurement on x1
1007  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
1008  hfg.push_back(PriorFactor<double>(X(1), means[1], prior_noise));
1009 
1010  auto bn = hfg.linearize(values)->eliminateSequential();
1011  HybridValues actual = bn->optimize();
1012 
1014  VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
1015  DiscreteValues{{M(1), 1}});
1016 
1017  EXPECT(assert_equal(expected, actual));
1018 
1019  {
1020  DiscreteValues dv{{M(1), 0}};
1021  VectorValues cont = bn->optimize(dv);
1022  double error = bn->error(HybridValues(cont, dv));
1023  // regression
1024  EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9);
1025  }
1026  {
1027  DiscreteValues dv{{M(1), 1}};
1028  VectorValues cont = bn->optimize(dv);
1029  double error = bn->error(HybridValues(cont, dv));
1030  // regression
1031  EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9);
1032  }
1033  }
1034 }
1035 
1036 /* ************************************************************************* */
1044 TEST(HybridNonlinearFactorGraph, DifferentCovariances) {
1045  using namespace test_relinearization;
1046 
1047  DiscreteKey m1(M(1), 2);
1048 
1049  Values values;
1050  double x0 = 1.0, x1 = 1.0;
1051  values.insert(X(0), x0);
1052  values.insert(X(1), x1);
1053 
1054  std::vector<double> means = {0.0, 0.0}, sigmas = {1e2, 1e-2};
1055 
1056  // Create FG with HybridNonlinearFactor and prior on X1
1058  // Linearize
1059  auto hgfg = hfg.linearize(values);
1060  // and eliminate
1061  auto hbn = hgfg->eliminateSequential();
1062 
1063  VectorValues cv;
1064  cv.insert(X(0), Vector1(0.0));
1065  cv.insert(X(1), Vector1(0.0));
1066 
1067  DiscreteValues dv0{{M(1), 0}};
1068  DiscreteValues dv1{{M(1), 1}};
1069 
1070  DiscreteConditional expected_m1(m1, "0.5/0.5");
1071  DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
1072 
1073  EXPECT(assert_equal(expected_m1, actual_m1));
1074 }
1075 
1077  using namespace test_relinearization;
1078 
1079  DiscreteKey m1(M(1), 2);
1080 
1081  Values values;
1082  double x0 = 0.0, x1 = 0.8;
1083  values.insert(X(0), x0);
1084  values.insert(X(1), x1);
1085 
1086  std::vector<double> means = {0.0, 1.0}, sigmas = {1e-2, 1e-2};
1087 
1088  double prior_sigma = 1e-2;
1089  // Create FG with HybridNonlinearFactor and prior on X1
1091  CreateFactorGraph(means, sigmas, m1, 0.0, prior_sigma);
1093  X(1), 1.2, noiseModel::Isotropic::Sigma(1, prior_sigma)));
1094 
1095  // Linearize
1096  auto hgfg = hfg.linearize(values);
1097  // and eliminate
1098  auto hbn = hgfg->eliminateSequential();
1099 
1100  HybridValues delta = hbn->optimize();
1101  values = values.retract(delta.continuous());
1102 
1103  Values expected_first_result;
1104  expected_first_result.insert(X(0), 0.0666666666667);
1105  expected_first_result.insert(X(1), 1.13333333333);
1106  EXPECT(assert_equal(expected_first_result, values));
1107 
1108  // Re-linearize
1109  hgfg = hfg.linearize(values);
1110  // and eliminate
1111  hbn = hgfg->eliminateSequential();
1112  delta = hbn->optimize();
1113  HybridValues result(delta.continuous(), delta.discrete(),
1114  values.retract(delta.continuous()));
1115 
1116  HybridValues expected_result(
1117  VectorValues{{X(0), Vector1(0)}, {X(1), Vector1(0)}},
1118  DiscreteValues{{M(1), 1}}, expected_first_result);
1119  EXPECT(assert_equal(expected_result, result));
1120 }
1121 
1122 /* *************************************************************************
1123  */
1124 int main() {
1125  TestResult tr;
1126  return TestRegistry::runAllTests(tr);
1127 }
1128 /* *************************************************************************
1129  */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
Pose2.h
2D Pose
DiscreteBayesNet.h
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::HybridValues
Definition: HybridValues.h:37
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
gtsam::DiscreteFactorGraph
Definition: DiscreteFactorGraph.h:98
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
gtsam::Values::size
size_t size() const
Definition: Values.h:178
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Definition: FactorGraph-inst.h:66
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
HybridNonlinearFactorGraph.h
Nonlinear hybrid factor graph that uses type erasure.
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
TestHarness.h
DiscreteFactorGraph.h
gtsam::HybridNonlinearFactorGraph
Definition: HybridNonlinearFactorGraph.h:33
gtsam::HybridValues::retract
HybridValues retract(const VectorValues &delta) const
Definition: HybridValues.cpp:79
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:88
Switching.h
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
main
int main()
Definition: testHybridNonlinearFactorGraph.cpp:1124
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::EliminationTree
Definition: EliminationTree.h:51
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
test_relinearization::CreateFactorGraph
static HybridNonlinearFactorGraph CreateFactorGraph(const std::vector< double > &means, const std::vector< double > &sigmas, DiscreteKey &m1, double x0_measurement, double measurement_noise=1e-3)
Create a Factor Graph by directly specifying all the factors instead of creating conditionals first....
Definition: testHybridNonlinearFactorGraph.cpp:932
test_motion::noise_model
auto noise_model
Definition: testHybridNonlinearFactorGraph.cpp:119
gtsam::Switching::linearizedFactorGraph
HybridGaussianFactorGraph linearizedFactorGraph() const
Get the full linear factor graph.
Definition: Switching.h:218
utilities.h
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::AlgebraicDecisionTree< Key >
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::EliminationTree::roots
const FastVector< sharedNode > & roots() const
Definition: EliminationTree.h:152
gtsam::Switching
ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
Definition: Switching.h:122
odometry
Pose2 odometry(2.0, 0.0, 0.0)
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
PriorFactor.h
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
HybridFactor.h
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::DiscreteBayesNet
Definition: DiscreteBayesNet.h:38
gtsam::VectorValues
Definition: VectorValues.h:74
test_constructor::f0
auto f0
Definition: testHybridNonlinearFactor.cpp:55
BetweenFactor.h
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:151
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition: DiscreteFactorGraph.cpp:205
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
zero
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:296
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
x0
static Symbol x0('x', 0)
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:105
L
MatrixXd L
Definition: LLT_example.cpp:6
test_motion::m1
gtsam::DiscreteKey m1(M(1), 2)
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::EliminateHybrid
std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > > EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &keys)
Main elimination function for HybridGaussianFactorGraph.
Definition: HybridGaussianFactorGraph.cpp:432
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
test_relinearization
Definition: testHybridNonlinearFactorGraph.cpp:919
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::HybridEliminationTree
Definition: HybridEliminationTree.h:31
ordering
static enum @1096 ordering
TEST
TEST(HybridNonlinearFactorGraph, GaussianFactorGraph)
Definition: testHybridNonlinearFactorGraph.cpp:52
TestResult
Definition: TestResult.h:26
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
batchGFG
GaussianFactorGraph::shared_ptr batchGFG(double between, Values linearizationPoint)
Definition: testHybridNonlinearFactorGraph.cpp:333
HybridEliminationTree.h
HybridNonlinearFactor.h
A set of nonlinear factors indexed by a set of discrete keys.
gtsam::Rot2
Definition: Rot2.h:35
gtsam::DiscreteConditional
Definition: DiscreteConditional.h:37
gtsam
traits
Definition: SFMdata.h:40
switching3::graph2
const HybridGaussianFactorGraph graph2
Definition: testHybridGaussianISAM.cpp:53
error
static double error
Definition: testRot3.cpp:37
NoiseModel.h
test_motion
Definition: testHybridNonlinearFactorGraph.cpp:117
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
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
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::FactorGraph::end
const_iterator end() const
Definition: FactorGraph.h:342
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::HybridNonlinearFactorGraph::linearize
std::shared_ptr< HybridGaussianFactorGraph > linearize(const Values &continuousValues) const
Linearize all the continuous factors in the HybridNonlinearFactorGraph.
Definition: HybridNonlinearFactorGraph.cpp:139
switching3::graph1
const HybridGaussianFactorGraph graph1
Definition: testHybridGaussianISAM.cpp:50
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::FactorGraph::begin
const_iterator begin() const
Definition: FactorGraph.h:339
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:238
test_motion::components
std::vector< NoiseModelFactor::shared_ptr > components
Definition: testHybridNonlinearFactorGraph.cpp:120
gtsam::HybridBayesNet::shared_ptr
std::shared_ptr< HybridBayesNet > shared_ptr
Definition: HybridBayesNet.h:42
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
model1
noiseModel::Isotropic::shared_ptr model1
Definition: testEssentialMatrixFactor.cpp:26
gtsam::Ordering
Definition: inference/Ordering.h:33
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::HybridNonlinearFactor
Implementation of a discrete-conditioned hybrid factor.
Definition: HybridNonlinearFactor.h:58
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
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 Nov 1 2024 03:40:14