diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index ef2ae9c41..4b7dd9fd6 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -115,103 +115,6 @@ TEST(HybridEstimation, Full) { EXPECT(assert_equal(expected_continuous, result)); } -/****************************************************************************/ -// Test approximate inference with an additional pruning step. -TEST(HybridEstimation, IncrementalSmoother) { - using namespace estimation_fixture; - - size_t K = 15; - - // 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; - - HybridGaussianFactorGraph linearized; - - constexpr size_t maxNrLeaves = 3; - 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 - - initial.insert(X(k), switching.linearizationPoint.at(X(k))); - - linearized = *graph.linearize(initial); - Ordering ordering = smoother.getOrdering(linearized); - - smoother.update(linearized, maxNrLeaves, ordering); - graph.resize(0); - } - - HybridValues delta = smoother.hybridBayesNet().optimize(); - - Values result = initial.retract(delta.continuous()); - - DiscreteValues expected_discrete; - for (size_t k = 0; k < K - 1; k++) { - expected_discrete[M(k)] = discrete_seq[k]; - } - EXPECT(assert_equal(expected_discrete, delta.discrete())); - - Values expected_continuous; - for (size_t k = 0; k < K; k++) { - expected_continuous.insert(X(k), measurements[k]); - } - EXPECT(assert_equal(expected_continuous, result)); -} - -/****************************************************************************/ -// Test if pruned factor is set to correct error and no errors are thrown. -TEST(HybridEstimation, ValidPruningError) { - using namespace estimation_fixture; - - size_t K = 8; - - HybridNonlinearFactorGraph graph; - Values initial; - Switching switching = InitializeEstimationProblem(K, 1e-2, 1e-3, measurements, - "1/1 1/1", graph, initial); - HybridSmoother smoother; - - HybridGaussianFactorGraph linearized; - - constexpr size_t maxNrLeaves = 3; - 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 - - initial.insert(X(k), switching.linearizationPoint.at(X(k))); - - linearized = *graph.linearize(initial); - Ordering ordering = smoother.getOrdering(linearized); - - smoother.update(linearized, maxNrLeaves, ordering); - - graph.resize(0); - } - - HybridValues delta = smoother.hybridBayesNet().optimize(); - - Values result = initial.retract(delta.continuous()); - - DiscreteValues expected_discrete; - for (size_t k = 0; k < K - 1; k++) { - expected_discrete[M(k)] = discrete_seq[k]; - } - EXPECT(assert_equal(expected_discrete, delta.discrete())); - - Values expected_continuous; - for (size_t k = 0; k < K; k++) { - expected_continuous.insert(X(k), measurements[k]); - } - EXPECT(assert_equal(expected_continuous, result)); -} - /****************************************************************************/ // Test approximate inference with an additional pruning step. TEST(HybridEstimation, ISAM) { diff --git a/gtsam/hybrid/tests/testHybridSmoother.cpp b/gtsam/hybrid/tests/testHybridSmoother.cpp new file mode 100644 index 000000000..2a3be124e --- /dev/null +++ b/gtsam/hybrid/tests/testHybridSmoother.cpp @@ -0,0 +1,177 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridSmoother.cpp + * @brief Unit tests for HybridSmoother + * @author Varun Agrawal + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Include for test suite +#include + +#include + +#include "Switching.h" + +using namespace std; +using namespace gtsam; + +using symbol_shorthand::X; +using symbol_shorthand::Z; + +namespace estimation_fixture { +std::vector measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6, + 7, 8, 9, 9, 9, 10, 11, 11, 11, 11}; +// Ground truth discrete seq +std::vector discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0, + 1, 1, 1, 0, 0, 1, 1, 0, 0, 0}; + +Switching InitializeEstimationProblem( + const size_t K, const double between_sigma, const double measurement_sigma, + const std::vector& measurements, + const std::string& transitionProbabilityTable, + HybridNonlinearFactorGraph* graph, Values* initial) { + Switching switching(K, between_sigma, measurement_sigma, measurements, + transitionProbabilityTable); + + // Add prior on M(0) + graph->push_back(switching.modeChain.at(0)); + + // Add the X(0) prior + graph->push_back(switching.unaryFactors.at(0)); + initial->insert(X(0), switching.linearizationPoint.at(X(0))); + + return switching; +} + +} // namespace estimation_fixture + +/****************************************************************************/ +// Test approximate inference with an additional pruning step. +TEST(HybridSmoother, IncrementalSmoother) { + using namespace estimation_fixture; + + size_t K = 5; + + // 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; + 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 + + initial.insert(X(k), switching.linearizationPoint.at(X(k))); + + HybridGaussianFactorGraph linearized = *graph.linearize(initial); + Ordering ordering = smoother.getOrdering(linearized); + + smoother.update(linearized, maxNrLeaves, ordering); + + // Clear all the factors from the graph + graph.resize(0); + } + + EXPECT_LONGS_EQUAL(11, + smoother.hybridBayesNet().at(0)->asDiscrete()->nrValues()); + + // Get the continuous delta update as well as + // the optimal discrete assignment. + HybridValues delta = smoother.hybridBayesNet().optimize(); + + // Check discrete assignment + DiscreteValues expected_discrete; + for (size_t k = 0; k < K - 1; k++) { + expected_discrete[M(k)] = discrete_seq[k]; + } + EXPECT(assert_equal(expected_discrete, delta.discrete())); + + // Update nonlinear solution and verify + Values result = initial.retract(delta.continuous()); + Values expected_continuous; + for (size_t k = 0; k < K; k++) { + expected_continuous.insert(X(k), measurements[k]); + } + EXPECT(assert_equal(expected_continuous, result)); +} + +/****************************************************************************/ +// Test if pruned Bayes net is set to correct error and no errors are thrown. +TEST(HybridSmoother, ValidPruningError) { + using namespace estimation_fixture; + + size_t K = 8; + + // Switching example of robot moving in 1D + // with given measurements and equal mode priors. + HybridNonlinearFactorGraph graph; + Values initial; + Switching switching = InitializeEstimationProblem( + K, 0.1, 0.1, measurements, "1/1 1/1", &graph, &initial); + HybridSmoother smoother; + + constexpr size_t maxNrLeaves = 3; + 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 + + initial.insert(X(k), switching.linearizationPoint.at(X(k))); + + HybridGaussianFactorGraph linearized = *graph.linearize(initial); + Ordering ordering = smoother.getOrdering(linearized); + + smoother.update(linearized, maxNrLeaves, ordering); + + // Clear all the factors from the graph + graph.resize(0); + } + + EXPECT_LONGS_EQUAL(14, + smoother.hybridBayesNet().at(0)->asDiscrete()->nrValues()); + + // Get the continuous delta update as well as + // the optimal discrete assignment. + HybridValues delta = smoother.hybridBayesNet().optimize(); + + auto errorTree = smoother.hybridBayesNet().errorTree(delta.continuous()); + EXPECT_DOUBLES_EQUAL(0.0, errorTree(delta.discrete()), 1e-8); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */