diff --git a/gtsam/hybrid/tests/testHybridSmoother.cpp b/gtsam/hybrid/tests/testHybridSmoother.cpp index bd85501d8..960649b64 100644 --- a/gtsam/hybrid/tests/testHybridSmoother.cpp +++ b/gtsam/hybrid/tests/testHybridSmoother.cpp @@ -70,56 +70,6 @@ Switching InitializeEstimationProblem( } // namespace estimation_fixture -/****************************************************************************/ -// Test HybridSmoother::addConditionals -TEST(HybridSmoother, AddConditionals) { - using namespace estimation_fixture; - - size_t K = 7; - - // Switching example of robot moving in 1D - // with given measurements and equal mode priors. - HybridNonlinearFactorGraph graph; - Values initial; - Switching switching = InitializeEstimationProblem( - K, 1.0, 0.1, measurements, "1/1 1/1", &graph, &initial); - - HybridSmoother smoother(0.5); - constexpr size_t maxNrLeaves = 5; - - // Loop over timesteps from 1...K-1 - for (size_t k = 1; k < K; k++) { - if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain - graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model - graph.push_back(switching.unaryFactors.at(k)); // Measurement - - // Add a loop closure factor every 3rd step between `k-3` and `k`. - if (k % 3 == 0) { - size_t l = int(k / 3); - auto open = std::make_shared( - X(k - 3), X(k), 0.0, noiseModel::Isotropic::Sigma(1, 10.0)), - close = std::make_shared( - X(k - 3), X(k), 0.0, noiseModel::Isotropic::Sigma(1, 0.1)); - - HybridNonlinearFactor loopFactor( - DiscreteKey(L(l), 2), - std::vector{open, close}); - graph.push_back(loopFactor); - } - - initial.insert(X(k), switching.linearizationPoint.at(X(k))); - - HybridGaussianFactorGraph linearized = *graph.linearize(initial); - - smoother.update(linearized, maxNrLeaves); - - // Clear all the factors from the graph - graph.resize(0); - } - - //TODO(Varun) recreate scenario which would fail before commit b627fc422. -} - /****************************************************************************/ // Test approximate inference with an additional pruning step. TEST(HybridSmoother, IncrementalSmoother) {