testHybridNonlinearFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
25 #include <gtsam/inference/Symbol.h>
27 
28 // Include for test suite
30 
31 using namespace std;
32 using namespace gtsam;
36 
37 /* ************************************************************************* */
38 // Check iterators of empty hybrid factor.
39 TEST(HybridNonlinearFactor, Constructor) {
40  HybridNonlinearFactor factor;
41  HybridNonlinearFactor::const_iterator const_it = factor.begin();
42  CHECK(const_it == factor.end());
43  HybridNonlinearFactor::iterator it = factor.begin();
44  CHECK(it == factor.end());
45 }
46 /* ************************************************************************* */
47 namespace test_constructor {
48 DiscreteKey m1(1, 2);
49 double between0 = 0.0;
50 double between1 = 1.0;
51 
53 auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
54 
55 auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
56 auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
57 } // namespace test_constructor
58 
59 /* ************************************************************************* */
60 // Test simple to complex constructors...
61 TEST(HybridGaussianFactor, ConstructorVariants) {
62  using namespace test_constructor;
63  HybridNonlinearFactor fromFactors(m1, {f0, f1});
64 
65  std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
66  HybridNonlinearFactor fromPairs(m1, pairs);
67  assert_equal(fromFactors, fromPairs);
68 
69  HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
70  HybridNonlinearFactor fromDecisionTree({m1}, decisionTree);
71  assert_equal(fromDecisionTree, fromPairs);
72 }
73 
74 /* ************************************************************************* */
75 // Test .print() output.
77  using namespace test_constructor;
78  HybridNonlinearFactor hybridFactor({m1}, {f0, f1});
79 
80  std::string expected =
81  R"(Hybrid [x1 x2; 1]
82 HybridNonlinearFactor
83  Choice(1)
84  0 Leaf Nonlinear factor on 2 keys
85  1 Leaf Nonlinear factor on 2 keys
86 )";
87  EXPECT(assert_print_equal(expected, hybridFactor));
88 }
89 
90 /* ************************************************************************* */
92  DiscreteKey m1(1, 2);
93 
94  double between0 = 0.0;
95  double between1 = 1.0;
96 
97  Vector1 sigmas = Vector1(1.0);
98  auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
99 
100  auto f0 =
101  std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
102  auto f1 =
103  std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
105 }
106 
107 /* ************************************************************************* */
108 // Test the error of the HybridNonlinearFactor
109 TEST(HybridNonlinearFactor, Error) {
110  auto hybridFactor = getHybridNonlinearFactor();
111 
112  Values continuousValues;
113  continuousValues.insert<double>(X(1), 0);
114  continuousValues.insert<double>(X(2), 1);
115 
116  AlgebraicDecisionTree<Key> error_tree =
117  hybridFactor.errorTree(continuousValues);
118 
119  DiscreteKey m1(1, 2);
120  std::vector<DiscreteKey> discrete_keys = {m1};
121  std::vector<double> errors = {0.5, 0};
122  AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
123 
124  EXPECT(assert_equal(expected_error, error_tree));
125 }
126 
127 /* ************************************************************************* */
128 // Test dim of the HybridNonlinearFactor
130  auto hybridFactor = getHybridNonlinearFactor();
131  EXPECT_LONGS_EQUAL(1, hybridFactor.dim());
132 }
133 
134 /* ************************************************************************* */
135 int main() {
136  TestResult tr;
137  return TestRegistry::runAllTests(tr);
138 }
139 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
main
int main()
Definition: testHybridNonlinearFactor.cpp:130
test_constructor::between1
double between1
Definition: testHybridNonlinearFactor.cpp:50
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
HybridNonlinearFactorGraph.h
Nonlinear hybrid factor graph that uses type erasure.
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
TestHarness.h
test_constructor
Definition: testHybridGaussianFactor.cpp:58
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
m1
Matrix3d m1
Definition: IOFormat.cpp:2
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
X
#define X
Definition: icosphere.cpp:20
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::AlgebraicDecisionTree< Key >
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
test_constructor::f0
auto f0
Definition: testHybridNonlinearFactor.cpp:55
BetweenFactor.h
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
Symbol.h
test_constructor::model
auto model
Definition: testHybridNonlinearFactor.cpp:53
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
TEST
TEST(HybridNonlinearFactor, Constructor)
Definition: testHybridNonlinearFactor.cpp:39
TestResult
Definition: TestResult.h:26
gtsam::DecisionTree< Key, NonlinearFactorValuePair >
test_constructor::between0
double between0
Definition: testHybridNonlinearFactor.cpp:49
HybridNonlinearFactor.h
A set of nonlinear factors indexed by a set of discrete keys.
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: SFMdata.h:40
DiscreteValues.h
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
std
Definition: BFloat16.h:88
gtsam::HybridGaussianFactor
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
Definition: HybridGaussianFactor.h:60
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::HybridNonlinearFactor
Implementation of a discrete-conditioned hybrid factor.
Definition: HybridNonlinearFactor.h:58
getHybridNonlinearFactor
static HybridNonlinearFactor getHybridNonlinearFactor()
Definition: testHybridNonlinearFactor.cpp:86
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:14