diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 7b63c0dff3..9378d07fe2 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include @@ -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& factors) + const std::vector& factors) : discreteKeys({discreteKey}) { std::vector 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& factors) + const std::vector& 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& a, const std::pair& b) { - return traits::Equals(*a.first, *b.first, tol) && - (a.second == b.second); + return a.first->equals(*b.first, tol) && (a.second == b.second); }; if (!factors_.equals(f.factors_, compare)) return false; @@ -185,7 +185,15 @@ std::shared_ptr HybridNonlinearFactor::linearize( [continuousValues]( const std::pair& f) -> GaussianFactorValuePair { auto [factor, val] = f; - return {factor->linearize(continuousValues), val}; + if (auto gaussian = std::dynamic_pointer_cast( + 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> diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index a3b6ad4d94..325fa3eaab 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -26,25 +26,23 @@ #include #include -#include -#include -#include #include namespace gtsam { -/// Alias for a NonlinearFactor shared pointer and double scalar pair. -using NonlinearFactorValuePair = std::pair; +/// Alias for a NoiseModelFactor shared pointer and double scalar pair. +using NonlinearFactorValuePair = + std::pair; /** * @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 - * the correct operation. + * one of (NoiseModelFactor, GaussianFactor) which can then be checked to + * 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; - using sharedFactor = std::shared_ptr; + using sharedFactor = std::shared_ptr; /** * @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; @@ -95,7 +93,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { */ HybridNonlinearFactor( const DiscreteKey& discreteKey, - const std::vector& factors); + const std::vector& factors); /** * @brief Construct a new HybridNonlinearFactor on a single discrete key, diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 9ed96d1154..2d59c100f1 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -245,16 +245,16 @@ class HybridNonlinearFactorGraph { class HybridNonlinearFactor : gtsam::HybridFactor { HybridNonlinearFactor( const gtsam::DiscreteKey& discreteKey, - const std::vector& factors); + const std::vector& factors); HybridNonlinearFactor( const gtsam::DiscreteKey& discreteKey, - const std::vector>& factors); + const std::vector>& factors); HybridNonlinearFactor( const gtsam::DiscreteKeys& discreteKeys, const gtsam::DecisionTree< - gtsam::Key, std::pair>& factors); + gtsam::Key, std::pair>& factors); double error(const gtsam::Values& continuousValues, const gtsam::DiscreteValues& discreteValues) const; diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 23a102fd5b..90137a4e30 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/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 motionModels( + static std::vector motionModels( size_t k, double sigma = 1.0) { auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); auto still = diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 8659100acb..769512bede 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/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 @@ -389,7 +390,7 @@ TEST(HybridBayesNet, Sampling) { std::make_shared>(X(0), X(1), 1, noise_model); nfg.emplace_shared( DiscreteKey(M(0), 2), - std::vector{zero_motion, one_motion}); + std::vector{zero_motion, one_motion}); DiscreteKey mode(M(0), 2); nfg.emplace_shared(mode, "1/1"); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 1b0286084d..2a5fb93ba3 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -39,6 +39,7 @@ #include #include "Switching.h" +#include "gtsam/nonlinear/NonlinearFactor.h" using namespace std; using namespace gtsam; @@ -435,8 +436,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - std::vector components = {zero_motion, - one_motion}; + std::vector components = {zero_motion, + one_motion}; nfg.emplace_shared(m, components); return nfg; @@ -526,49 +527,6 @@ TEST(HybridEstimation, CorrectnessViaSampling) { } } -/****************************************************************************/ -/** - * Helper function to add the constant term corresponding to - * the difference in noise models. - */ -std::shared_ptr 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& 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(c); - return std::make_shared(_gfg); - } else { - return dynamic_pointer_cast(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(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( X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector components = {model0, model1}; + std::vector components = {model0, model1}; HybridNonlinearFactor mf({M(0), 2}, components); initial.insert(X(0), 0.0); initial.insert(X(1), 0.0); - auto gmf = - mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1); + auto gmf = mf.linearize(initial); 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>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); - std::vector components = {model0, model1}; + std::vector components = {model0, model1}; HybridNonlinearFactor mf({M(0), 2}, components); initial.insert(X(0), Z_3x1); initial.insert(X(1), Z_3x1); - auto gmf = - mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1); + auto gmf = mf.linearize(initial); graph.add(gmf); auto gfg = graph.linearize(initial); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 080185d88d..c2ffe24c89 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/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(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(X(1), Vector1(mu0), I_1x1, X(0), - -I_1x1, model0), - c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), - -I_1x1, model1); - DiscreteKeys discreteParents{m1}; - return std::make_shared( - 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(m1, "50/50"); - - return hbn; -} - -/// Approximate the discrete marginal P(m1) using importance sampling -std::pair 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::FromMeanAndStddev( - X(0), given.at(Z(0)), /* sigmaQ = */ 3.0)); // Add proposal q(x0) for x0 - q.emplace_shared(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 diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 280c6cb531..7572a64e26 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -30,6 +30,7 @@ #include #include "Switching.h" +#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include @@ -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 components; + std::vector components; components.emplace_back( new PlanarMotionModel(W(0), W(1), odometry, noise_model)); // moving components.emplace_back( diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp new file mode 100644 index 0000000000..1a2a09e153 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Include for test suite +#include + +#include + +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(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(X(1), Vector1(mu0), I_1x1, X(0), + -I_1x1, model0), + c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), + -I_1x1, model1); + return std::make_shared(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(m1, "50/50"); + + return hbn; +} + +/// Approximate the discrete marginal P(m1) using importance sampling +std::pair 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::FromMeanAndStddev( + X(0), given.at(Z(0)), /* sigmaQ = */ 3.0)); // Add proposal q(x0) for x0 + q.emplace_shared(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); +} +/* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index f9a546c814..d323125086 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -36,6 +36,7 @@ #include #include "Switching.h" +#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include @@ -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 components = { +std::vector components = { std::make_shared(X(0), X(1), 0.0, noise_model), std::make_shared(X(0), X(1), 1.0, noise_model)}; } // namespace test_motion @@ -207,7 +208,7 @@ TEST(HybridNonlinearFactorGraph, PushBack) { factors.emplace_shared>(1, Pose2(1, 0, 0), noise); factors.emplace_shared>(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 motion_models = { + std::vector motion_models = { std::make_shared(X(0), X(1), Pose2(0, 0, 0), noise_model), std::make_shared(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 factors{{f0, model0->negLogConstant()}, - {f1, model1->negLogConstant()}}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridNonlinearFactor mixtureFactor(m1, factors); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 9e93116fc0..5a04fc089e 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -30,6 +30,7 @@ #include #include "Switching.h" +#include "gtsam/nonlinear/NonlinearFactor.h" // Include for test suite #include @@ -438,7 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); - std::vector components{moving, still}; + std::vector components{moving, still}; fg.emplace_shared(DiscreteKey(M(1), 2), components); // Add equivalent of ImuFactor