diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 6e66e5fcb..c732ec6c5 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -476,9 +476,9 @@ TEST(HybridFactorGraph, Full_Elimination) { ->equals(*discreteBayesNet.at(1))); } -/* -****************************************************************************/ -// Test printing +/**************************************************************************** + * Test printing + */ TEST(HybridFactorGraph, Printing) { Switching self(3); @@ -663,80 +663,81 @@ factor 2: Hybrid P( x3 | m1 m2) EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet)); } -// /* ************************************************************************* -// */ -// // Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose -// // connects to 1 landmark) to expose issue with default decision tree -// creation -// // in hybrid elimination. The hybrid factor is between the poses X0 and X1. -// The -// // issue arises if we eliminate a landmark variable first since it is not -// // connected to a DCFactor. -// TEST(HybridFactorGraph, DefaultDecisionTree) { -// HybridNonlinearFactorGraph fg; +/**************************************************************************** + * Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose + * connects to 1 landmark) to expose issue with default decision tree creation + * in hybrid elimination. The hybrid factor is between the poses X0 and X1. + * The issue arises if we eliminate a landmark variable first since it is not + * connected to a HybridFactor. + */ +TEST(HybridFactorGraph, DefaultDecisionTree) { + HybridNonlinearFactorGraph fg; -// // Add a prior on pose x1 at the origin. A prior factor consists of a mean -// and -// // a noise model (covariance matrix) -// Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin -// auto priorNoise = noiseModel::Diagonal::Sigmas( -// Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta -// fg.emplace_nonlinear>(X(0), prior, priorNoise); + // Add a prior on pose x1 at the origin. + // A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin + auto priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + fg.emplace_nonlinear>(X(0), prior, priorNoise); -// using PlanarMotionModel = BetweenFactor; + using PlanarMotionModel = BetweenFactor; -// // 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); -// auto still = boost::make_shared(X(0), X(1), Pose2(0, 0, -// 0), -// noise_model), -// moving = boost::make_shared(X(0), X(1), odometry, -// noise_model); -// std::vector components = {still, moving}; -// auto dcFactor = boost::make_shared>( -// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); -// fg.push_back(dcFactor); + // 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); + auto still = boost::make_shared(X(0), X(1), Pose2(0, 0, + 0), + noise_model), + moving = boost::make_shared(X(0), X(1), odometry, + noise_model); + std::vector motion_models = {still, moving}; + //TODO(Varun) Make a templated constructor for MixtureFactor which does this? + std::vector components; + for (auto&& f : motion_models) { + components.push_back(boost::dynamic_pointer_cast(f)); + } + fg.emplace_hybrid( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); -// // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. -// // create a noise model for the landmark measurements -// auto measurementNoise = noiseModel::Diagonal::Sigmas( -// Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range -// // create the measurement values - indices are (pose id, landmark id) -// Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90); -// double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0; + // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. + // create a noise model for the landmark measurements + auto measurementNoise = noiseModel::Diagonal::Sigmas( + Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + // create the measurement values - indices are (pose id, landmark id) + Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90); + double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0; -// // Add Bearing-Range factors -// fg.emplace_nonlinear>( -// X(0), L(0), bearing11, range11, measurementNoise); -// fg.emplace_nonlinear>( -// X(1), L(1), bearing22, range22, measurementNoise); + // Add Bearing-Range factors + fg.emplace_nonlinear>( + X(0), L(0), bearing11, range11, measurementNoise); + fg.emplace_nonlinear>( + X(1), L(1), bearing22, range22, measurementNoise); -// // Create (deliberately inaccurate) initial estimate -// Values initialEstimate; -// initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2)); -// initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2)); -// initialEstimate.insert(L(0), Point2(1.8, 2.1)); -// initialEstimate.insert(L(1), Point2(4.1, 1.8)); + // Create (deliberately inaccurate) initial estimate + Values initialEstimate; + initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(L(0), Point2(1.8, 2.1)); + initialEstimate.insert(L(1), Point2(4.1, 1.8)); -// // We want to eliminate variables not connected to DCFactors first. -// Ordering ordering; -// ordering += L(0); -// ordering += L(1); -// ordering += X(0); -// ordering += X(1); + // We want to eliminate variables not connected to DCFactors first. + Ordering ordering; + ordering += L(0); + ordering += L(1); + ordering += X(0); + ordering += X(1); -// HybridGaussianFactorGraph linearized = fg.linearize(initialEstimate); -// gtsam::HybridBayesNet::shared_ptr hybridBayesNet; -// gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; + HybridGaussianFactorGraph linearized = fg.linearize(initialEstimate); + gtsam::HybridBayesNet::shared_ptr hybridBayesNet; + gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; -// // This should NOT fail -// std::tie(hybridBayesNet, remainingFactorGraph) = -// linearized.eliminatePartialSequential(ordering); -// EXPECT_LONGS_EQUAL(4, hybridBayesNet->size()); -// EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size()); -// } + // This should NOT fail + std::tie(hybridBayesNet, remainingFactorGraph) = + linearized.eliminatePartialSequential(ordering); + EXPECT_LONGS_EQUAL(4, hybridBayesNet->size()); + EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size()); +} /* ************************************************************************* */ int main() {