Merge pull request #1851 from borglab/feature/linearize

Better HybridNonlinear!

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
+#include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <memory>
@@ -29,7 +30,7 @@ struct HybridNonlinearFactor::ConstructorHelper {
   DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
   FactorValuePairs factorTree;
 
-  void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
+  void copyOrCheckContinuousKeys(const NoiseModelFactor::shared_ptr& factor) {
     if (!factor) return;
     if (continuousKeys.empty()) {
       continuousKeys = factor->keys();
@@ -40,7 +41,7 @@ struct HybridNonlinearFactor::ConstructorHelper {
   }
 
   ConstructorHelper(const DiscreteKey& discreteKey,
-                    const std::vector<NonlinearFactor::shared_ptr>& factors)
+                    const std::vector<NoiseModelFactor::shared_ptr>& factors)
       : discreteKeys({discreteKey}) {
     std::vector<NonlinearFactorValuePair> pairs;
     // Extract continuous keys from the first non-null factor
@@ -78,7 +79,7 @@ HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)
 
 HybridNonlinearFactor::HybridNonlinearFactor(
     const DiscreteKey& discreteKey,
-    const std::vector<NonlinearFactor::shared_ptr>& factors)
+    const std::vector<NoiseModelFactor::shared_ptr>& factors)
     : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
 
 HybridNonlinearFactor::HybridNonlinearFactor(
@@ -158,8 +159,7 @@ bool HybridNonlinearFactor::equals(const HybridFactor& other,
   // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
   auto compare = [tol](const std::pair<sharedFactor, double>& a,
                        const std::pair<sharedFactor, double>& b) {
-    return traits<NonlinearFactor>::Equals(*a.first, *b.first, tol) &&
+    return a.first->equals(*b.first, tol) && (a.second == b.second);
-           (a.second == b.second);
   };
   if (!factors_.equals(f.factors_, compare)) return false;
 
@@ -185,7 +185,15 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
       [continuousValues](
           const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair {
     auto [factor, val] = f;
-    return {factor->linearize(continuousValues), val};
+    if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
+            factor->noiseModel())) {
+      return {factor->linearize(continuousValues),
+              val + gaussian->negLogConstant()};
+    } else {
+      throw std::runtime_error(
+          "HybridNonlinearFactor: linearize() only supports NoiseModelFactors "
+          "with Gaussian (or derived) noise models.");
+    }
   };
 
   DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>

gtsam/hybrid/HybridNonlinearFactor.h

@@ -26,25 +26,23 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Symbol.h>
 
-#include <algorithm>
-#include <cmath>
-#include <limits>
 #include <vector>
 
 namespace gtsam {
 
-/// Alias for a NonlinearFactor shared pointer and double scalar pair.
+/// Alias for a NoiseModelFactor shared pointer and double scalar pair.
-using NonlinearFactorValuePair = std::pair<NonlinearFactor::shared_ptr, double>;
+using NonlinearFactorValuePair =
+    std::pair<NoiseModelFactor::shared_ptr, double>;
 
 /**
  * @brief Implementation of a discrete-conditioned hybrid factor.
  *
  * Implements a joint discrete-continuous factor where the discrete variable
- * serves to "select" a hybrid component corresponding to a NonlinearFactor.
+ * serves to "select" a hybrid component corresponding to a NoiseModelFactor.
  *
  * This class stores all factors as HybridFactors which can then be typecast to
- * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
+ * one of (NoiseModelFactor, GaussianFactor) which can then be checked to
- * the correct operation.
+ * perform the correct operation.
  *
  * In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e.,
  * the negative log-likelihood for a Gaussian noise model.
@@ -62,11 +60,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
   using Base = HybridFactor;
   using This = HybridNonlinearFactor;
   using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
-  using sharedFactor = std::shared_ptr<NonlinearFactor>;
+  using sharedFactor = std::shared_ptr<NoiseModelFactor>;
 
   /**
    * @brief typedef for DecisionTree which has Keys as node labels and
-   * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
+   * pairs of NoiseModelFactor & an arbitrary scalar as leaf nodes.
    */
   using FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>;
 
@@ -95,7 +93,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    */
   HybridNonlinearFactor(
       const DiscreteKey& discreteKey,
-      const std::vector<NonlinearFactor::shared_ptr>& factors);
+      const std::vector<NoiseModelFactor::shared_ptr>& factors);
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a single discrete key,

gtsam/hybrid/hybrid.i

@@ -245,16 +245,16 @@ class HybridNonlinearFactorGraph {
 class HybridNonlinearFactor : gtsam::HybridFactor {
   HybridNonlinearFactor(
       const gtsam::DiscreteKey& discreteKey,
-      const std::vector<gtsam::NonlinearFactor*>& factors);
+      const std::vector<gtsam::NoiseModelFactor*>& factors);
 
   HybridNonlinearFactor(
       const gtsam::DiscreteKey& discreteKey,
-      const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
+      const std::vector<std::pair<gtsam::NoiseModelFactor*, double>>& factors);
 
   HybridNonlinearFactor(
       const gtsam::DiscreteKeys& discreteKeys,
       const gtsam::DecisionTree<
-          gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
+          gtsam::Key, std::pair<gtsam::NoiseModelFactor*, double>>& factors);
 
   double error(const gtsam::Values& continuousValues,
                const gtsam::DiscreteValues& discreteValues) const;

gtsam/hybrid/tests/Switching.h

@@ -33,6 +33,7 @@
 
 #include "gtsam/linear/GaussianFactor.h"
 #include "gtsam/linear/GaussianFactorGraph.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 #pragma once
 
@@ -185,7 +186,7 @@ struct Switching {
   }
 
   // Create motion models for a given time step
-  static std::vector<NonlinearFactor::shared_ptr> motionModels(
+  static std::vector<NoiseModelFactor::shared_ptr> motionModels(
       size_t k, double sigma = 1.0) {
     auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
     auto still =

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -24,6 +24,7 @@
 
 #include "Switching.h"
 #include "TinyHybridExample.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -389,7 +390,7 @@ TEST(HybridBayesNet, Sampling) {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
       DiscreteKey(M(0), 2),
-      std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
+      std::vector<NoiseModelFactor::shared_ptr>{zero_motion, one_motion});
 
   DiscreteKey mode(M(0), 2);
   nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -39,6 +39,7 @@
 #include <bitset>
 
 #include "Switching.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 using namespace std;
 using namespace gtsam;
@@ -435,7 +436,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   const auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
+  std::vector<NoiseModelFactor::shared_ptr> components = {zero_motion,
                                                           one_motion};
   nfg.emplace_shared<HybridNonlinearFactor>(m, components);
 
@@ -526,49 +527,6 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
   }
 }
 
-/****************************************************************************/
-/**
- * Helper function to add the constant term corresponding to
- * the difference in noise models.
- */
-std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
-    const HybridNonlinearFactor& mf, const Values& initial, const Key& mode,
-    double noise_tight, double noise_loose, size_t d, size_t tight_index) {
-  HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial);
-
-  constexpr double log2pi = 1.8378770664093454835606594728112;
-  // logConstant will be of the tighter model
-  double logNormalizationConstant = log(1.0 / noise_tight);
-  double logConstant = -0.5 * d * log2pi + logNormalizationConstant;
-
-  auto func = [&](const Assignment<Key>& assignment,
-                  const GaussianFactor::shared_ptr& gf) {
-    if (assignment.at(mode) != tight_index) {
-      double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose);
-
-      GaussianFactorGraph _gfg;
-      _gfg.push_back(gf);
-      Vector c(d);
-      for (size_t i = 0; i < d; i++) {
-        c(i) = std::sqrt(2.0 * (logConstant - factor_log_constant));
-      }
-
-      _gfg.emplace_shared<JacobianFactor>(c);
-      return std::make_shared<JacobianFactor>(_gfg);
-    } else {
-      return dynamic_pointer_cast<JacobianFactor>(gf);
-    }
-  };
-  auto updated_components = gmf->factors().apply(func);
-  auto updated_pairs = HybridGaussianFactor::FactorValuePairs(
-      updated_components,
-      [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
-        return {gf, 0.0};
-      });
-  return std::make_shared<HybridGaussianFactor>(gmf->discreteKeys(),
-                                                updated_pairs);
-}
-
 /****************************************************************************/
 TEST(HybridEstimation, ModeSelection) {
   HybridNonlinearFactorGraph graph;
@@ -588,15 +546,14 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<MotionModel>(
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+  std::vector<NoiseModelFactor::shared_ptr> components = {model0, model1};
 
   HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert(X(0), 0.0);
   initial.insert(X(1), 0.0);
 
-  auto gmf =
+  auto gmf = mf.linearize(initial);
-      mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
   graph.add(gmf);
 
   auto gfg = graph.linearize(initial);
@@ -676,15 +633,14 @@ TEST(HybridEstimation, ModeSelection2) {
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+  std::vector<NoiseModelFactor::shared_ptr> components = {model0, model1};
 
   HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert<Vector3>(X(0), Z_3x1);
   initial.insert<Vector3>(X(1), Z_3x1);
 
-  auto gmf =
+  auto gmf = mf.linearize(initial);
-      mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
   graph.add(gmf);
 
   auto gfg = graph.linearize(initial);

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -208,354 +208,7 @@ TEST(HybridGaussianFactor, Error) {
       4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9);
 }
 
-namespace test_two_state_estimation {
-
-DiscreteKey m1(M(1), 2);
-
-void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) {
-  auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma);
-  hbn.emplace_shared<GaussianConditional>(z_key, Vector1(0.0), I_1x1, x_key,
-                                          -I_1x1, measurement_model);
-}
-
-/// Create hybrid motion model p(x1 | x0, m1)
-static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
-    double mu0, double mu1, double sigma0, double sigma1) {
-  auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
-  auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
-  auto c0 = make_shared<GaussianConditional>(X(1), Vector1(mu0), I_1x1, X(0),
-                                             -I_1x1, model0),
-       c1 = make_shared<GaussianConditional>(X(1), Vector1(mu1), I_1x1, X(0),
-                                             -I_1x1, model1);
-  DiscreteKeys discreteParents{m1};
-  return std::make_shared<HybridGaussianConditional>(
-      discreteParents, HybridGaussianConditional::Conditionals(
-                           discreteParents, std::vector{c0, c1}));
-}
-
-/// Create two state Bayes network with 1 or two measurement models
-HybridBayesNet CreateBayesNet(
-    const HybridGaussianConditional::shared_ptr &hybridMotionModel,
-    bool add_second_measurement = false) {
-  HybridBayesNet hbn;
-
-  // Add measurement model p(z0 | x0)
-  addMeasurement(hbn, Z(0), X(0), 3.0);
-
-  // Optionally add second measurement model p(z1 | x1)
-  if (add_second_measurement) {
-    addMeasurement(hbn, Z(1), X(1), 3.0);
-  }
-
-  // Add hybrid motion model
-  hbn.push_back(hybridMotionModel);
-
-  // Discrete uniform prior.
-  hbn.emplace_shared<DiscreteConditional>(m1, "50/50");
-
-  return hbn;
-}
-
-/// Approximate the discrete marginal P(m1) using importance sampling
-std::pair<double, double> approximateDiscreteMarginal(
-    const HybridBayesNet &hbn,
-    const HybridGaussianConditional::shared_ptr &hybridMotionModel,
-    const VectorValues &given, size_t N = 100000) {
-  /// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1),
-  /// using q(x0) = N(z0, sigmaQ) to sample x0.
-  HybridBayesNet q;
-  q.push_back(hybridMotionModel);  // Add hybrid motion model
-  q.emplace_shared<GaussianConditional>(GaussianConditional::FromMeanAndStddev(
-      X(0), given.at(Z(0)), /* sigmaQ = */ 3.0));  // Add proposal q(x0) for x0
-  q.emplace_shared<DiscreteConditional>(m1, "50/50");  // Discrete prior.
-
-  // Do importance sampling
-  double w0 = 0.0, w1 = 0.0;
-  std::mt19937_64 rng(42);
-  for (int i = 0; i < N; i++) {
-    HybridValues sample = q.sample(&rng);
-    sample.insert(given);
-    double weight = hbn.evaluate(sample) / q.evaluate(sample);
-    (sample.atDiscrete(M(1)) == 0) ? w0 += weight : w1 += weight;
-  }
-  double pm1 = w1 / (w0 + w1);
-  std::cout << "p(m0) = " << 100 * (1.0 - pm1) << std::endl;
-  std::cout << "p(m1) = " << 100 * pm1 << std::endl;
-  return {1.0 - pm1, pm1};
-}
-
-}  // namespace test_two_state_estimation
-
 /* ************************************************************************* */
-/**
- * Test a model p(z0|x0)p(z1|x1)p(x1|x0,m1)P(m1).
- *
- * p(x1|x0,m1) has mode-dependent mean but same covariance.
- *
- * Converting to a factor graph gives us ϕ(x0;z0)ϕ(x1;z1)ϕ(x1,x0,m1)P(m1)
- *
- * If we only have a measurement on x0, then
- * the posterior probability of m1 should be 0.5/0.5.
- * Getting a measurement on z1 gives use more information.
- */
-TEST(HybridGaussianFactor, TwoStateModel) {
-  using namespace test_two_state_estimation;
-
-  double mu0 = 1.0, mu1 = 3.0;
-  double sigma = 0.5;
-  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma);
-
-  // Start with no measurement on x1, only on x0
-  const Vector1 z0(0.5);
-
-  VectorValues given;
-  given.insert(Z(0), z0);
-
-  {
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Since no measurement on x1, we hedge our bets
-    // Importance sampling run with 100k samples gives 50.051/49.949
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "50/50");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete())));
-  }
-
-  {
-    // If we set z1=4.5 (>> 2.5 which is the halfway point),
-    // probability of discrete mode should be leaning to m1==1.
-    const Vector1 z1(4.5);
-    given.insert(Z(1), z1);
-
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Since we have a measurement on x1, we get a definite result
-    // Values taken from an importance sampling run with 100k samples:
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "44.3854/55.6146");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
-  }
-}
-
-/* ************************************************************************* */
-/**
- * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
- *
- * P(x1|x0,m1) has different means and different covariances.
- *
- * Converting to a factor graph gives us
- * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
- *
- * If we only have a measurement on z0, then
- * the P(m1) should be 0.5/0.5.
- * Getting a measurement on z1 gives use more information.
- */
-TEST(HybridGaussianFactor, TwoStateModel2) {
-  using namespace test_two_state_estimation;
-
-  double mu0 = 1.0, mu1 = 3.0;
-  double sigma0 = 0.5, sigma1 = 2.0;
-  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
-
-  // Start with no measurement on x1, only on x0
-  const Vector1 z0(0.5);
-  VectorValues given;
-  given.insert(Z(0), z0);
-
-  {
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-
-    // Check that ratio of Bayes net and factor graph for different modes is
-    // equal for several values of {x0,x1}.
-    for (VectorValues vv :
-         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
-          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
-      vv.insert(given);  // add measurements for HBN
-      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
-      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
-                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
-    }
-
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Importance sampling run with 100k samples gives 50.095/49.905
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-
-    // Since no measurement on x1, we a 50/50 probability
-    auto p_m = bn->at(2)->asDiscrete();
-    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
-    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
-  }
-
-  {
-    // Now we add a measurement z1 on x1
-    const Vector1 z1(4.0);  // favors m==1
-    given.insert(Z(1), z1);
-
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-
-    // Check that ratio of Bayes net and factor graph for different modes is
-    // equal for several values of {x0,x1}.
-    for (VectorValues vv :
-         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
-          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
-      vv.insert(given);  // add measurements for HBN
-      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
-      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
-                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
-    }
-
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Values taken from an importance sampling run with 100k samples:
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "48.3158/51.6842");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
-  }
-
-  {
-    // Add a different measurement z1 on x1 that favors m==0
-    const Vector1 z1(1.1);
-    given.insert_or_assign(Z(1), z1);
-
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Values taken from an importance sampling run with 100k samples:
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "55.396/44.604");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
-  }
-}
-
-/* ************************************************************************* */
-/**
- * Test a model p(z0|x0)p(x1|x0,m1)p(z1|x1)p(m1).
- *
- * p(x1|x0,m1) has the same means but different covariances.
- *
- * Converting to a factor graph gives us
- * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)p(m1)
- *
- * If we only have a measurement on z0, then
- * the p(m1) should be 0.5/0.5.
- * Getting a measurement on z1 gives use more information.
- */
-TEST(HybridGaussianFactor, TwoStateModel3) {
-  using namespace test_two_state_estimation;
-
-  double mu = 1.0;
-  double sigma0 = 0.5, sigma1 = 2.0;
-  auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1);
-
-  // Start with no measurement on x1, only on x0
-  const Vector1 z0(0.5);
-  VectorValues given;
-  given.insert(Z(0), z0);
-
-  {
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-
-    // Check that ratio of Bayes net and factor graph for different modes is
-    // equal for several values of {x0,x1}.
-    for (VectorValues vv :
-         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
-          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
-      vv.insert(given);  // add measurements for HBN
-      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
-      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
-                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
-    }
-
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Importance sampling run with 100k samples gives 50.095/49.905
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-
-    // Since no measurement on x1, we a 50/50 probability
-    auto p_m = bn->at(2)->asDiscrete();
-    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
-    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
-  }
-
-  {
-    // Now we add a measurement z1 on x1
-    const Vector1 z1(4.0);  // favors m==1
-    given.insert(Z(1), z1);
-
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-
-    // Check that ratio of Bayes net and factor graph for different modes is
-    // equal for several values of {x0,x1}.
-    for (VectorValues vv :
-         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
-          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
-      vv.insert(given);  // add measurements for HBN
-      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
-      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
-                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
-    }
-
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Values taken from an importance sampling run with 100k samples:
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "51.7762/48.2238");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
-  }
-
-  {
-    // Add a different measurement z1 on x1 that favors m==1
-    const Vector1 z1(7.0);
-    given.insert_or_assign(Z(1), z1);
-
-    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
-    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-    // Values taken from an importance sampling run with 100k samples:
-    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-    DiscreteConditional expected(m1, "49.0762/50.9238");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005));
-  }
-}
-
-/* ************************************************************************* */
-/**
- * Same model, P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1), but now with very informative
- * measurements and vastly different motion model: either stand still or move
- * far. This yields a very informative posterior.
- */
-TEST(HybridGaussianFactor, TwoStateModel4) {
-  using namespace test_two_state_estimation;
-
-  double mu0 = 0.0, mu1 = 10.0;
-  double sigma0 = 0.2, sigma1 = 5.0;
-  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
-
-  // We only check the 2-measurement case
-  const Vector1 z0(0.0), z1(10.0);
-  VectorValues given{{Z(0), z0}, {Z(1), z1}};
-
-  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
-  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
-  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
-
-  // Values taken from an importance sampling run with 100k samples:
-  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
-  DiscreteConditional expected(m1, "8.91527/91.0847");
-  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
-}
-
 namespace test_direct_factor_graph {
 /**
  * @brief Create a Factor Graph by directly specifying all

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -30,6 +30,7 @@
 #include <numeric>
 
 #include "Switching.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -415,7 +416,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
   // Add odometry factor with discrete modes.
   Pose2 odometry(1.0, 0.0, 0.0);
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  std::vector<NonlinearFactor::shared_ptr> components;
+  std::vector<NoiseModelFactor::shared_ptr> components;
   components.emplace_back(
       new PlanarMotionModel(W(0), W(1), odometry, noise_model));  // moving
   components.emplace_back(

gtsam/hybrid/tests/testHybridMotionModel.cpp

@@ -0,0 +1,385 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testHybridMotionModel.cpp
+ * @brief   Tests hybrid inference with a simple switching motion model
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridGaussianConditional.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridValues.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+// Include for test suite
+#include <CppUnitLite/TestHarness.h>
+
+#include <memory>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+using symbol_shorthand::Z;
+
+DiscreteKey m1(M(1), 2);
+
+void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) {
+  auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma);
+  hbn.emplace_shared<GaussianConditional>(z_key, Vector1(0.0), I_1x1, x_key,
+                                          -I_1x1, measurement_model);
+}
+
+/// Create hybrid motion model p(x1 | x0, m1)
+static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
+    double mu0, double mu1, double sigma0, double sigma1) {
+  auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
+  auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
+  auto c0 = make_shared<GaussianConditional>(X(1), Vector1(mu0), I_1x1, X(0),
+                                             -I_1x1, model0),
+       c1 = make_shared<GaussianConditional>(X(1), Vector1(mu1), I_1x1, X(0),
+                                             -I_1x1, model1);
+  return std::make_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});
+}
+
+/// Create two state Bayes network with 1 or two measurement models
+HybridBayesNet CreateBayesNet(
+    const HybridGaussianConditional::shared_ptr &hybridMotionModel,
+    bool add_second_measurement = false) {
+  HybridBayesNet hbn;
+
+  // Add measurement model p(z0 | x0)
+  addMeasurement(hbn, Z(0), X(0), 3.0);
+
+  // Optionally add second measurement model p(z1 | x1)
+  if (add_second_measurement) {
+    addMeasurement(hbn, Z(1), X(1), 3.0);
+  }
+
+  // Add hybrid motion model
+  hbn.push_back(hybridMotionModel);
+
+  // Discrete uniform prior.
+  hbn.emplace_shared<DiscreteConditional>(m1, "50/50");
+
+  return hbn;
+}
+
+/// Approximate the discrete marginal P(m1) using importance sampling
+std::pair<double, double> approximateDiscreteMarginal(
+    const HybridBayesNet &hbn,
+    const HybridGaussianConditional::shared_ptr &hybridMotionModel,
+    const VectorValues &given, size_t N = 100000) {
+  /// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1),
+  /// using q(x0) = N(z0, sigmaQ) to sample x0.
+  HybridBayesNet q;
+  q.push_back(hybridMotionModel);  // Add hybrid motion model
+  q.emplace_shared<GaussianConditional>(GaussianConditional::FromMeanAndStddev(
+      X(0), given.at(Z(0)), /* sigmaQ = */ 3.0));  // Add proposal q(x0) for x0
+  q.emplace_shared<DiscreteConditional>(m1, "50/50");  // Discrete prior.
+
+  // Do importance sampling
+  double w0 = 0.0, w1 = 0.0;
+  std::mt19937_64 rng(42);
+  for (int i = 0; i < N; i++) {
+    HybridValues sample = q.sample(&rng);
+    sample.insert(given);
+    double weight = hbn.evaluate(sample) / q.evaluate(sample);
+    (sample.atDiscrete(M(1)) == 0) ? w0 += weight : w1 += weight;
+  }
+  double pm1 = w1 / (w0 + w1);
+  std::cout << "p(m0) = " << 100 * (1.0 - pm1) << std::endl;
+  std::cout << "p(m1) = " << 100 * pm1 << std::endl;
+  return {1.0 - pm1, pm1};
+}
+
+/* ************************************************************************* */
+/**
+ * Test a model p(z0|x0)p(z1|x1)p(x1|x0,m1)P(m1).
+ *
+ * p(x1|x0,m1) has mode-dependent mean but same covariance.
+ *
+ * Converting to a factor graph gives us ϕ(x0;z0)ϕ(x1;z1)ϕ(x1,x0,m1)P(m1)
+ *
+ * If we only have a measurement on x0, then
+ * the posterior probability of m1 should be 0.5/0.5.
+ * Getting a measurement on z1 gives use more information.
+ */
+TEST(HybridGaussianFactor, TwoStateModel) {
+  double mu0 = 1.0, mu1 = 3.0;
+  double sigma = 0.5;
+  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma);
+
+  // Start with no measurement on x1, only on x0
+  const Vector1 z0(0.5);
+
+  VectorValues given;
+  given.insert(Z(0), z0);
+
+  {
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Since no measurement on x1, we hedge our bets
+    // Importance sampling run with 100k samples gives 50.051/49.949
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+    DiscreteConditional expected(m1, "50/50");
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete())));
+  }
+
+  {
+    // If we set z1=4.5 (>> 2.5 which is the halfway point),
+    // probability of discrete mode should be leaning to m1==1.
+    const Vector1 z1(4.5);
+    given.insert(Z(1), z1);
+
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Since we have a measurement on x1, we get a definite result
+    // Values taken from an importance sampling run with 100k samples:
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+    DiscreteConditional expected(m1, "44.3854/55.6146");
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+  }
+}
+
+/* ************************************************************************* */
+/**
+ * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
+ *
+ * P(x1|x0,m1) has different means and different covariances.
+ *
+ * Converting to a factor graph gives us
+ * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
+ *
+ * If we only have a measurement on z0, then
+ * the P(m1) should be 0.5/0.5.
+ * Getting a measurement on z1 gives use more information.
+ */
+TEST(HybridGaussianFactor, TwoStateModel2) {
+  double mu0 = 1.0, mu1 = 3.0;
+  double sigma0 = 0.5, sigma1 = 2.0;
+  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
+
+  // Start with no measurement on x1, only on x0
+  const Vector1 z0(0.5);
+  VectorValues given;
+  given.insert(Z(0), z0);
+
+  {
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+
+    // Check that ratio of Bayes net and factor graph for different modes is
+    // equal for several values of {x0,x1}.
+    for (VectorValues vv :
+         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
+          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
+      vv.insert(given);  // add measurements for HBN
+      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
+      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
+                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
+    }
+
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Importance sampling run with 100k samples gives 50.095/49.905
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+
+    // Since no measurement on x1, we a 50/50 probability
+    auto p_m = bn->at(2)->asDiscrete();
+    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
+    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
+  }
+
+  {
+    // Now we add a measurement z1 on x1
+    const Vector1 z1(4.0);  // favors m==1
+    given.insert(Z(1), z1);
+
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+
+    // Check that ratio of Bayes net and factor graph for different modes is
+    // equal for several values of {x0,x1}.
+    for (VectorValues vv :
+         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
+          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
+      vv.insert(given);  // add measurements for HBN
+      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
+      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
+                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
+    }
+
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Values taken from an importance sampling run with 100k samples:
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+    DiscreteConditional expected(m1, "48.3158/51.6842");
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+  }
+
+  {
+    // Add a different measurement z1 on x1 that favors m==0
+    const Vector1 z1(1.1);
+    given.insert_or_assign(Z(1), z1);
+
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Values taken from an importance sampling run with 100k samples:
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+    DiscreteConditional expected(m1, "55.396/44.604");
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+  }
+}
+
+/* ************************************************************************* */
+/**
+ * Test a model p(z0|x0)p(x1|x0,m1)p(z1|x1)p(m1).
+ *
+ * p(x1|x0,m1) has the same means but different covariances.
+ *
+ * Converting to a factor graph gives us
+ * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)p(m1)
+ *
+ * If we only have a measurement on z0, then
+ * the p(m1) should be 0.5/0.5.
+ * Getting a measurement on z1 gives use more information.
+ */
+TEST(HybridGaussianFactor, TwoStateModel3) {
+  double mu = 1.0;
+  double sigma0 = 0.5, sigma1 = 2.0;
+  auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1);
+
+  // Start with no measurement on x1, only on x0
+  const Vector1 z0(0.5);
+  VectorValues given;
+  given.insert(Z(0), z0);
+
+  {
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+
+    // Check that ratio of Bayes net and factor graph for different modes is
+    // equal for several values of {x0,x1}.
+    for (VectorValues vv :
+         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
+          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
+      vv.insert(given);  // add measurements for HBN
+      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
+      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
+                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
+    }
+
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Importance sampling run with 100k samples gives 50.095/49.905
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+
+    // Since no measurement on x1, we a 50/50 probability
+    auto p_m = bn->at(2)->asDiscrete();
+    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
+    EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
+  }
+
+  {
+    // Now we add a measurement z1 on x1
+    const Vector1 z1(4.0);  // favors m==1
+    given.insert(Z(1), z1);
+
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+
+    // Check that ratio of Bayes net and factor graph for different modes is
+    // equal for several values of {x0,x1}.
+    for (VectorValues vv :
+         {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
+          VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
+      vv.insert(given);  // add measurements for HBN
+      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
+      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
+                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
+    }
+
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Values taken from an importance sampling run with 100k samples:
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+    DiscreteConditional expected(m1, "51.7762/48.2238");
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+  }
+
+  {
+    // Add a different measurement z1 on x1 that favors m==1
+    const Vector1 z1(7.0);
+    given.insert_or_assign(Z(1), z1);
+
+    HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
+    HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+    // Values taken from an importance sampling run with 100k samples:
+    // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+    DiscreteConditional expected(m1, "49.0762/50.9238");
+    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005));
+  }
+}
+
+/* ************************************************************************* */
+/**
+ * Same model, P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1), but now with very informative
+ * measurements and vastly different motion model: either stand still or move
+ * far. This yields a very informative posterior.
+ */
+TEST(HybridGaussianFactor, TwoStateModel4) {
+  double mu0 = 0.0, mu1 = 10.0;
+  double sigma0 = 0.2, sigma1 = 5.0;
+  auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1);
+
+  // We only check the 2-measurement case
+  const Vector1 z0(0.0), z1(10.0);
+  VectorValues given{{Z(0), z0}, {Z(1), z1}};
+
+  HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
+  HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+  HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+
+  // Values taken from an importance sampling run with 100k samples:
+  // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
+  DiscreteConditional expected(m1, "8.91527/91.0847");
+  EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -36,6 +36,7 @@
 #include <numeric>
 
 #include "Switching.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -119,7 +120,7 @@ TEST(HybridNonlinearFactorGraph, Resize) {
 namespace test_motion {
 gtsam::DiscreteKey m1(M(1), 2);
 auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
-std::vector<NonlinearFactor::shared_ptr> components = {
+std::vector<NoiseModelFactor::shared_ptr> components = {
     std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
     std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model)};
 }  // namespace test_motion
@@ -207,7 +208,7 @@ TEST(HybridNonlinearFactorGraph, PushBack) {
   factors.emplace_shared<PriorFactor<Pose2>>(1, Pose2(1, 0, 0), noise);
   factors.emplace_shared<PriorFactor<Pose2>>(2, Pose2(2, 0, 0), noise);
   // TODO(Varun) This does not currently work. It should work once HybridFactor
-  // becomes a base class of NonlinearFactor.
+  // becomes a base class of NoiseModelFactor.
   // hnfg.push_back(factors.begin(), factors.end());
 
   // EXPECT_LONGS_EQUAL(3, hnfg.size());
@@ -807,7 +808,7 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
   // Add odometry factor
   Pose2 odometry(2.0, 0.0, 0.0);
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  std::vector<NonlinearFactor::shared_ptr> motion_models = {
+  std::vector<NoiseModelFactor::shared_ptr> motion_models = {
       std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
                                           noise_model),
       std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
@@ -874,8 +875,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
   // Create HybridNonlinearFactor
   // We take negative since we want
   // the underlying scalar to be log(\sqrt(|2πΣ|))
-  std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
+  std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
-                                                {f1, model1->negLogConstant()}};
 
   HybridNonlinearFactor mixtureFactor(m1, factors);
 

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -30,6 +30,7 @@
 #include <numeric>
 
 #include "Switching.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -438,7 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
-  std::vector<NonlinearFactor::shared_ptr> components{moving, still};
+  std::vector<NoiseModelFactor::shared_ptr> components{moving, still};
   fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
 
   // Add equivalent of ImuFactor