diff --git a/gtsam/hybrid/tests/testHybridSmoother.cpp b/gtsam/hybrid/tests/testHybridSmoother.cpp index 3a0f376cc..893740b7c 100644 --- a/gtsam/hybrid/tests/testHybridSmoother.cpp +++ b/gtsam/hybrid/tests/testHybridSmoother.cpp @@ -39,6 +39,7 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::L; using symbol_shorthand::X; using symbol_shorthand::Z; @@ -69,6 +70,56 @@ 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) {