testHybridGaussianConditional.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 
27 #include <gtsam/inference/Symbol.h>
29 
30 #include <memory>
31 #include <vector>
32 
33 // Include for test suite
35 
36 using namespace gtsam;
40 
41 // Common constants
42 static const Key modeKey = M(0);
43 static const DiscreteKey mode(modeKey, 2);
44 static const VectorValues vv{{Z(0), Vector1(4.9)}, {X(0), Vector1(5.0)}};
45 static const DiscreteValues assignment0{{M(0), 0}}, assignment1{{M(0), 1}};
46 static const HybridValues hv0{vv, assignment0};
47 static const HybridValues hv1{vv, assignment1};
48 
49 /* ************************************************************************* */
50 namespace equal_constants {
51 // Create a simple HybridGaussianConditional
52 const double commonSigma = 2.0;
53 const std::vector<GaussianConditional::shared_ptr> conditionals{
55  commonSigma),
57  commonSigma)};
59 } // namespace equal_constants
60 
61 /* ************************************************************************* */
64  using namespace equal_constants;
65 
66  // Check that the conditional (negative log) normalization constant is the min
67  // of all constants which are all equal, in this case, hence:
68  const double K = hybrid_conditional.negLogConstant();
69  EXPECT_DOUBLES_EQUAL(K, conditionals[0]->negLogConstant(), 1e-8);
70  EXPECT_DOUBLES_EQUAL(K, conditionals[1]->negLogConstant(), 1e-8);
71 
74 }
75 
76 /* ************************************************************************* */
78 TEST(HybridGaussianConditional, LogProbability) {
79  using namespace equal_constants;
80  for (size_t mode : {0, 1}) {
81  const HybridValues hv{vv, {{M(0), mode}}};
82  EXPECT_DOUBLES_EQUAL(conditionals[mode]->logProbability(vv),
84  }
85 }
86 
87 /* ************************************************************************* */
90  using namespace equal_constants;
91  auto actual = hybrid_conditional.errorTree(vv);
92 
93  // Check result.
94  DiscreteKeys discrete_keys{mode};
95  std::vector<double> leaves = {conditionals[0]->error(vv),
96  conditionals[1]->error(vv)};
97  AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
98 
99  EXPECT(assert_equal(expected, actual, 1e-6));
100 
101  // Check for non-tree version.
102  for (size_t mode : {0, 1}) {
103  const HybridValues hv{vv, {{M(0), mode}}};
105  hybrid_conditional.error(hv), 1e-8);
106  }
107 }
108 
109 /* ************************************************************************* */
113  using namespace equal_constants;
114 
115  // Compute likelihood
116  auto likelihood = hybrid_conditional.likelihood(vv);
117 
118  // Check that the hybrid conditional error and the likelihood error are the
119  // same.
120  EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0),
121  1e-8);
122  EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
123  1e-8);
124 
125  // Check that likelihood error is as expected, i.e., just the errors of the
126  // individual likelihoods, in the `equal_constants` case.
127  std::vector<DiscreteKey> discrete_keys = {mode};
128  std::vector<double> leaves = {conditionals[0]->likelihood(vv)->error(vv),
129  conditionals[1]->likelihood(vv)->error(vv)};
130  AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
131  EXPECT(assert_equal(expected, likelihood->errorTree(vv), 1e-6));
132 
133  // Check that the ratio of probPrime to evaluate is the same for all modes.
134  std::vector<double> ratio(2);
135  for (size_t mode : {0, 1}) {
136  const HybridValues hv{vv, {{M(0), mode}}};
137  ratio[mode] =
138  std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
139  }
140  EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
141 }
142 
143 /* ************************************************************************* */
145 // Create a HybridGaussianConditional with mode-dependent noise models.
146 // 0 is low-noise, 1 is high-noise.
147 const std::vector<GaussianConditional::shared_ptr> conditionals{
149  0.5),
151  3.0)};
153 } // namespace mode_dependent_constants
154 
155 /* ************************************************************************* */
156 // Create a test for continuousParents.
157 TEST(HybridGaussianConditional, ContinuousParents) {
158  using namespace mode_dependent_constants;
159  const KeyVector continuousParentKeys = hybrid_conditional.continuousParents();
160  // Check that the continuous parent keys are correct:
161  EXPECT(continuousParentKeys.size() == 1);
162  EXPECT(continuousParentKeys[0] == X(0));
163 
166 }
167 
168 /* ************************************************************************* */
171  using namespace mode_dependent_constants;
172  auto actual = hybrid_conditional.errorTree(vv);
173 
174  // Check result.
175  DiscreteKeys discrete_keys{mode};
176  double negLogConstant0 = conditionals[0]->negLogConstant();
177  double negLogConstant1 = conditionals[1]->negLogConstant();
178  double minErrorConstant = std::min(negLogConstant0, negLogConstant1);
179 
180  // Expected error is e(X) + log(sqrt(|2πΣ|)).
181  // We normalize log(sqrt(|2πΣ|)) with min(negLogConstant)
182  // so it is non-negative.
183  std::vector<double> leaves = {
184  conditionals[0]->error(vv) + negLogConstant0 - minErrorConstant,
185  conditionals[1]->error(vv) + negLogConstant1 - minErrorConstant};
186  AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
187 
188  EXPECT(assert_equal(expected, actual, 1e-6));
189 
190  // Check for non-tree version.
191  for (size_t mode : {0, 1}) {
192  const HybridValues hv{vv, {{M(0), mode}}};
194  conditionals[mode]->negLogConstant() -
195  minErrorConstant,
196  hybrid_conditional.error(hv), 1e-8);
197  }
198 }
199 
200 /* ************************************************************************* */
204  using namespace mode_dependent_constants;
205 
206  // Compute likelihood
207  auto likelihood = hybrid_conditional.likelihood(vv);
208 
209  // Check that the hybrid conditional error and the likelihood error are as
210  // expected, this invariant is the same as the equal noise case:
211  EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0),
212  1e-8);
213  EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
214  1e-8);
215 
216  // Check the detailed JacobianFactor calculation for mode==1.
217  {
218  // We have a JacobianFactor
219  const auto [gf1, _] = (*likelihood)(assignment1);
220  const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1);
221  CHECK(jf1);
222 
223  // Check that the JacobianFactor error with constants is equal to the
224  // conditional error:
226  jf1->error(hv1) + conditionals[1]->negLogConstant() -
228  1e-8);
229  }
230 
231  // Check that the ratio of probPrime to evaluate is the same for all modes.
232  std::vector<double> ratio(2);
233  for (size_t mode : {0, 1}) {
234  const HybridValues hv{vv, {{M(0), mode}}};
235  ratio[mode] =
236  std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
237  }
238  EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
239 }
240 
241 /* ************************************************************************* */
242 // Test pruning a HybridGaussianConditional with two discrete keys, based on a
243 // DecisionTreeFactor with 3 keys:
245  // Create a two key conditional:
246  DiscreteKeys modes{{M(1), 2}, {M(2), 2}};
247  std::vector<GaussianConditional::shared_ptr> gcs;
248  for (size_t i = 0; i < 4; i++) {
249  gcs.push_back(
251  }
252  auto empty = std::make_shared<GaussianConditional>();
255 
256  DiscreteKeys keys = modes;
257  keys.push_back({M(3), 2});
258  {
259  for (size_t i = 0; i < 8; i++) {
260  std::vector<double> potentials{0, 0, 0, 0, 0, 0, 0, 0};
261  potentials[i] = 1;
262  const DecisionTreeFactor decisionTreeFactor(keys, potentials);
263  // Prune the HybridGaussianConditional
264  const auto pruned = hgc.prune(decisionTreeFactor);
265  // Check that the pruned HybridGaussianConditional has 1 conditional
266  EXPECT_LONGS_EQUAL(1, pruned->nrComponents());
267  }
268  }
269  {
270  const std::vector<double> potentials{0, 0, 0.5, 0, //
271  0, 0, 0.5, 0};
272  const DecisionTreeFactor decisionTreeFactor(keys, potentials);
273 
274  const auto pruned = hgc.prune(decisionTreeFactor);
275 
276  // Check that the pruned HybridGaussianConditional has 2 conditionals
277  EXPECT_LONGS_EQUAL(2, pruned->nrComponents());
278 
279  // Check that the minimum negLogConstant is set correctly
281  hgc.conditionals()({{M(1), 0}, {M(2), 1}})->negLogConstant(),
282  pruned->negLogConstant(), 1e-9);
283  }
284  {
285  const std::vector<double> potentials{0.2, 0, 0.3, 0, //
286  0, 0, 0.5, 0};
287  const DecisionTreeFactor decisionTreeFactor(keys, potentials);
288 
289  const auto pruned = hgc.prune(decisionTreeFactor);
290 
291  // Check that the pruned HybridGaussianConditional has 3 conditionals
292  EXPECT_LONGS_EQUAL(3, pruned->nrComponents());
293 
294  // Check that the minimum negLogConstant is correct
295  EXPECT_DOUBLES_EQUAL(hgc.negLogConstant(), pruned->negLogConstant(), 1e-9);
296  }
297 }
298 
299 /* *************************************************************************
300  */
301 int main() {
302  TestResult tr;
303  return TestRegistry::runAllTests(tr);
304 }
305 /* *************************************************************************
306  */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: Vector.h:41
gtsam::DecisionTreeFactor
Definition: DecisionTreeFactor.h:44
gtsam::HybridValues
Definition: HybridValues.h:37
GaussianConditional.h
Conditional Gaussian Base class.
equal_constants::commonSigma
const double commonSigma
Definition: testHybridGaussianConditional.cpp:52
HybridGaussianConditional.h
A hybrid conditional in the Conditional Linear Gaussian scheme.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::HybridGaussianFactor::errorTree
AlgebraicDecisionTree< Key > errorTree(const VectorValues &continuousValues) const override
Compute error of the HybridGaussianFactor as a tree.
Definition: HybridGaussianFactor.cpp:186
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::symbol_shorthand::M
Key M(std::uint64_t j)
Definition: inference/Symbol.h:160
mode_dependent_constants::hybrid_conditional
const HybridGaussianConditional hybrid_conditional(mode, conditionals)
assignment0
static const DiscreteValues assignment0
Definition: testHybridGaussianConditional.cpp:45
X
#define X
Definition: icosphere.cpp:20
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41
vv
static const VectorValues vv
Definition: testHybridGaussianConditional.cpp:44
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
modeKey
static const Key modeKey
Definition: testHybridGaussianConditional.cpp:42
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
HybridGaussianFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
assignment1
static const DiscreteValues assignment1
Definition: testHybridGaussianConditional.cpp:45
gtsam::AlgebraicDecisionTree< Key >
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
gtsam::HybridGaussianConditional::evaluate
double evaluate(const HybridValues &values) const override
Calculate probability density for given values.
Definition: HybridGaussianConditional.cpp:346
mode_dependent_constants
Definition: testHybridGaussianConditional.cpp:144
gtsam::HybridGaussianConditional::logProbability
double logProbability(const HybridValues &values) const override
Compute the logProbability of this hybrid Gaussian conditional.
Definition: HybridGaussianConditional.cpp:338
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::symbol_shorthand::Z
Key Z(std::uint64_t j)
Definition: inference/Symbol.h:173
mode
static const DiscreteKey mode(modeKey, 2)
DecisionTree.h
Decision Tree for use in DiscreteFactors.
gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional >::CheckInvariants
static bool CheckInvariants(const HybridGaussianConditional &conditional, const VALUES &x)
Definition: Conditional-inst.h:69
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::HybridGaussianConditional
A conditional of gaussian conditionals indexed by discrete variables, as part of a Bayes Network....
Definition: HybridGaussianConditional.h:54
Symbol.h
gtsam::HybridGaussianConditional::continuousParents
KeyVector continuousParents() const
Returns the continuous keys among the parents.
Definition: HybridGaussianConditional.cpp:244
gtsam::HybridGaussianConditional::likelihood
std::shared_ptr< HybridGaussianFactor > likelihood(const VectorValues &given) const
Definition: HybridGaussianConditional.cpp:271
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
DiscreteKey.h
specialized key for discrete variables
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition: DecisionTree.h:62
empty
Definition: test_copy_move.cpp:19
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
DiscreteValues.h
error
static double error
Definition: testRot3.cpp:37
K
#define K
Definition: igam.h:8
gtsam::DiscreteValues
Definition: DiscreteValues.h:34
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
main
int main()
Definition: testHybridGaussianConditional.cpp:301
equal_constants
Definition: testHybridGaussianConditional.cpp:50
ratio
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
Definition: gnuplot_common_settings.hh:44
mode_dependent_constants::conditionals
const std::vector< GaussianConditional::shared_ptr > conditionals
Definition: testHybridGaussianConditional.cpp:147
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::HybridGaussianConditional::negLogConstant
double negLogConstant() const override
Return log normalization constant in negative log space.
Definition: HybridGaussianConditional.h:197
hv0
static const HybridValues hv0
Definition: testHybridGaussianConditional.cpp:46
gtsam::HybridGaussianFactor::error
double error(const HybridValues &values) const override
Compute the log-likelihood, including the log-normalizing constant.
Definition: HybridGaussianFactor.cpp:196
different_sigmas::hgc
const auto hgc
Definition: testHybridBayesNet.cpp:236
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
hv1
static const HybridValues hv1
Definition: testHybridGaussianConditional.cpp:47
_
constexpr descr< N - 1 > _(char const (&text)[N])
Definition: descr.h:109
Z
#define Z
Definition: icosphere.cpp:21
HybridValues.h
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::GaussianConditional::sharedMeanAndStddev
static shared_ptr sharedMeanAndStddev(Args &&... args)
Create shared pointer by forwarding arguments to fromMeanAndStddev.
Definition: GaussianConditional.h:105
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:38