diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 9613233b15..a541722c44 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -21,10 +21,57 @@ namespace gtsam { /* *******************************************************************************/ -HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& keys, +static void checkKeys(const KeyVector& continuousKeys, + std::vector& pairs) { + KeySet factor_keys_set; + for (const auto& pair : pairs) { + auto f = pair.first; + // Insert all factor continuous keys in the continuous keys set. + std::copy(f->keys().begin(), f->keys().end(), + std::inserter(factor_keys_set, factor_keys_set.end())); + } + + KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end()); + if (continuous_keys_set != factor_keys_set) { + throw std::runtime_error( + "HybridNonlinearFactor: The specified continuous keys and the keys in " + "the factors do not match!"); + } +} + +/* *******************************************************************************/ +HybridNonlinearFactor::HybridNonlinearFactor( + const KeyVector& continuousKeys, const DiscreteKey& discreteKey, + const std::vector& factors) + : Base(continuousKeys, {discreteKey}) { + std::vector pairs; + for (auto&& f : factors) { + pairs.emplace_back(f, 0.0); + } + checkKeys(continuousKeys, pairs); + factors_ = FactorValuePairs({discreteKey}, pairs); +} + +/* *******************************************************************************/ +HybridNonlinearFactor::HybridNonlinearFactor( + const KeyVector& continuousKeys, const DiscreteKey& discreteKey, + const std::vector& factors) + : Base(continuousKeys, {discreteKey}) { + std::vector pairs; + KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end()); + KeySet factor_keys_set; + for (auto&& [f, val] : factors) { + pairs.emplace_back(f, val); + } + checkKeys(continuousKeys, pairs); + factors_ = FactorValuePairs({discreteKey}, pairs); +} + +/* *******************************************************************************/ +HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys, const DiscreteKeys& discreteKeys, - const Factors& factors) - : Base(keys, discreteKeys), factors_(factors) {} + const FactorValuePairs& factors) + : Base(continuousKeys, discreteKeys), factors_(factors) {} /* *******************************************************************************/ AlgebraicDecisionTree HybridNonlinearFactor::errorTree( diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 9852602de4..766467518b 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -68,11 +68,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { * @brief typedef for DecisionTree which has Keys as node labels and * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes. */ - using Factors = DecisionTree; + using FactorValuePairs = DecisionTree; private: - /// Decision tree of Gaussian factors indexed by discrete keys. - Factors factors_; + /// Decision tree of nonlinear factors indexed by discrete keys. + FactorValuePairs factors_; /// HybridFactor method implementation. Should not be used. AlgebraicDecisionTree errorTree( @@ -82,62 +82,49 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { } public: + /// Default constructor, mainly for serialization. HybridNonlinearFactor() = default; /** - * @brief Construct from Decision tree. + * @brief Construct a new HybridNonlinearFactor on a single discrete key, + * providing the factors for each mode m as a vector of factors ϕ_m(x). + * The value ϕ(x,m) for the factor is simply ϕ_m(x). * - * @param keys Vector of keys for continuous factors. - * @param discreteKeys Vector of discrete keys. - * @param factors Decision tree with of shared factors. + * @param continuousKeys Vector of keys for continuous factors. + * @param discreteKey The discrete key for the "mode", indexing components. + * @param factors Vector of gaussian factors, one for each mode. */ - HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const Factors& factors); + HybridNonlinearFactor( + const KeyVector& continuousKeys, const DiscreteKey& discreteKey, + const std::vector& factors); /** - * @brief Convenience constructor that generates the underlying factor - * decision tree for us. - * - * Here it is important that the vector of factors has the correct number of - * elements based on the number of discrete keys and the cardinality of the - * keys, so that the decision tree is constructed appropriately. + * @brief Construct a new HybridNonlinearFactor on a single discrete key, + * including a scalar error value for each mode m. The factors and scalars are + * provided as a vector of pairs (ϕ_m(x), E_m). + * The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m. * - * @tparam FACTOR The type of the factor shared pointers being passed in. - * Will be typecast to NonlinearFactor shared pointers. - * @param keys Vector of keys for continuous factors. - * @param discreteKey The discrete key indexing each component factor. - * @param factors Vector of nonlinear factor and scalar pairs. - * Same size as the cardinality of discreteKey. + * @param continuousKeys Vector of keys for continuous factors. + * @param discreteKey The discrete key for the "mode", indexing components. + * @param factors Vector of gaussian factor-scalar pairs, one per mode. */ - template - HybridNonlinearFactor( - const KeyVector& keys, const DiscreteKey& discreteKey, - const std::vector, double>>& factors) - : Base(keys, {discreteKey}) { - std::vector nonlinear_factors; - KeySet continuous_keys_set(keys.begin(), keys.end()); - KeySet factor_keys_set; - for (auto&& [f, val] : factors) { - // Insert all factor continuous keys in the continuous keys set. - std::copy(f->keys().begin(), f->keys().end(), - std::inserter(factor_keys_set, factor_keys_set.end())); - - if (auto nf = std::dynamic_pointer_cast(f)) { - nonlinear_factors.emplace_back(nf, val); - } else { - throw std::runtime_error( - "Factors passed into HybridNonlinearFactor need to be nonlinear!"); - } - } - factors_ = Factors({discreteKey}, nonlinear_factors); - - if (continuous_keys_set != factor_keys_set) { - throw std::runtime_error( - "The specified continuous keys and the keys in the factors don't " - "match!"); - } - } + HybridNonlinearFactor(const KeyVector& continuousKeys, + const DiscreteKey& discreteKey, + const std::vector& factors); + /** + * @brief Construct a new HybridNonlinearFactor on a several discrete keys M, + * including a scalar error value for each assignment m. The factors and + * scalars are provided as a DecisionTree of pairs (ϕ_M(x), E_M). + * The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m. + * + * @param continuousKeys A vector of keys representing continuous variables. + * @param discreteKeys Discrete variables and their cardinalities. + * @param factors The decision tree of nonlinear factor/scalar pairs. + */ + HybridNonlinearFactor(const KeyVector& continuousKeys, + const DiscreteKeys& discreteKeys, + const FactorValuePairs& factors); /** * @brief Compute error of the HybridNonlinearFactor as a tree. * @@ -196,4 +183,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { const Values& continuousValues) const; }; +// traits +template <> +struct traits : public Testable { +}; + } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 82881ac478..d1b8fbf6d1 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -246,14 +246,18 @@ class HybridNonlinearFactorGraph { #include class HybridNonlinearFactor : gtsam::HybridFactor { HybridNonlinearFactor( - const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, - const gtsam::DecisionTree< - gtsam::Key, std::pair>& factors); + const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, + const std::vector& factors); HybridNonlinearFactor( const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, const std::vector>& factors); + HybridNonlinearFactor( + const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const gtsam::DecisionTree< + 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 2de8e7fc03..8836d04c88 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -161,13 +161,8 @@ struct Switching { for (size_t k = 0; k < K - 1; k++) { KeyVector keys = {X(k), X(k + 1)}; auto motion_models = motionModels(k, between_sigma); - std::vector components; - for (auto &&f : motion_models) { - components.push_back( - {std::dynamic_pointer_cast(f), 0.0}); - } nonlinearFactorGraph.emplace_shared(keys, modes[k], - components); + motion_models); } // Add measurement factors @@ -191,8 +186,8 @@ struct Switching { } // Create motion models for a given time step - static std::vector motionModels(size_t k, - double sigma = 1.0) { + static std::vector motionModels( + size_t k, double sigma = 1.0) { auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); auto still = std::make_shared(X(k), X(k + 1), 0.0, noise_model), diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 42ccf16009..6e23db91e3 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -391,8 +391,7 @@ TEST(HybridBayesNet, Sampling) { std::make_shared>(X(0), X(1), 1, noise_model); nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2), - std::vector{{zero_motion, 0.0}, - {one_motion, 0.0}}); + 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 6f28f639da..4899205dfc 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -435,8 +435,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, 0.0}, - {one_motion, 0.0}}; + std::vector components = {zero_motion, + one_motion}; nfg.emplace_shared(KeyVector{X(0), X(1)}, m, components); @@ -566,10 +566,8 @@ std::shared_ptr mixedVarianceFactor( [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { return {gf, 0.0}; }); - auto updated_gmf = std::make_shared( + return std::make_shared( gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs); - - return updated_gmf; } /****************************************************************************/ @@ -591,8 +589,7 @@ 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, 0.0}, - {model1, 0.0}}; + std::vector components = {model0, model1}; KeyVector keys = {X(0), X(1)}; DiscreteKey modes(M(0), 2); @@ -688,8 +685,7 @@ 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, 0.0}, - {model1, 0.0}}; + std::vector components = {model0, model1}; KeyVector keys = {X(0), X(1)}; DiscreteKey modes(M(0), 2); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 4b664b8b4a..fff8e48fb1 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -73,7 +73,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) { HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11}); std::vector pairs{{f10, 0.0}, {f11, 0.0}}; - HybridGaussianFactor fromPairs({X(1), X(2)}, {m1}, pairs); + HybridGaussianFactor fromPairs({X(1), X(2)}, m1, pairs); assert_equal(fromFactors, fromPairs); HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 72ab43a2ab..9ce109647e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -416,12 +416,11 @@ TEST(HybridGaussianISAM, NonTrivial) { Pose2 odometry(1.0, 0.0, 0.0); KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); - auto still = std::make_shared(W(0), W(1), Pose2(0, 0, 0), - noise_model), - moving = std::make_shared(W(0), W(1), odometry, - noise_model); - std::vector> components = { - {moving, 0.0}, {still, 0.0}}; + std::vector components; + components.emplace_back( + new PlanarMotionModel(W(0), W(1), odometry, noise_model)); // moving + components.emplace_back( + new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model)); // still fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(1), 2), components); @@ -456,11 +455,11 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 3 ***************/ // Add odometry factor with discrete modes. contKeys = {W(1), W(2)}; - still = std::make_shared(W(1), W(2), Pose2(0, 0, 0), - noise_model); - moving = - std::make_shared(W(1), W(2), odometry, noise_model); - components = {{moving, 0.0}, {still, 0.0}}; + components.clear(); + components.emplace_back( + new PlanarMotionModel(W(1), W(2), odometry, noise_model)); // moving + components.emplace_back( + new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model)); // still fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(2), 2), components); @@ -498,11 +497,11 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. contKeys = {W(2), W(3)}; - still = std::make_shared(W(2), W(3), Pose2(0, 0, 0), - noise_model); - moving = - std::make_shared(W(2), W(3), odometry, noise_model); - components = {{moving, 0.0}, {still, 0.0}}; + components.clear(); + components.emplace_back( + new PlanarMotionModel(W(2), W(3), odometry, noise_model)); // moving + components.emplace_back( + new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model)); // still fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(3), 2), components); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index 92efa680a3..d949a1e061 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -43,24 +43,39 @@ TEST(HybridNonlinearFactor, Constructor) { HybridNonlinearFactor::iterator it = factor.begin(); CHECK(it == factor.end()); } - /* ************************************************************************* */ -// Test .print() output. -TEST(HybridNonlinearFactor, Printing) { - DiscreteKey m1(1, 2); - double between0 = 0.0; - double between1 = 1.0; +namespace test_constructor { +DiscreteKey m1(1, 2); +double between0 = 0.0; +double between1 = 1.0; - Vector1 sigmas = Vector1(1.0); - auto model = noiseModel::Diagonal::Sigmas(sigmas, false); +Vector1 sigmas = Vector1(1.0); +auto model = noiseModel::Diagonal::Sigmas(sigmas, false); - auto f0 = - std::make_shared>(X(1), X(2), between0, model); - auto f1 = - std::make_shared>(X(1), X(2), between1, model); - std::vector factors{{f0, 0.0}, {f1, 0.0}}; +auto f0 = std::make_shared>(X(1), X(2), between0, model); +auto f1 = std::make_shared>(X(1), X(2), between1, model); +} // namespace test_constructor - HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, factors); +/* ************************************************************************* */ +// Test simple to complex constructors... +TEST(HybridGaussianFactor, ConstructorVariants) { + using namespace test_constructor; + HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1}); + + std::vector pairs{{f0, 0.0}, {f1, 0.0}}; + HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs); + assert_equal(fromFactors, fromPairs); + + HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs); + HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree); + assert_equal(fromDecisionTree, fromPairs); +} + +/* ************************************************************************* */ +// Test .print() output. +TEST(HybridNonlinearFactor, Printing) { + using namespace test_constructor; + HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1}); std::string expected = R"(Hybrid [x1 x2; 1] @@ -86,9 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector factors{{f0, 0.0}, {f1, 0.0}}; - - return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors); + return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1}); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 495444c79d..63a41a63a1 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -115,35 +115,40 @@ TEST(HybridNonlinearFactorGraph, Resize) { EXPECT_LONGS_EQUAL(fg.size(), 0); } +/***************************************************************************/ +namespace test_motion { +KeyVector contKeys = {X(0), X(1)}; +gtsam::DiscreteKey m1(M(1), 2); +auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); +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 + /*************************************************************************** * Test that the resize method works correctly for a * HybridGaussianFactorGraph. */ TEST(HybridGaussianFactorGraph, Resize) { - HybridNonlinearFactorGraph nhfg; + using namespace test_motion; + + HybridNonlinearFactorGraph hnfg; auto nonlinearFactor = std::make_shared>( X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); - nhfg.push_back(nonlinearFactor); + hnfg.push_back(nonlinearFactor); auto discreteFactor = std::make_shared(); - nhfg.push_back(discreteFactor); - - KeyVector contKeys = {X(0), X(1)}; - auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); - auto still = std::make_shared(X(0), X(1), 0.0, noise_model), - moving = std::make_shared(X(0), X(1), 1.0, noise_model); + hnfg.push_back(discreteFactor); - std::vector> components = { - {still, 0.0}, {moving, 0.0}}; - auto dcFactor = std::make_shared( - contKeys, gtsam::DiscreteKey(M(1), 2), components); - nhfg.push_back(dcFactor); + auto dcFactor = + std::make_shared(contKeys, m1, components); + hnfg.push_back(dcFactor); Values linearizationPoint; linearizationPoint.insert(X(0), 0); linearizationPoint.insert(X(1), 1); // Generate `HybridGaussianFactorGraph` by linearizing - HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint); + HybridGaussianFactorGraph gfg = *hnfg.linearize(linearizationPoint); EXPECT_LONGS_EQUAL(gfg.size(), 3); @@ -156,26 +161,19 @@ TEST(HybridGaussianFactorGraph, Resize) { * continuous keys provided do not match the keys in the factors. */ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { + using namespace test_motion; + auto nonlinearFactor = std::make_shared>( X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); auto discreteFactor = std::make_shared(); - auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); - auto still = std::make_shared(X(0), X(1), 0.0, noise_model), - moving = std::make_shared(X(0), X(1), 1.0, noise_model); - - std::vector> components = { - {still, 0.0}, {moving, 0.0}}; - // Check for exception when number of continuous keys are under-specified. - KeyVector contKeys = {X(0)}; - THROWS_EXCEPTION(std::make_shared( - contKeys, gtsam::DiscreteKey(M(1), 2), components)); + THROWS_EXCEPTION( + std::make_shared(KeyVector{X(0)}, m1, components)); // Check for exception when number of continuous keys are too many. - contKeys = {X(0), X(1), X(2)}; THROWS_EXCEPTION(std::make_shared( - contKeys, gtsam::DiscreteKey(M(1), 2), components)); + KeyVector{X(0), X(1), X(2)}, m1, components)); } /***************************************************************************** @@ -832,12 +830,10 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) { Pose2 odometry(2.0, 0.0, 0.0); KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); - auto still = std::make_shared(X(0), X(1), Pose2(0, 0, 0), - noise_model), - moving = std::make_shared(X(0), X(1), odometry, - noise_model); - std::vector> motion_models = - {{still, 0.0}, {moving, 0.0}}; + 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)}; fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(1), 2), motion_models); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 86e045bb03..fe81fc13e5 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -439,8 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); - std::vector> components = { - {moving, 0.0}, {still, 0.0}}; + std::vector components{moving, still}; fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(1), 2), components); @@ -479,7 +478,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); - components = {{moving, 0.0}, {still, 0.0}}; + components = {moving, still}; fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(2), 2), components); @@ -521,7 +520,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); - components = {{moving, 0.0}, {still, 0.0}}; + components = {moving, still}; fg.emplace_shared( contKeys, gtsam::DiscreteKey(M(3), 2), components); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 701b870f1c..4bf90019bd 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -83,7 +83,7 @@ TEST(HybridSerialization, HybridGaussianFactor) { auto b1 = Matrix::Ones(2, 1); auto f0 = std::make_shared(X(0), A, b0); auto f1 = std::make_shared(X(0), A, b1); - std::vector factors{{f0, 0.0}, {f1, 0.0}}; + std::vector factors{f0, f1}; const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);