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  remainingFactorGraph_partial->discreteFactors();
480 
481  ordering.clear();
482  for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k));
483  discreteBayesNet =
485  }
486 
487  // Create ordering.
489  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
490  for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k));
491 
492  // Eliminate partially.
493  HybridBayesNet::shared_ptr hybridBayesNet =
494  self.linearizedFactorGraph().eliminateSequential(ordering);
495 
496  CHECK(hybridBayesNet);
497  EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
498  // p(x0 | x1, m0)
499  EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(0)});
500  EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(1), M(0)}));
501  // p(x1 | x2, m0, m1)
502  EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(1)});
503  EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(2), M(0), M(1)}));
504  // p(x2 | m0, m1)
505  EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(2)});
506  EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(0), M(1)}));
507  // P(m0 | m1)
508  EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(0)});
509  EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(1)}));
510  EXPECT(
511  dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
512  ->equals(*discreteBayesNet.at(0)));
513  // P(m1)
514  EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)});
515  EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
516  EXPECT(
517  dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
518  ->equals(*discreteBayesNet.at(1)));
519 }
520 
521 /****************************************************************************
522  * Test printing
523  */
525  Switching self(3);
526 
527  auto linearizedFactorGraph = self.linearizedFactorGraph();
528 
529  // Create ordering.
531  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
532 
533  // Eliminate partially.
534  const auto [hybridBayesNet, remainingFactorGraph] =
535  linearizedFactorGraph.eliminatePartialSequential(ordering);
536 
537 #ifdef GTSAM_DT_MERGING
538  string expected_hybridFactorGraph = R"(
539 size: 7
540 Factor 0
541 GaussianFactor:
542 
543  A[x0] = [
544  10
545 ]
546  b = [ -10 ]
547  No noise model
548 
549 Factor 1
550 GaussianFactor:
551 
552  A[x1] = [
553  10
554 ]
555  b = [ -10 ]
556  No noise model
557 
558 Factor 2
559 GaussianFactor:
560 
561  A[x2] = [
562  10
563 ]
564  b = [ -10 ]
565  No noise model
566 
567 Factor 3
568 HybridGaussianFactor:
569 Hybrid [x0 x1; m0]{
570  Choice(m0)
571  0 Leaf :
572  A[x0] = [
573  -1
574 ]
575  A[x1] = [
576  1
577 ]
578  b = [ -1 ]
579  No noise model
580 scalar: 0.918939
581 
582  1 Leaf :
583  A[x0] = [
584  -1
585 ]
586  A[x1] = [
587  1
588 ]
589  b = [ -0 ]
590  No noise model
591 scalar: 0.918939
592 
593 }
594 
595 Factor 4
596 HybridGaussianFactor:
597 Hybrid [x1 x2; m1]{
598  Choice(m1)
599  0 Leaf :
600  A[x1] = [
601  -1
602 ]
603  A[x2] = [
604  1
605 ]
606  b = [ -1 ]
607  No noise model
608 scalar: 0.918939
609 
610  1 Leaf :
611  A[x1] = [
612  -1
613 ]
614  A[x2] = [
615  1
616 ]
617  b = [ -0 ]
618  No noise model
619 scalar: 0.918939
620 
621 }
622 
623 Factor 5
624 DiscreteFactor:
625  P( m0 ):
626  Leaf 0.5
627 
628 
629 Factor 6
630 DiscreteFactor:
631  P( m1 | m0 ):
632  Choice(m1)
633  0 Choice(m0)
634  0 0 Leaf 0.33333333
635  0 1 Leaf 0.6
636  1 Choice(m0)
637  1 0 Leaf 0.66666667
638  1 1 Leaf 0.4
639 
640 
641 )";
642 #else
643  string expected_hybridFactorGraph = R"(
644 size: 7
645 Factor 0
646 GaussianFactor:
647 
648  A[x0] = [
649  10
650 ]
651  b = [ -10 ]
652  No noise model
653 
654 Factor 1
655 GaussianFactor:
656 
657  A[x1] = [
658  10
659 ]
660  b = [ -10 ]
661  No noise model
662 
663 Factor 2
664 GaussianFactor:
665 
666  A[x2] = [
667  10
668 ]
669  b = [ -10 ]
670  No noise model
671 
672 Factor 3
673 HybridGaussianFactor:
674 Hybrid [x0 x1; m0]{
675  Choice(m0)
676  0 Leaf :
677  A[x0] = [
678  -1
679 ]
680  A[x1] = [
681  1
682 ]
683  b = [ -1 ]
684  No noise model
685 scalar: 0.918939
686 
687  1 Leaf :
688  A[x0] = [
689  -1
690 ]
691  A[x1] = [
692  1
693 ]
694  b = [ -0 ]
695  No noise model
696 scalar: 0.918939
697 
698 }
699 
700 Factor 4
701 HybridGaussianFactor:
702 Hybrid [x1 x2; m1]{
703  Choice(m1)
704  0 Leaf :
705  A[x1] = [
706  -1
707 ]
708  A[x2] = [
709  1
710 ]
711  b = [ -1 ]
712  No noise model
713 scalar: 0.918939
714 
715  1 Leaf :
716  A[x1] = [
717  -1
718 ]
719  A[x2] = [
720  1
721 ]
722  b = [ -0 ]
723  No noise model
724 scalar: 0.918939
725 
726 }
727 
728 Factor 5
729 DiscreteFactor:
730  P( m0 ):
731  Choice(m0)
732  0 Leaf 0.5
733  1 Leaf 0.5
734 
735 
736 Factor 6
737 DiscreteFactor:
738  P( m1 | m0 ):
739  Choice(m1)
740  0 Choice(m0)
741  0 0 Leaf 0.33333333
742  0 1 Leaf 0.6
743  1 Choice(m0)
744  1 0 Leaf 0.66666667
745  1 1 Leaf 0.4
746 
747 
748 )";
749 #endif
750 
751  EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph));
752 
753  // Expected output for hybridBayesNet.
754  string expected_hybridBayesNet = R"(
755 size: 3
756 conditional 0: P( x0 | x1 m0)
757  Discrete Keys = (m0, 2),
758  logNormalizationConstant: 1.38862
759 
760  Choice(m0)
761  0 Leaf p(x0 | x1)
762  R = [ 10.0499 ]
763  S[x1] = [ -0.0995037 ]
764  d = [ -9.85087 ]
765  logNormalizationConstant: 1.38862
766  No noise model
767 
768  1 Leaf p(x0 | x1)
769  R = [ 10.0499 ]
770  S[x1] = [ -0.0995037 ]
771  d = [ -9.95037 ]
772  logNormalizationConstant: 1.38862
773  No noise model
774 
775 conditional 1: P( x1 | x2 m0 m1)
776  Discrete Keys = (m0, 2), (m1, 2),
777  logNormalizationConstant: 1.3935
778 
779  Choice(m1)
780  0 Choice(m0)
781  0 0 Leaf p(x1 | x2)
782  R = [ 10.099 ]
783  S[x2] = [ -0.0990196 ]
784  d = [ -9.99901 ]
785  logNormalizationConstant: 1.3935
786  No noise model
787 
788  0 1 Leaf p(x1 | x2)
789  R = [ 10.099 ]
790  S[x2] = [ -0.0990196 ]
791  d = [ -9.90098 ]
792  logNormalizationConstant: 1.3935
793  No noise model
794 
795  1 Choice(m0)
796  1 0 Leaf p(x1 | x2)
797  R = [ 10.099 ]
798  S[x2] = [ -0.0990196 ]
799  d = [ -10.098 ]
800  logNormalizationConstant: 1.3935
801  No noise model
802 
803  1 1 Leaf p(x1 | x2)
804  R = [ 10.099 ]
805  S[x2] = [ -0.0990196 ]
806  d = [ -10 ]
807  logNormalizationConstant: 1.3935
808  No noise model
809 
810 conditional 2: P( x2 | m0 m1)
811  Discrete Keys = (m0, 2), (m1, 2),
812  logNormalizationConstant: 1.38857
813 
814  Choice(m1)
815  0 Choice(m0)
816  0 0 Leaf p(x2)
817  R = [ 10.0494 ]
818  d = [ -10.1489 ]
819  mean: 1 elements
820  x2: -1.0099
821  logNormalizationConstant: 1.38857
822  No noise model
823 
824  0 1 Leaf p(x2)
825  R = [ 10.0494 ]
826  d = [ -10.1479 ]
827  mean: 1 elements
828  x2: -1.0098
829  logNormalizationConstant: 1.38857
830  No noise model
831 
832  1 Choice(m0)
833  1 0 Leaf p(x2)
834  R = [ 10.0494 ]
835  d = [ -10.0504 ]
836  mean: 1 elements
837  x2: -1.0001
838  logNormalizationConstant: 1.38857
839  No noise model
840 
841  1 1 Leaf p(x2)
842  R = [ 10.0494 ]
843  d = [ -10.0494 ]
844  mean: 1 elements
845  x2: -1
846  logNormalizationConstant: 1.38857
847  No noise model
848 
849 )";
850  EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet));
851 }
852 
853 /****************************************************************************
854  * Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose
855  * connects to 1 landmark) to expose issue with default decision tree creation
856  * in hybrid elimination. The hybrid factor is between the poses X0 and X1.
857  * The issue arises if we eliminate a landmark variable first since it is not
858  * connected to a HybridFactor.
859  */
860 TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
862 
863  // Add a prior on pose x0 at the origin.
864  // A prior factor consists of a mean and a noise model (covariance matrix)
865  Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
866  auto priorNoise = noiseModel::Diagonal::Sigmas(
867  Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
869 
870  using PlanarMotionModel = BetweenFactor<Pose2>;
871 
872  // Add odometry factor
873  Pose2 odometry(2.0, 0.0, 0.0);
874  auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
875  std::vector<NoiseModelFactor::shared_ptr> motion_models = {
876  std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
877  noise_model),
878  std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
879  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey{M(1), 2}, motion_models);
880 
881  // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
882  // create a noise model for the landmark measurements
883  auto measurementNoise = noiseModel::Diagonal::Sigmas(
884  Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
885  // create the measurement values - indices are (pose id, landmark id)
886  Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
887  double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
888 
889  // Add Bearing-Range factors
891  X(0), L(0), bearing11, range11, measurementNoise);
893  X(1), L(1), bearing22, range22, measurementNoise);
894 
895  // Create (deliberately inaccurate) initial estimate
896  Values initialEstimate;
897  initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2));
898  initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2));
899  initialEstimate.insert(L(0), Point2(1.8, 2.1));
900  initialEstimate.insert(L(1), Point2(4.1, 1.8));
901 
902  // We want to eliminate variables not connected to DCFactors first.
903  const Ordering ordering{L(0), L(1), X(0), X(1)};
904 
905  HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate);
906 
907  // This should NOT fail
908  const auto [hybridBayesNet, remainingFactorGraph] =
910  EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
911  EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());
912 }
913 
928  const std::vector<double> &means, const std::vector<double> &sigmas,
929  DiscreteKey &m1, double x0_measurement, double measurement_noise = 1e-3) {
930  auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]);
931  auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]);
932  auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
933 
934  auto f0 =
935  std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0);
936  auto f1 =
937  std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1);
938 
939  // Create HybridNonlinearFactor
940  // We take negative since we want
941  // the underlying scalar to be log(\sqrt(|2πΣ|))
942  std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
943 
944  HybridNonlinearFactor mixtureFactor(m1, factors);
945 
947  hfg.push_back(mixtureFactor);
948 
949  hfg.push_back(PriorFactor<double>(X(0), x0_measurement, prior_noise));
950 
951  return hfg;
952 }
953 } // namespace test_relinearization
954 
955 /* ************************************************************************* */
964  using namespace test_relinearization;
965 
966  DiscreteKey m1(M(1), 2);
967 
968  Values values;
969  double x0 = 0.0, x1 = 1.75;
970  values.insert(X(0), x0);
971  values.insert(X(1), x1);
972 
973  std::vector<double> means = {0.0, 2.0}, sigmas = {1e-0, 1e-0};
974 
976 
977  {
978  auto bn = hfg.linearize(values)->eliminateSequential();
979  HybridValues actual = bn->optimize();
980 
982  VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}},
983  DiscreteValues{{M(1), 0}});
984 
985  EXPECT(assert_equal(expected, actual));
986 
987  DiscreteValues dv0{{M(1), 0}};
988  VectorValues cont0 = bn->optimize(dv0);
989  double error0 = bn->error(HybridValues(cont0, dv0));
990 
991  // regression
992  EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9);
993 
994  DiscreteValues dv1{{M(1), 1}};
995  VectorValues cont1 = bn->optimize(dv1);
996  double error1 = bn->error(HybridValues(cont1, dv1));
997  EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9);
998  }
999 
1000  {
1001  // Add measurement on x1
1002  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
1003  hfg.push_back(PriorFactor<double>(X(1), means[1], prior_noise));
1004 
1005  auto bn = hfg.linearize(values)->eliminateSequential();
1006  HybridValues actual = bn->optimize();
1007 
1009  VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
1010  DiscreteValues{{M(1), 1}});
1011 
1012  EXPECT(assert_equal(expected, actual));
1013 
1014  {
1015  DiscreteValues dv{{M(1), 0}};
1016  VectorValues cont = bn->optimize(dv);
1017  double error = bn->error(HybridValues(cont, dv));
1018  // regression
1019  EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9);
1020  }
1021  {
1022  DiscreteValues dv{{M(1), 1}};
1023  VectorValues cont = bn->optimize(dv);
1024  double error = bn->error(HybridValues(cont, dv));
1025  // regression
1026  EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9);
1027  }
1028  }
1029 }
1030 
1031 /* ************************************************************************* */
1039 TEST(HybridNonlinearFactorGraph, DifferentCovariances) {
1040  using namespace test_relinearization;
1041 
1042  DiscreteKey m1(M(1), 2);
1043 
1044  Values values;
1045  double x0 = 1.0, x1 = 1.0;
1046  values.insert(X(0), x0);
1047  values.insert(X(1), x1);
1048 
1049  std::vector<double> means = {0.0, 0.0}, sigmas = {1e2, 1e-2};
1050 
1051  // Create FG with HybridNonlinearFactor and prior on X1
1053  // Linearize
1054  auto hgfg = hfg.linearize(values);
1055  // and eliminate
1056  auto hbn = hgfg->eliminateSequential();
1057 
1058  VectorValues cv;
1059  cv.insert(X(0), Vector1(0.0));
1060  cv.insert(X(1), Vector1(0.0));
1061 
1062  DiscreteValues dv0{{M(1), 0}};
1063  DiscreteValues dv1{{M(1), 1}};
1064 
1065  DiscreteConditional expected_m1(m1, "0.5/0.5");
1066  DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
1067 
1068  EXPECT(assert_equal(expected_m1, actual_m1));
1069 }
1070 
1072  using namespace test_relinearization;
1073 
1074  DiscreteKey m1(M(1), 2);
1075 
1076  Values values;
1077  double x0 = 0.0, x1 = 0.8;
1078  values.insert(X(0), x0);
1079  values.insert(X(1), x1);
1080 
1081  std::vector<double> means = {0.0, 1.0}, sigmas = {1e-2, 1e-2};
1082 
1083  double prior_sigma = 1e-2;
1084  // Create FG with HybridNonlinearFactor and prior on X1
1086  CreateFactorGraph(means, sigmas, m1, 0.0, prior_sigma);
1088  X(1), 1.2, noiseModel::Isotropic::Sigma(1, prior_sigma)));
1089 
1090  // Linearize
1091  auto hgfg = hfg.linearize(values);
1092  // and eliminate
1093  auto hbn = hgfg->eliminateSequential();
1094 
1095  HybridValues delta = hbn->optimize();
1096  values = values.retract(delta.continuous());
1097 
1098  Values expected_first_result;
1099  expected_first_result.insert(X(0), 0.0666666666667);
1100  expected_first_result.insert(X(1), 1.13333333333);
1101  EXPECT(assert_equal(expected_first_result, values));
1102 
1103  // Re-linearize
1104  hgfg = hfg.linearize(values);
1105  // and eliminate
1106  hbn = hgfg->eliminateSequential();
1107  delta = hbn->optimize();
1108  HybridValues result(delta.continuous(), delta.discrete(),
1109  values.retract(delta.continuous()));
1110 
1111  HybridValues expected_result(
1112  VectorValues{{X(0), Vector1(0)}, {X(1), Vector1(0)}},
1113  DiscreteValues{{M(1), 1}}, expected_first_result);
1114  EXPECT(assert_equal(expected_result, result));
1115 }
1116 
1117 /* *************************************************************************
1118  */
1119 int main() {
1120  TestResult tr;
1121  return TestRegistry::runAllTests(tr);
1122 }
1123 /* *************************************************************************
1124  */
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:90
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:1119
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:927
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:220
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:124
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:106
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:433
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
test_relinearization
Definition: testHybridNonlinearFactorGraph.cpp:914
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 Sat Nov 16 2024 04:07:38