diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b240e1626..daee44ab6 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -658,7 +658,7 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, } /* ****************************************************************************/ -// Check that the factor graph unnormalized probability is proportional to the +// Check that the bayes net unnormalized probability is proportional to the // Bayes net probability for the given measurements. bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, const HybridBayesNet &posterior, size_t num_samples = 100) { @@ -907,6 +907,102 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { EXPECT(ratioTest(bn, measurements, *posterior)); } +/* ****************************************************************************/ +// Test printErrors with multivariate example. +TEST(HybridGaussianFactorGraph, PrintErrors) { + HybridGaussianFactorGraph hfg; + HybridBayesNet bayesNet; + + size_t num_measurements = 1; + // Create Gaussian mixture z_i = x0 + noise for each measurement. + for (size_t i = 0; i < num_measurements; i++) { + const DiscreteKey mode_i{M(i), 2}; + bayesNet.emplace_back(new GaussianMixture( + {Z(i)}, {X(0)}, {mode_i}, + {GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, 10), + GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, + 0.1)})); + } + + // Create prior on X(0). + bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( + X(0), Vector3(1.0, 2.0, 5.0), 0.5)); + + // Add prior on mode. + const size_t nrModes = 1; + for (size_t i = 0; i < nrModes; i++) { + bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); + } + + VectorValues measurements{{Z(0), Vector3(1.0, 2.0, 5.0)}}; + HybridGaussianFactorGraph measurement_fg = + bayesNet.toFactorGraph(measurements); + HybridValues values = bayesNet.optimize(); + + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf *old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + measurement_fg.printErrors(values); + + // Get output string and reset stdout + std::string actual = buffer.str(); + std::cout.rdbuf(old); + + std::string expected = R"(HybridGaussianFactorGraph: size: 3 + +Factor 0: Hybrid [x0; m0]{ + Choice(m0) + 0 Leaf : + A[x0] = [ + -1, -0, -0; + -0, -1, -0; + -0, -0, -1; + 0, 0, 0 +] + b = [ -1 -2 -5 5.25652 ] + Noise model: diagonal sigmas [10; 10; 10; 1]; + + 1 Leaf : + A[x0] = [ + -1, -0, -0; + -0, -1, -0; + -0, -0, -1 +] + b = [ -1 -2 -5 ] +isotropic dim=3 sigma=0.1 + +} +error = Choice(m0) + 0 Leaf 13.815511 + 1 Leaf 0 + + +Factor 1: p(x0) + R = [ 1 0 0 ] + [ 0 1 0 ] + [ 0 0 1 ] + d = [ 1 2 5 ] + mean: 1 elements + x0: 1 2 5 +isotropic dim=3 sigma=0.5 +error = 0 + +Factor 2: P( m0 ): + Choice(m0) + 0 Leaf 0.4 + 1 Leaf 0.6 + +error = Choice(m0) + 0 Leaf 0.91629073 + 1 Leaf 0.51082562 + + +)"; + EXPECT(expected == actual); +} + /* ************************************************************************* */ int main() { TestResult tr;