gtsam  4.1.0
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
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 
20 #pragma once
21 
22 #include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
23 
24 #include <stdarg.h>
25 
26 namespace gtsam {
27 
28 /* ************************************************************************* */
29 namespace internal {
30 template<class BAYESTREE>
31 void 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 /* ************************************************************************* */
41 template<class BAYESTREE>
42 double 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
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28