diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 9144e3b6dd..23a102fd5b 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -159,9 +159,8 @@ struct Switching { // Add "motion models". for (size_t k = 0; k < K - 1; k++) { - KeyVector keys = {X(k), X(k + 1)}; auto motion_models = motionModels(k, between_sigma); - nonlinearFactorGraph.emplace_shared(keys, modes[k], + nonlinearFactorGraph.emplace_shared(modes[k], motion_models); } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index b049b03fd2..8659100acb 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -388,7 +388,7 @@ TEST(HybridBayesNet, Sampling) { auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); nfg.emplace_shared( - KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2), + DiscreteKey(M(0), 2), std::vector{zero_motion, one_motion}); DiscreteKey mode(M(0), 2); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 2a20f5f5ea..1b0286084d 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -437,8 +437,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 1, noise_model); std::vector components = {zero_motion, one_motion}; - nfg.emplace_shared(KeyVector{X(0), X(1)}, m, - components); + nfg.emplace_shared(m, components); return nfg; } @@ -591,9 +590,7 @@ TEST(HybridEstimation, ModeSelection) { X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); std::vector components = {model0, model1}; - KeyVector keys = {X(0), X(1)}; - DiscreteKey modes(M(0), 2); - HybridNonlinearFactor mf(keys, modes, components); + HybridNonlinearFactor mf({M(0), 2}, components); initial.insert(X(0), 0.0); initial.insert(X(1), 0.0); @@ -681,9 +678,7 @@ TEST(HybridEstimation, ModeSelection2) { X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); std::vector components = {model0, model1}; - KeyVector keys = {X(0), X(1)}; - DiscreteKey modes(M(0), 2); - HybridNonlinearFactor mf(keys, modes, components); + HybridNonlinearFactor mf({M(0), 2}, components); initial.insert(X(0), Z_3x1); initial.insert(X(1), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 9ce109647e..280c6cb531 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -414,15 +414,13 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add odometry factor with discrete modes. Pose2 odometry(1.0, 0.0, 0.0); - KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.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); + fg.emplace_shared(DiscreteKey(M(1), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), @@ -454,14 +452,12 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 3 ***************/ // Add odometry factor with discrete modes. - contKeys = {W(1), W(2)}; 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); + fg.emplace_shared(DiscreteKey(M(2), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), @@ -496,14 +492,12 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. - contKeys = {W(2), W(3)}; 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); + fg.emplace_shared(DiscreteKey(M(3), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index d949a1e061..2b441ab131 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -60,14 +60,14 @@ auto f1 = std::make_shared>(X(1), X(2), between1, model); // Test simple to complex constructors... TEST(HybridGaussianFactor, ConstructorVariants) { using namespace test_constructor; - HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1}); + HybridNonlinearFactor fromFactors(m1, {f0, f1}); std::vector pairs{{f0, 0.0}, {f1, 0.0}}; - HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs); + HybridNonlinearFactor fromPairs(m1, pairs); assert_equal(fromFactors, fromPairs); HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs); - HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree); + HybridNonlinearFactor fromDecisionTree({m1}, decisionTree); assert_equal(fromDecisionTree, fromPairs); } @@ -75,7 +75,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) { // Test .print() output. TEST(HybridNonlinearFactor, Printing) { using namespace test_constructor; - HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1}); + HybridNonlinearFactor hybridFactor({m1}, {f0, f1}); std::string expected = R"(Hybrid [x1 x2; 1] @@ -101,7 +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); - return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1}); + return HybridNonlinearFactor(m1, {f0, f1}); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 63a41a63a1..f9a546c814 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -117,7 +117,6 @@ TEST(HybridNonlinearFactorGraph, Resize) { /***************************************************************************/ 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 = { @@ -139,8 +138,7 @@ TEST(HybridGaussianFactorGraph, Resize) { auto discreteFactor = std::make_shared(); hnfg.push_back(discreteFactor); - auto dcFactor = - std::make_shared(contKeys, m1, components); + auto dcFactor = std::make_shared(m1, components); hnfg.push_back(dcFactor); Values linearizationPoint; @@ -156,26 +154,6 @@ TEST(HybridGaussianFactorGraph, Resize) { EXPECT_LONGS_EQUAL(gfg.size(), 0); } -/*************************************************************************** - * Test that the HybridNonlinearFactor reports correctly if the number of - * 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(); - - // Check for exception when number of continuous keys are under-specified. - THROWS_EXCEPTION( - std::make_shared(KeyVector{X(0)}, m1, components)); - - // Check for exception when number of continuous keys are too many. - THROWS_EXCEPTION(std::make_shared( - KeyVector{X(0), X(1), X(2)}, m1, components)); -} - /***************************************************************************** * Test push_back on HFG makes the correct distinction. */ @@ -828,14 +806,12 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) { // Add odometry factor Pose2 odometry(2.0, 0.0, 0.0); - KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.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); + fg.emplace_shared(DiscreteKey{M(1), 2}, motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // create a noise model for the landmark measurements @@ -901,7 +877,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph( std::vector factors{{f0, model0->negLogConstant()}, {f1, model1->negLogConstant()}}; - HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors); + HybridNonlinearFactor mixtureFactor(m1, factors); HybridNonlinearFactorGraph hfg; hfg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index fe81fc13e5..9e93116fc0 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -433,15 +433,13 @@ TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 2 ***************/ // Add odometry factor with discrete modes. 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, still}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(1), 2), components); + fg.emplace_shared(DiscreteKey(M(1), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), @@ -473,14 +471,12 @@ TEST(HybridNonlinearISAM, 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, still}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(2), 2), components); + fg.emplace_shared(DiscreteKey(M(2), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), @@ -515,14 +511,12 @@ TEST(HybridNonlinearISAM, 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, still}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(3), 2), components); + fg.emplace_shared(DiscreteKey(M(3), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0),