testHybridNonlinearFactorGraph.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
 * -------------------------------------------------------------------------- */
 
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/utilities.h>
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
 
#include <numeric>
 
#include "Switching.h"
 
// Include for test suite
#include <CppUnitLite/TestHarness.h>
 
using namespace std;
using namespace gtsam;
using noiseModel::Isotropic;
using symbol_shorthand::L;
using symbol_shorthand::M;
using symbol_shorthand::X;
 
/* ****************************************************************************
 * Test that any linearizedFactorGraph gaussian factors are appended to the
 * existing gaussian factor graph in the hybrid factor graph.
 */
TEST(HybridFactorGraph, GaussianFactorGraph) {
  HybridNonlinearFactorGraph fg;
 
  // Add a simple prior factor to the nonlinear factor graph
  fg.emplace_shared<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1));
 
  // Linearization point
  Values linearizationPoint;
  linearizationPoint.insert<double>(X(0), 0);
 
  // Linearize the factor graph.
  HybridGaussianFactorGraph ghfg = *fg.linearize(linearizationPoint);
  EXPECT_LONGS_EQUAL(1, ghfg.size());
 
  // Check that the error is the same for the nonlinear values.
  const VectorValues zero{{X(0), Vector1(0)}};
  const HybridValues hybridValues{zero, {}, linearizationPoint};
  EXPECT_DOUBLES_EQUAL(fg.error(hybridValues), ghfg.error(hybridValues), 1e-9);
}
 
/***************************************************************************
 * Test equality for Hybrid Nonlinear Factor Graph.
 */
TEST(HybridNonlinearFactorGraph, Equals) {
  HybridNonlinearFactorGraph graph1;
  HybridNonlinearFactorGraph graph2;
 
  // Test empty factor graphs
  EXPECT(assert_equal(graph1, graph2));
 
  auto f0 = std::make_shared<PriorFactor<Pose2>>(
      1, Pose2(), noiseModel::Isotropic::Sigma(3, 0.001));
  graph1.push_back(f0);
  graph2.push_back(f0);
 
  auto f1 = std::make_shared<BetweenFactor<Pose2>>(
      1, 2, Pose2(), noiseModel::Isotropic::Sigma(3, 0.1));
  graph1.push_back(f1);
  graph2.push_back(f1);
 
  // Test non-empty graphs
  EXPECT(assert_equal(graph1, graph2));
}
 
/***************************************************************************
 * Test that the resize method works correctly for a HybridNonlinearFactorGraph.
 */
TEST(HybridNonlinearFactorGraph, Resize) {
  HybridNonlinearFactorGraph fg;
  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>();
  fg.push_back(nonlinearFactor);
 
  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
  fg.push_back(discreteFactor);
 
  auto dcFactor = std::make_shared<MixtureFactor>();
  fg.push_back(dcFactor);
 
  EXPECT_LONGS_EQUAL(fg.size(), 3);
 
  fg.resize(0);
  EXPECT_LONGS_EQUAL(fg.size(), 0);
}
 
/***************************************************************************
 * Test that the resize method works correctly for a
 * HybridGaussianFactorGraph.
 */
TEST(HybridGaussianFactorGraph, Resize) {
  HybridNonlinearFactorGraph nhfg;
  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
      X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
  nhfg.push_back(nonlinearFactor);
  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
  nhfg.push_back(discreteFactor);
 
  KeyVector contKeys = {X(0), X(1)};
  auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
  auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
       moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
 
  std::vector<MotionModel::shared_ptr> components = {still, moving};
  auto dcFactor = std::make_shared<MixtureFactor>(
      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
  nhfg.push_back(dcFactor);
 
  Values linearizationPoint;
  linearizationPoint.insert<double>(X(0), 0);
  linearizationPoint.insert<double>(X(1), 1);
 
  // Generate `HybridGaussianFactorGraph` by linearizing
  HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint);
 
  EXPECT_LONGS_EQUAL(gfg.size(), 3);
 
  gfg.resize(0);
  EXPECT_LONGS_EQUAL(gfg.size(), 0);
}
 
/***************************************************************************
 * Test that the MixtureFactor reports correctly if the number of continuous
 * keys provided do not match the keys in the factors.
 */
TEST(HybridGaussianFactorGraph, MixtureFactor) {
  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
      X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
 
  auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
  auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
       moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
 
  std::vector<MotionModel::shared_ptr> components = {still, moving};
 
  // Check for exception when number of continuous keys are under-specified.
  KeyVector contKeys = {X(0)};
  THROWS_EXCEPTION(std::make_shared<MixtureFactor>(
      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
 
  // Check for exception when number of continuous keys are too many.
  contKeys = {X(0), X(1), X(2)};
  THROWS_EXCEPTION(std::make_shared<MixtureFactor>(
      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
}
 
/*****************************************************************************
 * Test push_back on HFG makes the correct distinction.
 */
TEST(HybridFactorGraph, PushBack) {
  HybridNonlinearFactorGraph fg;
 
  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>();
  fg.push_back(nonlinearFactor);
 
  EXPECT_LONGS_EQUAL(fg.size(), 1);
 
  fg = HybridNonlinearFactorGraph();
 
  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
  fg.push_back(discreteFactor);
 
  EXPECT_LONGS_EQUAL(fg.size(), 1);
 
  fg = HybridNonlinearFactorGraph();
 
  auto dcFactor = std::make_shared<MixtureFactor>();
  fg.push_back(dcFactor);
 
  EXPECT_LONGS_EQUAL(fg.size(), 1);
 
  // Now do the same with HybridGaussianFactorGraph
  HybridGaussianFactorGraph ghfg;
 
  auto gaussianFactor = std::make_shared<JacobianFactor>();
  ghfg.push_back(gaussianFactor);
 
  EXPECT_LONGS_EQUAL(ghfg.size(), 1);
 
  ghfg = HybridGaussianFactorGraph();
  ghfg.push_back(discreteFactor);
 
  EXPECT_LONGS_EQUAL(ghfg.size(), 1);
 
  ghfg = HybridGaussianFactorGraph();
  ghfg.push_back(dcFactor);
 
  HybridGaussianFactorGraph hgfg2;
  hgfg2.push_back(ghfg.begin(), ghfg.end());
 
  EXPECT_LONGS_EQUAL(ghfg.size(), 1);
 
  HybridNonlinearFactorGraph hnfg;
  NonlinearFactorGraph factors;
  auto noise = noiseModel::Isotropic::Sigma(3, 1.0);
  factors.emplace_shared<PriorFactor<Pose2>>(0, Pose2(0, 0, 0), noise);
  factors.emplace_shared<PriorFactor<Pose2>>(1, Pose2(1, 0, 0), noise);
  factors.emplace_shared<PriorFactor<Pose2>>(2, Pose2(2, 0, 0), noise);
  // TODO(Varun) This does not currently work. It should work once HybridFactor
  // becomes a base class of NonlinearFactor.
  // hnfg.push_back(factors.begin(), factors.end());
 
  // EXPECT_LONGS_EQUAL(3, hnfg.size());
}
 
/****************************************************************************
 * Test construction of switching-like hybrid factor graph.
 */
TEST(HybridFactorGraph, Switching) {
  Switching self(3);
 
  EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph.size());
  EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph.size());
}
 
/****************************************************************************
 * Test linearization on a switching-like hybrid factor graph.
 */
TEST(HybridFactorGraph, Linearization) {
  Switching self(3);
 
  // Linearize here:
  HybridGaussianFactorGraph actualLinearized =
      *self.nonlinearFactorGraph.linearize(self.linearizationPoint);
 
  EXPECT_LONGS_EQUAL(7, actualLinearized.size());
}
 
/****************************************************************************
 * Test elimination tree construction
 */
TEST(HybridFactorGraph, EliminationTree) {
  Switching self(3);
 
  // Create ordering.
  Ordering ordering;
  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
 
  // Create elimination tree.
  HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
  EXPECT_LONGS_EQUAL(1, etree.roots().size())
}
 
/****************************************************************************
 *Test elimination function by eliminating x0 in *-x0-*-x1 graph.
 */
TEST(GaussianElimination, Eliminate_x0) {
  Switching self(3);
 
  // Gather factors on x1, has a simple Gaussian and a mixture factor.
  HybridGaussianFactorGraph factors;
  // Add gaussian prior
  factors.push_back(self.linearizedFactorGraph[0]);
  // Add first hybrid factor
  factors.push_back(self.linearizedFactorGraph[1]);
 
  // Eliminate x0
  const Ordering ordering{X(0)};
 
  auto result = EliminateHybrid(factors, ordering);
  CHECK(result.first);
  EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
  CHECK(result.second);
  // Has two keys, x2 and m1
  EXPECT_LONGS_EQUAL(2, result.second->size());
}
 
/****************************************************************************
 * Test elimination function by eliminating x1 in x0-*-x1-*-x2 chain.
 *                                                m0/      \m1
 */
TEST(HybridsGaussianElimination, Eliminate_x1) {
  Switching self(3);
 
  // Gather factors on x1, will be two mixture factors (with x0 and x2, resp.).
  HybridGaussianFactorGraph factors;
  factors.push_back(self.linearizedFactorGraph[1]);  // involves m0
  factors.push_back(self.linearizedFactorGraph[2]);  // involves m1
 
  // Eliminate x1
  const Ordering ordering{X(1)};
 
  auto result = EliminateHybrid(factors, ordering);
  CHECK(result.first);
  EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
  CHECK(result.second);
  // Note: separator keys should include m1, m2.
  EXPECT_LONGS_EQUAL(4, result.second->size());
}
 
/****************************************************************************
 * Helper method to generate gaussian factor graphs with a specific mode.
 */
GaussianFactorGraph::shared_ptr batchGFG(double between,
                                         Values linearizationPoint) {
  NonlinearFactorGraph graph;
  graph.addPrior<double>(X(0), 0, Isotropic::Sigma(1, 0.1));
 
  auto between_x0_x1 = std::make_shared<MotionModel>(X(0), X(1), between,
                                                     Isotropic::Sigma(1, 1.0));
 
  graph.push_back(between_x0_x1);
 
  return graph.linearize(linearizationPoint);
}
 
/****************************************************************************
 * Test elimination function by eliminating x0 and x1 in graph.
 */
TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
  Switching self(2, 1.0, 0.1);
 
  auto factors = self.linearizedFactorGraph;
 
  // Eliminate x0
  const Ordering ordering{X(0), X(1)};
 
  const auto [hybridConditionalMixture, factorOnModes] =
      EliminateHybrid(factors, ordering);
 
  auto gaussianConditionalMixture =
      dynamic_pointer_cast<GaussianMixture>(hybridConditionalMixture->inner());
 
  CHECK(gaussianConditionalMixture);
  // Frontals = [x0, x1]
  EXPECT_LONGS_EQUAL(2, gaussianConditionalMixture->nrFrontals());
  // 1 parent, which is the mode
  EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents());
 
  // This is now a discreteFactor
  auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
  CHECK(discreteFactor);
  EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
  EXPECT(discreteFactor->root_->isLeaf() == false);
}
 
/****************************************************************************
 * Test partial elimination
 */
TEST(HybridFactorGraph, Partial_Elimination) {
  Switching self(3);
 
  auto linearizedFactorGraph = self.linearizedFactorGraph;
 
  // Create ordering of only continuous variables.
  Ordering ordering;
  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
 
  // Eliminate partially i.e. only continuous part.
  const auto [hybridBayesNet, remainingFactorGraph] =
      linearizedFactorGraph.eliminatePartialSequential(ordering);
 
  CHECK(hybridBayesNet);
  EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
  EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(0)});
  EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(1), M(0)}));
  EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(1)});
  EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(2), M(0), M(1)}));
  EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(2)});
  EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(0), M(1)}));
 
  CHECK(remainingFactorGraph);
  EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size());
  EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(0)}));
  EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(1), M(0)}));
  EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)}));
}
 
TEST(HybridFactorGraph, PrintErrors) {
  Switching self(3);
 
  // Get nonlinear factor graph and add linear factors to be holistic
  HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph;
  fg.add(self.linearizedFactorGraph);
 
  // Optimize to get HybridValues
  HybridBayesNet::shared_ptr bn =
      self.linearizedFactorGraph.eliminateSequential();
  HybridValues hv = bn->optimize();
 
  // Print and verify
  // fg.print();
  // std::cout << "\n\n\n" << std::endl;
  // fg.printErrors(
  //     HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint));
}
 
/****************************************************************************
 * Test full elimination
 */
TEST(HybridFactorGraph, Full_Elimination) {
  Switching self(3);
 
  auto linearizedFactorGraph = self.linearizedFactorGraph;
 
  // We first do a partial elimination
  DiscreteBayesNet discreteBayesNet;
 
  {
    // Create ordering.
    Ordering ordering;
    for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
 
    // Eliminate partially.
    const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
        linearizedFactorGraph.eliminatePartialSequential(ordering);
 
    DiscreteFactorGraph discrete_fg;
    // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
    for (auto& factor : (*remainingFactorGraph_partial)) {
      auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
      assert(df);
      discrete_fg.push_back(df);
    }
 
    ordering.clear();
    for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k));
    discreteBayesNet =
        *discrete_fg.eliminateSequential(ordering, EliminateDiscrete);
  }
 
  // Create ordering.
  Ordering ordering;
  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
  for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k));
 
  // Eliminate partially.
  HybridBayesNet::shared_ptr hybridBayesNet =
      linearizedFactorGraph.eliminateSequential(ordering);
 
  CHECK(hybridBayesNet);
  EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
  // p(x0 | x1, m0)
  EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(0)});
  EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(1), M(0)}));
  // p(x1 | x2, m0, m1)
  EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(1)});
  EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(2), M(0), M(1)}));
  // p(x2 | m0, m1)
  EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(2)});
  EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(0), M(1)}));
  // P(m0 | m1)
  EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(0)});
  EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(1)}));
  EXPECT(
      dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
          ->equals(*discreteBayesNet.at(0)));
  // P(m1)
  EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)});
  EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
  EXPECT(
      dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
          ->equals(*discreteBayesNet.at(1)));
}
 
/****************************************************************************
 * Test printing
 */
TEST(HybridFactorGraph, Printing) {
  Switching self(3);
 
  auto linearizedFactorGraph = self.linearizedFactorGraph;
 
  // Create ordering.
  Ordering ordering;
  for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
 
  // Eliminate partially.
  const auto [hybridBayesNet, remainingFactorGraph] =
      linearizedFactorGraph.eliminatePartialSequential(ordering);
 
#ifdef GTSAM_DT_MERGING
  string expected_hybridFactorGraph = R"(
size: 7
factor 0: 
  A[x0] = [
        10
]
  b = [ -10 ]
  No noise model
factor 1: 
Hybrid [x0 x1; m0]{
 Choice(m0) 
 0 Leaf :
  A[x0] = [
        -1
]
  A[x1] = [
        1
]
  b = [ -1 ]
  No noise model
 
 1 Leaf :
  A[x0] = [
        -1
]
  A[x1] = [
        1
]
  b = [ -0 ]
  No noise model
 
}
factor 2: 
Hybrid [x1 x2; m1]{
 Choice(m1) 
 0 Leaf :
  A[x1] = [
        -1
]
  A[x2] = [
        1
]
  b = [ -1 ]
  No noise model
 
 1 Leaf :
  A[x1] = [
        -1
]
  A[x2] = [
        1
]
  b = [ -0 ]
  No noise model
 
}
factor 3: 
  A[x1] = [
        10
]
  b = [ -10 ]
  No noise model
factor 4: 
  A[x2] = [
        10
]
  b = [ -10 ]
  No noise model
factor 5:  P( m0 ):
 Leaf  0.5
 
factor 6:  P( m1 | m0 ):
 Choice(m1) 
 0 Choice(m0) 
 0 0 Leaf 0.33333333
 0 1 Leaf  0.6
 1 Choice(m0) 
 1 0 Leaf 0.66666667
 1 1 Leaf  0.4
 
)";
#else
  string expected_hybridFactorGraph = R"(
size: 7
factor 0: 
  A[x0] = [
        10
]
  b = [ -10 ]
  No noise model
factor 1: 
Hybrid [x0 x1; m0]{
 Choice(m0) 
 0 Leaf:
  A[x0] = [
        -1
]
  A[x1] = [
        1
]
  b = [ -1 ]
  No noise model
 
 1 Leaf:
  A[x0] = [
        -1
]
  A[x1] = [
        1
]
  b = [ -0 ]
  No noise model
 
}
factor 2: 
Hybrid [x1 x2; m1]{
 Choice(m1) 
 0 Leaf:
  A[x1] = [
        -1
]
  A[x2] = [
        1
]
  b = [ -1 ]
  No noise model
 
 1 Leaf:
  A[x1] = [
        -1
]
  A[x2] = [
        1
]
  b = [ -0 ]
  No noise model
 
}
factor 3: 
  A[x1] = [
        10
]
  b = [ -10 ]
  No noise model
factor 4: 
  A[x2] = [
        10
]
  b = [ -10 ]
  No noise model
factor 5:  P( m0 ):
 Choice(m0) 
 0 Leaf 0.5
 1 Leaf 0.5
 
factor 6:  P( m1 | m0 ):
 Choice(m1) 
 0 Choice(m0) 
 0 0 Leaf 0.33333333
 0 1 Leaf  0.6
 1 Choice(m0) 
 1 0 Leaf 0.66666667
 1 1 Leaf  0.4
 
)";
#endif
 
  EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph));
 
  // Expected output for hybridBayesNet.
  string expected_hybridBayesNet = R"(
size: 3
conditional 0: Hybrid  P( x0 | x1 m0)
 Discrete Keys = (m0, 2), 
 Choice(m0) 
 0 Leaf p(x0 | x1)
  R = [ 10.0499 ]
  S[x1] = [ -0.0995037 ]
  d = [ -9.85087 ]
  No noise model
 
 1 Leaf p(x0 | x1)
  R = [ 10.0499 ]
  S[x1] = [ -0.0995037 ]
  d = [ -9.95037 ]
  No noise model
 
conditional 1: Hybrid  P( x1 | x2 m0 m1)
 Discrete Keys = (m0, 2), (m1, 2), 
 Choice(m1) 
 0 Choice(m0) 
 0 0 Leaf p(x1 | x2)
  R = [ 10.099 ]
  S[x2] = [ -0.0990196 ]
  d = [ -9.99901 ]
  No noise model
 
 0 1 Leaf p(x1 | x2)
  R = [ 10.099 ]
  S[x2] = [ -0.0990196 ]
  d = [ -9.90098 ]
  No noise model
 
 1 Choice(m0) 
 1 0 Leaf p(x1 | x2)
  R = [ 10.099 ]
  S[x2] = [ -0.0990196 ]
  d = [ -10.098 ]
  No noise model
 
 1 1 Leaf p(x1 | x2)
  R = [ 10.099 ]
  S[x2] = [ -0.0990196 ]
  d = [ -10 ]
  No noise model
 
conditional 2: Hybrid  P( x2 | m0 m1)
 Discrete Keys = (m0, 2), (m1, 2), 
 Choice(m1) 
 0 Choice(m0) 
 0 0 Leaf p(x2)
  R = [ 10.0494 ]
  d = [ -10.1489 ]
  mean: 1 elements
  x2: -1.0099
  No noise model
 
 0 1 Leaf p(x2)
  R = [ 10.0494 ]
  d = [ -10.1479 ]
  mean: 1 elements
  x2: -1.0098
  No noise model
 
 1 Choice(m0) 
 1 0 Leaf p(x2)
  R = [ 10.0494 ]
  d = [ -10.0504 ]
  mean: 1 elements
  x2: -1.0001
  No noise model
 
 1 1 Leaf p(x2)
  R = [ 10.0494 ]
  d = [ -10.0494 ]
  mean: 1 elements
  x2: -1
  No noise model
 
)";
  EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet));
}
 
/****************************************************************************
 * Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose
 * connects to 1 landmark) to expose issue with default decision tree creation
 * in hybrid elimination. The hybrid factor is between the poses X0 and X1.
 * The issue arises if we eliminate a landmark variable first since it is not
 * connected to a HybridFactor.
 */
TEST(HybridFactorGraph, DefaultDecisionTree) {
  HybridNonlinearFactorGraph fg;
 
  // Add a prior on pose x0 at the origin.
  // A prior factor consists of a mean and a noise model (covariance matrix)
  Pose2 prior(0.0, 0.0, 0.0);  // prior mean is at origin
  auto priorNoise = noiseModel::Diagonal::Sigmas(
      Vector3(0.3, 0.3, 0.1));  // 30cm std on x,y, 0.1 rad on theta
  fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
 
  using PlanarMotionModel = BetweenFactor<Pose2>;
 
  // Add odometry factor
  Pose2 odometry(2.0, 0.0, 0.0);
  KeyVector contKeys = {X(0), X(1)};
  auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
  auto still = std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
                                                   noise_model),
       moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
                                                    noise_model);
  std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
  fg.emplace_shared<MixtureFactor>(
      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
 
  // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
  // create a noise model for the landmark measurements
  auto measurementNoise = noiseModel::Diagonal::Sigmas(
      Vector2(0.1, 0.2));  // 0.1 rad std on bearing, 20cm on range
  // create the measurement values - indices are (pose id, landmark id)
  Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
  double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
 
  // Add Bearing-Range factors
  fg.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
      X(0), L(0), bearing11, range11, measurementNoise);
  fg.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
      X(1), L(1), bearing22, range22, measurementNoise);
 
  // Create (deliberately inaccurate) initial estimate
  Values initialEstimate;
  initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2));
  initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2));
  initialEstimate.insert(L(0), Point2(1.8, 2.1));
  initialEstimate.insert(L(1), Point2(4.1, 1.8));
 
  // We want to eliminate variables not connected to DCFactors first.
  const Ordering ordering{L(0), L(1), X(0), X(1)};
 
  HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate);
 
  // This should NOT fail
  const auto [hybridBayesNet, remainingFactorGraph] =
      linearized.eliminatePartialSequential(ordering);
  EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
  EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());
}
 
/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */