gtsam  4.1.0
gtsam
NonlinearClusterTree.h
Go to the documentation of this file.
1 
7 #pragma once
8 
11 #include <gtsam/inference/ClusterTree.h>
12 
13 namespace gtsam {
14 class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
15  public:
17 
18  struct NonlinearCluster : Cluster {
19  // Given graph, index, add factors with specified keys into
20  // Factors are erased in the graph
21  // TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead
22  NonlinearCluster(const VariableIndex& variableIndex, const KeyVector& keys,
23  NonlinearFactorGraph* graph) {
24  for (const Key key : keys) {
25  std::vector<NonlinearFactor::shared_ptr> factors;
26  for (auto i : variableIndex[key])
27  if (graph->at(i)) {
28  factors.push_back(graph->at(i));
29  graph->remove(i);
30  }
31  Cluster::addFactors(key, factors);
32  }
33  }
34 
35  GaussianFactorGraph::shared_ptr linearize(const Values& values) {
36  return factors.linearize(values);
37  }
38 
39  static NonlinearCluster* DownCast(const boost::shared_ptr<Cluster>& cluster) {
40  auto nonlinearCluster = boost::dynamic_pointer_cast<NonlinearCluster>(cluster);
41  if (!nonlinearCluster)
42  throw std::runtime_error("Expected NonlinearCluster");
43  return nonlinearCluster.get();
44  }
45 
46  // linearize local custer factors straight into hessianFactor, which is returned
47  // If no ordering given, uses colamd
48  HessianFactor::shared_ptr linearizeToHessianFactor(
49  const Values& values,
50  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
51  Ordering ordering;
52  ordering = Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true);
53  return factors.linearizeToHessianFactor(values, ordering, dampen);
54  }
55 
56  // linearize local custer factors straight into hessianFactor, which is returned
57  // If no ordering given, uses colamd
58  HessianFactor::shared_ptr linearizeToHessianFactor(
59  const Values& values, const Ordering& ordering,
60  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
61  return factors.linearizeToHessianFactor(values, ordering, dampen);
62  }
63 
64  // Helper function: recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
65  // TODO(frank): Use TBB to support deep trees and parallelism
66  std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
67  const Values& values,
68  const HessianFactor::shared_ptr& localFactor) const {
69  // Get contributions f(front) from children, as well as p(children|front)
70  GaussianBayesNet bayesNet;
71  for (const auto& child : children) {
72  auto message = DownCast(child)->linearizeAndEliminate(values, &bayesNet);
73  message->updateHessian(localFactor.get());
74  }
75  auto gaussianConditional = localFactor->eliminateCholesky(orderedFrontalKeys);
76  bayesNet.add(gaussianConditional);
77  return {bayesNet, localFactor};
78  }
79 
80  // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
81  // TODO(frank): Use TBB to support deep trees and parallelism
82  std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
83  const Values& values,
84  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
85  // Linearize and create HessianFactor f(front,separator)
86  HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, dampen);
87  return linearizeAndEliminate(values, localFactor);
88  }
89 
90  // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
91  // TODO(frank): Use TBB to support deep trees and parallelism
92  std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
93  const Values& values, const Ordering& ordering,
94  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
95  // Linearize and create HessianFactor f(front,separator)
96  HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen);
97  return linearizeAndEliminate(values, localFactor);
98  }
99 
100  // Recursively eliminate subtree rooted at this Cluster
101  // Version that updates existing Bayes net and returns a new Hessian factor on parent clique
102  // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor
103  HessianFactor::shared_ptr linearizeAndEliminate(
104  const Values& values, GaussianBayesNet* bayesNet,
105  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
106  auto bayesNet_newFactor_pair = linearizeAndEliminate(values, dampen);
107  if (bayesNet) {
108  bayesNet->push_back(bayesNet_newFactor_pair.first);
109  }
110  return bayesNet_newFactor_pair.second;
111  }
112 
113  // Recursively eliminate subtree rooted at this Cluster
114  // Version that updates existing Bayes net and returns a new Hessian factor on parent clique
115  // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor
116  HessianFactor::shared_ptr linearizeAndEliminate(
117  const Values& values, GaussianBayesNet* bayesNet,
118  const Ordering& ordering,
119  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
120  auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen);
121  if (bayesNet) {
122  bayesNet->push_back(bayesNet_newFactor_pair.first);
123  }
124  return bayesNet_newFactor_pair.second;
125  }
126  };
127 
128  // Linearize and update linearization point with values
129  Values updateCholesky(const Values& values) {
130  GaussianBayesNet bayesNet;
131  for (const auto& root : roots_) {
132  auto result = NonlinearCluster::DownCast(root)->linearizeAndEliminate(values);
133  bayesNet.push_back(result.first);
134  }
135  VectorValues delta = bayesNet.optimize();
136  return values.retract(delta);
137  }
138 };
139 } // namespace gtsam
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::Ordering
Definition: Ordering.h:34
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
gtsam::ClusterTree
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition: ClusterTree.h:25
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:162
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::GaussianBayesNet::optimize
VectorValues optimize() const
Solve the GaussianBayesNet, i.e. return , by back-substitution.
Definition: GaussianBayesNet.cpp:40
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Ordering::ColamdConstrainedFirst
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition: Ordering.h:128
gtsam::NonlinearFactorGraph::Dampen
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition: NonlinearFactorGraph.h:149
gtsam::GaussianBayesNet
A Bayes net made from linear-Gaussian densities.
Definition: GaussianBayesNet.h:31
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:109
gtsam::NonlinearClusterTree::NonlinearCluster
Definition: NonlinearClusterTree.h:18
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:186
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::NonlinearClusterTree
Definition: NonlinearClusterTree.h:14
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:74
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43