more code cleanup

gtsam/hybrid/HybridBayesTree.cpp

@@ -136,8 +136,7 @@ struct HybridAssignmentData {
   }
 };
 
-/* *************************************************************************
+/* ************************************************************************* */
- */
 GaussianBayesTree HybridBayesTree::choose(
     const DiscreteValues& assignment) const {
   GaussianBayesTree gbt;
@@ -157,8 +156,12 @@ GaussianBayesTree HybridBayesTree::choose(
   return gbt;
 }
 
-/* *************************************************************************
+/* ************************************************************************* */
- */
+double HybridBayesTree::error(const HybridValues& values) const {
+  return HybridGaussianFactorGraph(*this).error(values);
+}
+
+/* ************************************************************************* */
 VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
   GaussianBayesTree gbt = this->choose(assignment);
   // If empty GaussianBayesTree, means a clique is pruned hence invalid

gtsam/hybrid/HybridBayesTree.h

@@ -85,9 +85,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
   GaussianBayesTree choose(const DiscreteValues& assignment) const;
 
   /** Error for all conditionals. */
-  double error(const HybridValues& values) const {
+  double error(const HybridValues& values) const;
-    return HybridGaussianFactorGraph(*this).error(values);
-  }
 
   /**
    * @brief Optimize the hybrid Bayes tree by computing the MPE for the current

gtsam/hybrid/tests/testMixtureFactor.cpp

@@ -115,221 +115,6 @@ TEST(MixtureFactor, Dim) {
   EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
 }
 
-/* ************************************************************************* */
-// Test components with differing means
-TEST(MixtureFactor, DifferentMeans) {
-  DiscreteKey m1(M(1), 2), m2(M(2), 2);
-
-  Values values;
-  double x1 = 0.0, x2 = 1.75, x3 = 2.60;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-  values.insert(X(3), x3);
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
-
-  auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0);
-  auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
-  HybridNonlinearFactorGraph hnfg;
-  hnfg.push_back(mixtureFactor);
-
-  f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0);
-  f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors23{f0, f1};
-  hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23));
-
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise);
-  hnfg.push_back(prior);
-
-  hnfg.emplace_shared<PriorFactor<double>>(X(2), 2.0, prior_noise);
-
-  auto hgfg = hnfg.linearize(values);
-  auto bn = hgfg->eliminateSequential();
-  HybridValues actual = bn->optimize();
-
-  HybridValues expected(
-      VectorValues{
-          {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
-      DiscreteValues{{M(1), 1}, {M(2), 0}});
-
-  EXPECT(assert_equal(expected, actual));
-
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-}
-
-/* ************************************************************************* */
-// Test components with differing covariances
-TEST(MixtureFactor, DifferentCovariances) {
-  DiscreteKey m1(M(1), 2);
-
-  Values values;
-  double x1 = 1.0, x2 = 1.0;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-
-  double between = 0.0;
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  // Create via toFactorGraph
-  using symbol_shorthand::Z;
-  Matrix H0_1, H0_2, H1_1, H1_2;
-  Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
-  std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H0_1 /*Sp1*/},
-                                                {X(2), H0_2 /*Tp2*/}};
-
-  Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
-  std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H1_1 /*Sp1*/},
-                                                {X(2), H1_2 /*Tp2*/}};
-  auto gm = new gtsam::GaussianMixture(
-      {Z(1)}, {X(1), X(2)}, {m1},
-      {std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
-       std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
-  gtsam::HybridBayesNet bn;
-  bn.emplace_back(gm);
-
-  gtsam::VectorValues measurements;
-  measurements.insert(Z(1), gtsam::Z_1x1);
-  // Create FG with single GaussianMixtureFactor
-  HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
-
-  // Linearized prior factor on X1
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  mixture_fg.push_back(prior);
-
-  auto hbn = mixture_fg.eliminateSequential();
-
-  VectorValues cv;
-  cv.insert(X(1), Vector1(0.0));
-  cv.insert(X(2), Vector1(0.0));
-
-  // Check that we get different error values at the MLE point μ.
-  AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
-
-  HybridValues hv0(cv, DiscreteValues{{M(1), 0}});
-  HybridValues hv1(cv, DiscreteValues{{M(1), 1}});
-
-  auto cond0 = hbn->at(0)->asMixture();
-  auto cond1 = hbn->at(1)->asMixture();
-  auto discrete_cond = hbn->at(2)->asDiscrete();
-  AlgebraicDecisionTree<Key> expectedErrorTree(m1, 9.90348755254,
-                                               0.69314718056);
-  EXPECT(assert_equal(expectedErrorTree, errorTree));
-}
-
-/* ************************************************************************* */
-// Test components with differing means and covariances
-TEST(MixtureFactor, DifferentMeansAndCovariances) {
-  DiscreteKey m1(M(1), 2);
-
-  Values values;
-  double x1 = 0.0, x2 = 7.0;
-  values.insert(X(1), x1);
-
-  double between = 0.0;
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  // Create via toFactorGraph
-  using symbol_shorthand::Z;
-  Matrix H0_1, H0_2, H1_1, H1_2;
-  Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
-  std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H0_1 /*Sp1*/},
-                                                {X(2), H0_2 /*Tp2*/}};
-
-  Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
-  std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H1_1 /*Sp1*/},
-                                                {X(2), H1_2 /*Tp2*/}};
-  auto gm = new gtsam::GaussianMixture(
-      {Z(1)}, {X(1), X(2)}, {m1},
-      {std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
-       std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
-  gtsam::HybridBayesNet bn;
-  bn.emplace_back(gm);
-
-  gtsam::VectorValues measurements;
-  measurements.insert(Z(1), gtsam::Z_1x1);
-  // Create FG with single GaussianMixtureFactor
-  HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
-
-  // Linearized prior factor on X1
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  mixture_fg.push_back(prior);
-
-  VectorValues vv{{X(1), x1 * I_1x1}, {X(2), x2 * I_1x1}};
-
-  auto hbn = mixture_fg.eliminateSequential();
-
-  HybridValues actual_values = hbn->optimize();
-
-  VectorValues cv;
-  cv.insert(X(1), Vector1(0.0));
-  cv.insert(X(2), Vector1(-7.0));
-
-  // The first value is chosen as the tiebreaker
-  DiscreteValues dv;
-  dv.insert({M(1), 0});
-  HybridValues expected_values(cv, dv);
-
-  EXPECT(assert_equal(expected_values, actual_values));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;