diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 7b63c0dff..528b150ac 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 Gaussian factors."); + } }; DecisionTree> diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index a3b6ad4d9..325fa3eaa 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/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 23a102fd5..90137a4e3 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 8659100ac..769512bed 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 1b0286084..2a5fb93ba 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/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 280c6cb53..7572a64e2 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/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index f9a546c81..d32312508 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 9e93116fc..5a04fc089 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