testHybridNonlinearFactor.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/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
 
// Include for test suite
#include <CppUnitLite/TestHarness.h>
 
using namespace std;
using namespace gtsam;
using noiseModel::Isotropic;
using symbol_shorthand::M;
using symbol_shorthand::X;
 
/* ************************************************************************* */
// Check iterators of empty hybrid factor.
TEST(HybridNonlinearFactor, Constructor) {
  HybridNonlinearFactor factor;
  HybridNonlinearFactor::const_iterator const_it = factor.begin();
  CHECK(const_it == factor.end());
  HybridNonlinearFactor::iterator it = factor.begin();
  CHECK(it == factor.end());
}
/* ************************************************************************* */
namespace test_constructor {
DiscreteKey m1(1, 2);
double between0 = 0.0;
double between1 = 1.0;
 
Vector1 sigmas = Vector1(1.0);
auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
 
auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
}  // namespace test_constructor
 
/* ************************************************************************* */
// Test simple to complex constructors...
TEST(HybridGaussianFactor, ConstructorVariants) {
  using namespace test_constructor;
  HybridNonlinearFactor fromFactors(m1, {f0, f1});
 
  std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
  HybridNonlinearFactor fromPairs(m1, pairs);
  assert_equal(fromFactors, fromPairs);
 
  HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
  HybridNonlinearFactor fromDecisionTree({m1}, decisionTree);
  assert_equal(fromDecisionTree, fromPairs);
}
 
/* ************************************************************************* */
// Test .print() output.
TEST(HybridNonlinearFactor, Printing) {
  using namespace test_constructor;
  HybridNonlinearFactor hybridFactor({m1}, {f0, f1});
 
  std::string expected =
      R"(Hybrid [x1 x2; 1]
HybridNonlinearFactor
 Choice(1) 
 0 Leaf BetweenFactor(x1,x2)
  measured:  0
  noise model: diagonal sigmas [1];
 
 1 Leaf BetweenFactor(x1,x2)
  measured:  1
  noise model: diagonal sigmas [1];
 
)";
  EXPECT(assert_print_equal(expected, hybridFactor));
}
 
/* ************************************************************************* */
static HybridNonlinearFactor getHybridNonlinearFactor() {
  DiscreteKey m1(1, 2);
 
  double between0 = 0.0;
  double between1 = 1.0;
 
  Vector1 sigmas = Vector1(1.0);
  auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
 
  auto f0 =
      std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
  auto f1 =
      std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
  return HybridNonlinearFactor(m1, {f0, f1});
}
 
/* ************************************************************************* */
// Test the error of the HybridNonlinearFactor
TEST(HybridNonlinearFactor, Error) {
  auto hybridFactor = getHybridNonlinearFactor();
 
  Values continuousValues;
  continuousValues.insert<double>(X(1), 0);
  continuousValues.insert<double>(X(2), 1);
 
  AlgebraicDecisionTree<Key> error_tree =
      hybridFactor.errorTree(continuousValues);
 
  DiscreteKey m1(1, 2);
  std::vector<DiscreteKey> discrete_keys = {m1};
  std::vector<double> errors = {0.5, 0};
  AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
 
  EXPECT(assert_equal(expected_error, error_tree));
}
 
/* ************************************************************************* */
// Test dim of the HybridNonlinearFactor
TEST(HybridNonlinearFactor, Dim) {
  auto hybridFactor = getHybridNonlinearFactor();
  EXPECT_LONGS_EQUAL(1, hybridFactor.dim());
}
 
/* ************************************************************************* */
// Test restrict method
TEST(HybridNonlinearFactor, Restrict) {
  using namespace test_constructor;
  HybridNonlinearFactor factor(m1, {f0, f1});
  DiscreteValues assignment = {{m1.first, 0}};
  auto restricted = factor.restrict(assignment);
  auto betweenFactor = dynamic_pointer_cast<BetweenFactor<double>>(restricted);
  CHECK(betweenFactor);
  EXPECT(assert_equal(*f0, *betweenFactor));
}
 
/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */