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/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);