testHybridGaussianFactorGraph.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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 testHybridGaussianFactorGraph.cpp
 *  @date Mar 11, 2022
 *  @author Fan Jiang
 *  @author Varun Agrawal
 *  @author Frank Dellaert
 */
 
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Vector.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
 
#include <cstddef>
#include <memory>
#include <vector>
 
#include "Switching.h"
#include "TinyHybridExample.h"
 
using namespace std;
using namespace gtsam;
 
using gtsam::symbol_shorthand::M;
using gtsam::symbol_shorthand::N;
using gtsam::symbol_shorthand::X;
using gtsam::symbol_shorthand::Z;
 
// Set up sampling
std::mt19937_64 kRng(42);
 
static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2);
 
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, Creation) {
  HybridConditional conditional;
 
  HybridGaussianFactorGraph hfg;
 
  hfg.emplace_shared<JacobianFactor>(X(0), I_3x3, Z_3x1);
 
  // Define a hybrid gaussian conditional P(x0|x1, c0)
  // and add it to the factor graph.
  HybridGaussianConditional gm(
      m0,
      {std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
       std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
                                             I_3x3)});
  hfg.add(gm);
 
  EXPECT_LONGS_EQUAL(2, hfg.size());
}
 
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, EliminateSequential) {
  // Test elimination of a single variable.
  HybridGaussianFactorGraph hfg;
 
  hfg.emplace_shared<JacobianFactor>(0, I_3x3, Z_3x1);
 
  auto result = hfg.eliminatePartialSequential(KeyVector{0});
 
  EXPECT_LONGS_EQUAL(result.first->size(), 1);
}
 
/* ************************************************************************* */
 
namespace two {
std::vector<GaussianFactor::shared_ptr> components(Key key) {
  return {std::make_shared<JacobianFactor>(key, I_3x3, Z_3x1),
          std::make_shared<JacobianFactor>(key, I_3x3, Vector3::Ones())};
}
}  // namespace two
 
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
  HybridGaussianFactorGraph hfg;
  hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
 
  auto result = hfg.eliminate({X(1)});
 
  // Check that we have a valid Gaussian conditional.
  auto hgc = result.first->asHybrid();
  CHECK(hgc);
  const HybridValues values{{{X(1), Z_3x1}}, {{M(1), 1}}};
  EXPECT(HybridConditional::CheckInvariants(*result.first, values));
 
  // Check that factor is discrete and correct
  auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
  CHECK(factor);
  // regression test
  EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5));
}
 
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
  HybridGaussianFactorGraph hfg;
 
  // Add prior on x0
  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
 
  // Add factor between x0 and x1
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
  // Add a hybrid gaussian factor ϕ(x1, c1)
  hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
 
  auto result = hfg.eliminateSequential();
 
  auto dc = result->at(2)->asDiscrete();
  CHECK(dc);
  DiscreteValues dv;
  dv[M(1)] = 0;
  // Regression test
  EXPECT_DOUBLES_EQUAL(0.62245933120185448, dc->operator()(dv), 1e-3);
}
 
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
  HybridGaussianFactorGraph hfg;
 
  // Add prior on x0
  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  // Add factor between x0 and x1
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
  hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
 
  // Discrete probability table for c1
  hfg.add(DecisionTreeFactor(m1, {2, 8}));
  // Joint discrete probability table for c1, c2
  hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4"));
 
  HybridBayesNet::shared_ptr result = hfg.eliminateSequential();
 
  // There are 4 variables (2 continuous + 2 discrete) in the bayes net.
  EXPECT_LONGS_EQUAL(4, result->size());
}
 
/* ************************************************************************* */
// Test API for the smallest switching network.
// None of these are regression tests.
TEST(HybridBayesNet, Switching) {
  // Create switching network with two continuous variables and one discrete:
  // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1;z1) ϕ(m0)
  const double betweenSigma = 0.3, priorSigma = 0.1;
  Switching s(2, betweenSigma, priorSigma);
 
  // Check size of linearized factor graph
  const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph();
  EXPECT_LONGS_EQUAL(4, graph.size());
 
  // Create some continuous and discrete values
  const VectorValues continuousValues{{X(0), Vector1(0.1)},
                                      {X(1), Vector1(1.2)}};
  const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}};
 
  // Get the hybrid gaussian factor and check it is as expected
  auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(graph.at(2));
  CHECK(hgf);
 
  // Get factors and scalars for both modes
  auto [factor0, scalar0] = (*hgf)(modeZero);
  auto [factor1, scalar1] = (*hgf)(modeOne);
  CHECK(factor0);
  CHECK(factor1);
 
  // Check scalars against negLogConstant of noise model
  auto betweenModel = noiseModel::Isotropic::Sigma(1, betweenSigma);
  EXPECT_DOUBLES_EQUAL(betweenModel->negLogConstant(), scalar0, 1e-9);
  EXPECT_DOUBLES_EQUAL(betweenModel->negLogConstant(), scalar1, 1e-9);
 
  // Check error for M(0) = 0
  const HybridValues values0{continuousValues, modeZero};
  double expectedError0 = 0;
  for (const auto &factor : graph) expectedError0 += factor->error(values0);
  EXPECT_DOUBLES_EQUAL(expectedError0, graph.error(values0), 1e-5);
 
  // Check error for M(0) = 1
  const HybridValues values1{continuousValues, modeOne};
  double expectedError1 = 0;
  for (const auto &factor : graph) expectedError1 += factor->error(values1);
  EXPECT_DOUBLES_EQUAL(expectedError1, graph.error(values1), 1e-5);
 
  // Check errorTree
  AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues);
  // Create expected error tree
  const AlgebraicDecisionTree<Key> expectedErrors(M(0), expectedError0,
                                                  expectedError1);
 
  // Check that the actual error tree matches the expected one
  EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5));
 
  // Check probPrime
  const double probPrime0 = graph.probPrime(values0);
  EXPECT_DOUBLES_EQUAL(std::exp(-expectedError0), probPrime0, 1e-5);
 
  const double probPrime1 = graph.probPrime(values1);
  EXPECT_DOUBLES_EQUAL(std::exp(-expectedError1), probPrime1, 1e-5);
 
  // Check discretePosterior
  const AlgebraicDecisionTree<Key> graphPosterior =
      graph.discretePosterior(continuousValues);
  const double sum = probPrime0 + probPrime1;
  const AlgebraicDecisionTree<Key> expectedPosterior(M(0), probPrime0 / sum,
                                                     probPrime1 / sum);
  EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5));
 
  // Make the clique of factors connected to x0:
  HybridGaussianFactorGraph factors_x0;
  factors_x0.push_back(graph.at(0));
  factors_x0.push_back(hgf);
 
  // Test collectProductFactor
  auto productFactor = factors_x0.collectProductFactor();
 
  // For M(0) = 0
  auto [gaussianFactor0, actualScalar0] = productFactor(modeZero);
  EXPECT(gaussianFactor0.size() == 2);
  EXPECT_DOUBLES_EQUAL((*hgf)(modeZero).second, actualScalar0, 1e-5);
 
  // For M(0) = 1
  auto [gaussianFactor1, actualScalar1] = productFactor(modeOne);
  EXPECT(gaussianFactor1.size() == 2);
  EXPECT_DOUBLES_EQUAL((*hgf)(modeOne).second, actualScalar1, 1e-5);
 
  // Test eliminate x0
  const Ordering ordering{X(0)};
  auto [conditional, factor] = factors_x0.eliminate(ordering);
 
  // Check the conditional
  CHECK(conditional);
  EXPECT(conditional->isHybrid());
  auto p_x0_given_x1_m = conditional->asHybrid();
  CHECK(p_x0_given_x1_m);
  EXPECT(HybridGaussianConditional::CheckInvariants(*p_x0_given_x1_m, values1));
  EXPECT_LONGS_EQUAL(1, p_x0_given_x1_m->nrFrontals());  // x0
  EXPECT_LONGS_EQUAL(2, p_x0_given_x1_m->nrParents());   // x1, m0
 
  // Check the remaining factor
  EXPECT(factor);
  EXPECT(std::dynamic_pointer_cast<HybridGaussianFactor>(factor));
  auto phi_x1_m = std::dynamic_pointer_cast<HybridGaussianFactor>(factor);
  EXPECT_LONGS_EQUAL(2, phi_x1_m->keys().size());  // x1, m0
  // Check that the scalars incorporate the negative log constant of the
  // conditional
  EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(),
                       (*phi_x1_m)(modeZero).second, 1e-9);
  EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(),
                       (*phi_x1_m)(modeOne).second, 1e-9);
 
  // Check that the conditional and remaining factor are consistent for both
  // modes
  for (auto &&mode : {modeZero, modeOne}) {
    const auto gc = (*p_x0_given_x1_m)(mode);
    const auto [gf, scalar] = (*phi_x1_m)(mode);
 
    // The error of the original factors should equal the sum of errors of the
    // conditional and remaining factor, modulo the normalization constant of
    // the conditional.
    double originalError = factors_x0.error({continuousValues, mode});
    const double actualError = gc->negLogConstant() +
                               gc->error(continuousValues) +
                               gf->error(continuousValues) + scalar;
    EXPECT_DOUBLES_EQUAL(originalError, actualError, 1e-9);
  }
 
  // Create a clique for x1
  HybridGaussianFactorGraph factors_x1;
  factors_x1.push_back(
      factor);  // Use the remaining factor from previous elimination
  factors_x1.push_back(
      graph.at(1));  // Add the factor for x1 from the original graph
 
  // Test collectProductFactor for x1 clique
  auto productFactor_x1 = factors_x1.collectProductFactor();
 
  // For M(0) = 0
  auto [gaussianFactor_x1_0, actualScalar_x1_0] = productFactor_x1(modeZero);
  EXPECT_LONGS_EQUAL(2, gaussianFactor_x1_0.size());
  // NOTE(Frank): prior on x1 does not contribute to the scalar
  EXPECT_DOUBLES_EQUAL((*phi_x1_m)(modeZero).second, actualScalar_x1_0, 1e-5);
 
  // For M(0) = 1
  auto [gaussianFactor_x1_1, actualScalar_x1_1] = productFactor_x1(modeOne);
  EXPECT_LONGS_EQUAL(2, gaussianFactor_x1_1.size());
  // NOTE(Frank): prior on x1 does not contribute to the scalar
  EXPECT_DOUBLES_EQUAL((*phi_x1_m)(modeOne).second, actualScalar_x1_1, 1e-5);
 
  // Test eliminate for x1 clique
  Ordering ordering_x1{X(1)};
  auto [conditional_x1, factor_x1] = factors_x1.eliminate(ordering_x1);
 
  // Check the conditional for x1
  CHECK(conditional_x1);
  EXPECT(conditional_x1->isHybrid());
  auto p_x1_given_m = conditional_x1->asHybrid();
  CHECK(p_x1_given_m);
  EXPECT_LONGS_EQUAL(1, p_x1_given_m->nrFrontals());  // x1
  EXPECT_LONGS_EQUAL(1, p_x1_given_m->nrParents());   // m0
 
  // Check the remaining factor for x1
  CHECK(factor_x1);
  auto phi_x1 = std::dynamic_pointer_cast<DecisionTreeFactor>(factor_x1);
  CHECK(phi_x1);
  EXPECT_LONGS_EQUAL(1, phi_x1->keys().size());  // m0
  // We can't really check the error of the decision tree factor phi_x1, because
  // the continuous factor whose error(kEmpty) we need is not available.
 
  // Now test full elimination of the graph:
  auto hybridBayesNet = graph.eliminateSequential();
  CHECK(hybridBayesNet);
 
  // Check that the posterior P(M|X=continuousValues) from the Bayes net is the
  // same as the same posterior from the graph. This is a sanity check that the
  // elimination is done correctly.
  AlgebraicDecisionTree<Key> bnPosterior =
      hybridBayesNet->discretePosterior(continuousValues);
  EXPECT(assert_equal(graphPosterior, bnPosterior));
}
 
/* ****************************************************************************/
// Test subset of API for switching network with 3 states.
// None of these are regression tests.
TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
  // Create switching network with three continuous variables and two discrete:
  // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1)
  Switching s(3);
 
  // Check size of linearized factor graph
  const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph();
  EXPECT_LONGS_EQUAL(7, graph.size());
 
  // Eliminate the graph
  const HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
 
  const HybridValues delta = hybridBayesNet->optimize();
  const double error = graph.error(delta);
 
  // Check that the probability prime is the exponential of the error
  EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7));
 
  // Check that the posterior P(M|X=continuousValues) from the Bayes net is the
  // same as the same posterior from the graph. This is a sanity check that the
  // elimination is done correctly.
  const AlgebraicDecisionTree<Key> graphPosterior =
      graph.discretePosterior(delta.continuous());
  const AlgebraicDecisionTree<Key> bnPosterior =
      hybridBayesNet->discretePosterior(delta.continuous());
  EXPECT(assert_equal(graphPosterior, bnPosterior));
}
 
/* ************************************************************************* */
// Select a particular continuous factor graph given a discrete assignment
TEST(HybridGaussianFactorGraph, DiscreteSelection) {
  Switching s(3);
 
  HybridGaussianFactorGraph graph = s.linearizedFactorGraph();
 
  DiscreteValues dv00{{M(0), 0}, {M(1), 0}};
  GaussianFactorGraph continuous_00 = graph(dv00);
  GaussianFactorGraph expected_00;
  expected_00.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
  expected_00.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
  expected_00.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
  expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1)));
  expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1)));
 
  EXPECT(assert_equal(expected_00, continuous_00));
 
  DiscreteValues dv01{{M(0), 0}, {M(1), 1}};
  GaussianFactorGraph continuous_01 = graph(dv01);
  GaussianFactorGraph expected_01;
  expected_01.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
  expected_01.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
  expected_01.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
  expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1)));
  expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0)));
 
  EXPECT(assert_equal(expected_01, continuous_01));
 
  DiscreteValues dv10{{M(0), 1}, {M(1), 0}};
  GaussianFactorGraph continuous_10 = graph(dv10);
  GaussianFactorGraph expected_10;
  expected_10.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
  expected_10.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
  expected_10.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
  expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0)));
  expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1)));
 
  EXPECT(assert_equal(expected_10, continuous_10));
 
  DiscreteValues dv11{{M(0), 1}, {M(1), 1}};
  GaussianFactorGraph continuous_11 = graph(dv11);
  GaussianFactorGraph expected_11;
  expected_11.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
  expected_11.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
  expected_11.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
  expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0)));
  expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0)));
 
  EXPECT(assert_equal(expected_11, continuous_11));
}
 
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, Optimize) {
  HybridGaussianFactorGraph hfg;
 
  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
  hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
 
  auto result = hfg.eliminateSequential();
 
  HybridValues hv = result->optimize();
 
  EXPECT(assert_equal(hv.atDiscrete(M(1)), int(0)));
}
 
/* ************************************************************************* */
// Test adding of gaussian conditional and re-elimination.
TEST(HybridGaussianFactorGraph, Conditionals) {
  Switching switching(4);
 
  HybridGaussianFactorGraph hfg;
  hfg.push_back(switching.linearUnaryFactors.at(0));  // P(X0)
  Ordering ordering;
  ordering.push_back(X(0));
  HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering);
 
  HybridGaussianFactorGraph hfg2;
  hfg2.push_back(*bayes_net);                           // P(X0)
  hfg2.push_back(switching.linearBinaryFactors.at(0));  // P(X0, X1 | M0)
  hfg2.push_back(switching.linearBinaryFactors.at(1));  // P(X1, X2 | M1)
  hfg2.push_back(switching.linearUnaryFactors.at(2));   // P(X2)
  ordering += X(1), X(2), M(0), M(1);
 
  // Created product of first two factors and check eliminate:
  HybridGaussianFactorGraph fragment;
  fragment.push_back(hfg2[0]);
  fragment.push_back(hfg2[1]);
 
  // Check that product
  HybridGaussianProductFactor product = fragment.collectProductFactor();
  auto leaf = fragment(DiscreteValues{{M(0), 0}});
  EXPECT_LONGS_EQUAL(2, leaf.size());
 
  // Check product and that pruneEmpty does not touch it
  auto pruned = product.removeEmpty();
  LONGS_EQUAL(2, pruned.nrLeaves());
 
  // Test eliminate
  auto [hybridConditional, factor] = fragment.eliminate({X(0)});
  EXPECT(hybridConditional->isHybrid());
  EXPECT(hybridConditional->keys() == KeyVector({X(0), X(1), M(0)}));
 
  EXPECT(dynamic_pointer_cast<HybridGaussianFactor>(factor));
  EXPECT(factor->keys() == KeyVector({X(1), M(0)}));
 
  bayes_net = hfg2.eliminateSequential(ordering);
 
  HybridValues result = bayes_net->optimize();
 
  Values expected_continuous;
  expected_continuous.insert<double>(X(0), 0);
  expected_continuous.insert<double>(X(1), 1);
  expected_continuous.insert<double>(X(2), 2);
  expected_continuous.insert<double>(X(3), 4);
  Values result_continuous =
      switching.linearizationPoint.retract(result.continuous());
  EXPECT(assert_equal(expected_continuous, result_continuous));
 
  DiscreteValues expected_discrete;
  expected_discrete[M(0)] = 1;
  expected_discrete[M(1)] = 1;
  EXPECT(assert_equal(expected_discrete, result.discrete()));
}
 
/* ****************************************************************************/
// Test hybrid gaussian factor graph errorTree during incremental operation
TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
  Switching s(4);
 
  HybridGaussianFactorGraph graph;
  graph.push_back(s.linearUnaryFactors.at(0));   // f(X0)
  graph.push_back(s.linearBinaryFactors.at(0));  // f(X0, X1, M0)
  graph.push_back(s.linearBinaryFactors.at(1));  // f(X1, X2, M1)
  graph.push_back(s.linearUnaryFactors.at(1));   // f(X1)
  graph.push_back(s.linearUnaryFactors.at(2));   // f(X2)
  graph.push_back(s.modeChain.at(0));            // f(M0)
  graph.push_back(s.modeChain.at(1));            // f(M0, M1)
 
  HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
  EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
 
  // Check discrete posterior at optimum
  HybridValues delta = hybridBayesNet->optimize();
  AlgebraicDecisionTree<Key> graphPosterior =
      graph.discretePosterior(delta.continuous());
  AlgebraicDecisionTree<Key> bnPosterior =
      hybridBayesNet->discretePosterior(delta.continuous());
  EXPECT(assert_equal(graphPosterior, bnPosterior));
 
  graph = HybridGaussianFactorGraph();
  graph.push_back(*hybridBayesNet);
  graph.push_back(s.linearBinaryFactors.at(2));  // f(X2, X3, M2)
  graph.push_back(s.linearUnaryFactors.at(3));   // f(X3)
 
  hybridBayesNet = graph.eliminateSequential();
  EXPECT_LONGS_EQUAL(7, hybridBayesNet->size());
 
  delta = hybridBayesNet->optimize();
  graphPosterior = graph.discretePosterior(delta.continuous());
  bnPosterior = hybridBayesNet->discretePosterior(delta.continuous());
  EXPECT(assert_equal(graphPosterior, bnPosterior));
}
 
/* ****************************************************************************/
// Check that collectProductFactor works correctly.
TEST(HybridGaussianFactorGraph, CollectProductFactor) {
  const int num_measurements = 1;
  VectorValues vv{{Z(0), Vector1(5.0)}};
  auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, vv);
  EXPECT_LONGS_EQUAL(3, fg.size());
 
  // Assemble graph tree:
  auto actual = fg.collectProductFactor();
 
  // Create expected decision tree with two factor graphs:
 
  // Get hybrid factor:
  auto hybrid = fg.at<HybridGaussianFactor>(0);
  CHECK(hybrid);
 
  // Get prior factor:
  const auto gf = fg.at<HybridConditional>(1);
  CHECK(gf);
  using GF = GaussianFactor::shared_ptr;
  const GF prior = gf->asGaussian();
  CHECK(prior);
 
  // Create DiscreteValues for both 0 and 1:
  DiscreteValues d0{{M(0), 0}}, d1{{M(0), 1}};
 
  // Expected decision tree with two factor graphs:
  // f(x0;mode=0)P(x0)
  GaussianFactorGraph expectedFG0{(*hybrid)(d0).first, prior};
  EXPECT(assert_equal(expectedFG0, actual(d0).first, 1e-5));
  EXPECT(assert_equal(0.0, actual(d0).second, 1e-5));
 
  // f(x0;mode=1)P(x0)
  GaussianFactorGraph expectedFG1{(*hybrid)(d1).first, prior};
  EXPECT(assert_equal(expectedFG1, actual(d1).first, 1e-5));
  EXPECT(assert_equal(1.79176, actual(d1).second, 1e-5));
}
 
/* ****************************************************************************/
// Check that the factor graph unnormalized probability is proportional to the
// Bayes net probability for the given measurements.
bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
               const HybridGaussianFactorGraph &fg, size_t num_samples = 100) {
  auto compute_ratio = [&](HybridValues *sample) -> double {
    sample->update(measurements);  // update sample with given measurements:
    return bn.evaluate(*sample) / fg.probPrime(*sample);
  };
 
  HybridValues sample = bn.sample(&kRng);
  double expected_ratio = compute_ratio(&sample);
 
  // Test ratios for a number of independent samples:
  for (size_t i = 0; i < num_samples; i++) {
    HybridValues sample = bn.sample(&kRng);
    if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6) return false;
  }
  return true;
}
 
/* ****************************************************************************/
// 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) {
  auto compute_ratio = [&](HybridValues *sample) -> double {
    sample->update(measurements);  // update sample with given measurements:
    return bn.evaluate(*sample) / posterior.evaluate(*sample);
  };
 
  HybridValues sample = bn.sample(&kRng);
  double expected_ratio = compute_ratio(&sample);
 
  // Test ratios for a number of independent samples:
  for (size_t i = 0; i < num_samples; i++) {
    HybridValues sample = bn.sample(&kRng);
    // std::cout << "ratio: " << compute_ratio(&sample) << std::endl;
    if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6) return false;
  }
  return true;
}
 
/* ****************************************************************************/
// Check that eliminating tiny net with 1 measurement yields correct result.
TEST(HybridGaussianFactorGraph, EliminateTiny1) {
  const int num_measurements = 1;
  const VectorValues measurements{{Z(0), Vector1(5.0)}};
  auto bn = tiny::createHybridBayesNet(num_measurements);
  auto fg = bn.toFactorGraph(measurements);
  EXPECT_LONGS_EQUAL(3, fg.size());
 
  EXPECT(ratioTest(bn, measurements, fg));
 
  // Create expected Bayes Net:
  HybridBayesNet expectedBayesNet;
 
  // Create hybrid Gaussian factor on X(0).
  using tiny::mode;
  // regression, but mean checked to be 5.0 in both cases:
  const auto conditional0 = std::make_shared<GaussianConditional>(
                 X(0), Vector1(14.1421), I_1x1 * 2.82843),
             conditional1 = std::make_shared<GaussianConditional>(
                 X(0), Vector1(10.1379), I_1x1 * 2.02759);
  expectedBayesNet.emplace_shared<HybridGaussianConditional>(
      mode, std::vector{conditional0, conditional1});
 
  // Add prior on mode.
  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
 
  // Test elimination
  const auto posterior = fg.eliminateSequential();
  EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
 
  EXPECT(ratioTest(bn, measurements, *posterior));
}
 
/* ****************************************************************************/
// Check that eliminating tiny net with 1 measurement with mode order swapped
// yields correct result.
TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
  const VectorValues measurements{{Z(0), Vector1(5.0)}};
 
  HybridBayesNet bn;
 
  // mode-dependent: 1 is low-noise, 0 is high-noise.
  // Create hybrid Gaussian factor z_0 = x0 + noise for each measurement.
  std::vector<std::pair<Vector, double>> parms{{Z_1x1, 3}, {Z_1x1, 0.5}};
  bn.emplace_shared<HybridGaussianConditional>(m1, Z(0), I_1x1, X(0), parms);
 
  // Create prior on X(0).
  bn.push_back(
      GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5));
 
  // Add prior on m1.
  bn.emplace_shared<DiscreteConditional>(m1, "1/1");
 
  // bn.print();
  auto fg = bn.toFactorGraph(measurements);
  EXPECT_LONGS_EQUAL(3, fg.size());
 
  // fg.print();
 
  EXPECT(ratioTest(bn, measurements, fg));
 
  // Create expected Bayes Net:
  HybridBayesNet expectedBayesNet;
 
  // Create hybrid Gaussian factor on X(0).
  // regression, but mean checked to be 5.0 in both cases:
  const auto conditional0 = std::make_shared<GaussianConditional>(
                 X(0), Vector1(10.1379), I_1x1 * 2.02759),
             conditional1 = std::make_shared<GaussianConditional>(
                 X(0), Vector1(14.1421), I_1x1 * 2.82843);
  expectedBayesNet.emplace_shared<HybridGaussianConditional>(
      m1, std::vector{conditional0, conditional1});
 
  // Add prior on m1.
  expectedBayesNet.emplace_shared<DiscreteConditional>(m1, "1/1");
 
  // Test elimination
  const auto posterior = fg.eliminateSequential();
  // EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
 
  EXPECT(ratioTest(bn, measurements, *posterior));
 
  // posterior->print();
  // posterior->optimize().print();
}
 
/* ****************************************************************************/
// Check that eliminating tiny net with 2 measurements yields correct result.
TEST(HybridGaussianFactorGraph, EliminateTiny2) {
  // Create factor graph with 2 measurements such that posterior mean = 5.0.
  const int num_measurements = 2;
  const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}};
  auto bn = tiny::createHybridBayesNet(num_measurements);
  auto fg = bn.toFactorGraph(measurements);
  EXPECT_LONGS_EQUAL(4, fg.size());
 
  // Create expected Bayes Net:
  HybridBayesNet expectedBayesNet;
 
  // Create hybrid Gaussian factor on X(0).
  using tiny::mode;
  // regression, but mean checked to be 5.0 in both cases:
  const auto conditional0 = std::make_shared<GaussianConditional>(
                 X(0), Vector1(17.3205), I_1x1 * 3.4641),
             conditional1 = std::make_shared<GaussianConditional>(
                 X(0), Vector1(10.274), I_1x1 * 2.0548);
  expectedBayesNet.emplace_shared<HybridGaussianConditional>(
      mode, std::vector{conditional0, conditional1});
 
  // Add prior on mode.
  expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
 
  // Test elimination
  const auto posterior = fg.eliminateSequential();
  EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
 
  EXPECT(ratioTest(bn, measurements, *posterior));
}
 
/* ****************************************************************************/
// Test eliminating tiny net with 1 mode per measurement.
TEST(HybridGaussianFactorGraph, EliminateTiny22) {
  // Create factor graph with 2 measurements such that posterior mean = 5.0.
  const int num_measurements = 2;
  const bool manyModes = true;
 
  // Create Bayes net and convert to factor graph.
  auto bn = tiny::createHybridBayesNet(num_measurements, manyModes);
  const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}};
  auto fg = bn.toFactorGraph(measurements);
  EXPECT_LONGS_EQUAL(5, fg.size());
 
  EXPECT(ratioTest(bn, measurements, fg));
 
  // Test elimination
  const auto posterior = fg.eliminateSequential();
 
  EXPECT(ratioTest(bn, measurements, *posterior));
}
 
/* ****************************************************************************/
// Test elimination of a switching network with one mode per measurement.
TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
  // Create a switching network with one mode per measurement.
  HybridBayesNet bn;
 
  // NOTE: we add reverse topological so we can sample from the Bayes net.:
 
  // Add measurements:
  std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3},
                                                           {Z_1x1, 0.5}};
  for (size_t t : {0, 1, 2}) {
    // Create hybrid Gaussian factor on Z(t) conditioned on X(t) and mode N(t):
    const auto noise_mode_t = DiscreteKey{N(t), 2};
    bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, Z(t), I_1x1,
                                                 X(t), measurementModels);
 
    // Create prior on discrete mode N(t):
    bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
  }
 
  // Add motion models. TODO(frank): why are they exactly the same?
  std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2},
                                                      {Z_1x1, 0.2}};
  for (size_t t : {2, 1}) {
    // Create hybrid Gaussian factor on X(t) conditioned on X(t-1)
    // and mode M(t-1):
    const auto motion_model_t = DiscreteKey{M(t), 2};
    bn.emplace_shared<HybridGaussianConditional>(motion_model_t, X(t), I_1x1,
                                                 X(t - 1), motionModels);
 
    // Create prior on motion model M(t):
    bn.emplace_shared<DiscreteConditional>(motion_model_t, "40/60");
  }
 
  // Create Gaussian prior on continuous X(0) using sharedMeanAndStddev:
  bn.push_back(GaussianConditional::sharedMeanAndStddev(X(0), Z_1x1, 0.1));
 
  // Make sure we an sample from the Bayes net:
  EXPECT_LONGS_EQUAL(6, bn.sample().continuous().size());
 
  // Create measurements consistent with moving right every time:
  const VectorValues measurements{
      {Z(0), Vector1(0.0)}, {Z(1), Vector1(1.0)}, {Z(2), Vector1(2.0)}};
  const HybridGaussianFactorGraph fg = bn.toFactorGraph(measurements);
 
  // Factor graph is:
  //      D     D
  //      |     |
  //      m1    m2
  //      |     |
  // C-x0-HC-x1-HC-x2
  //   |     |     |
  //   HF    HF    HF
  //   |     |     |
  //   n0    n1    n2
  //   |     |     |
  //   D     D     D
  EXPECT_LONGS_EQUAL(11, fg.size());
  EXPECT(ratioTest(bn, measurements, fg));
 
  // Do elimination of X(2) only:
  auto [bn1, fg1] = fg.eliminatePartialSequential(Ordering{X(2)});
  fg1->push_back(*bn1);
  EXPECT(ratioTest(bn, measurements, *fg1));
 
  // Create ordering that eliminates in time order, then discrete modes:
  Ordering ordering{X(2), X(1), X(0), N(0), N(1), N(2), M(1), M(2)};
 
  // Do elimination:
  const HybridBayesNet::shared_ptr posterior = fg.eliminateSequential(ordering);
 
  // Test resulting posterior Bayes net has correct size:
  EXPECT_LONGS_EQUAL(8, posterior->size());
 
  // Ratio test
  EXPECT(ratioTest(bn, measurements, *posterior));
}
 
/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */