gtsam 4.1.1 gtsam
GaussianBayesTree-inl.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
20#pragma once
21
22#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
23
24#include <stdarg.h>
25
26namespace gtsam {
27
28/* ************************************************************************* */
29namespace internal {
30template<class BAYESTREE>
31void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) {
32 // parents are assumed to already be solved and available in result
33 clique->conditional()->solveInPlace(result);
34
35 // starting from the root, call optimize on each conditional
36 for(const typename BAYESTREE::sharedClique& child: clique->children_)
37 optimizeInPlace<BAYESTREE>(child, result);
38}
39
40/* ************************************************************************* */
41template<class BAYESTREE>
42double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
43 double result = 0.0;
44
45 // this clique
46 result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
47
48 // sum of children
49 for(const typename BAYESTREE::sharedClique& child: clique->children_)
50 result += logDeterminant<BAYESTREE>(child);
51
52 return result;
53}
54
55/* ************************************************************************* */
56} // \namespace internal
57} // \namespace gtsam
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28