gtsam 4.1.1 gtsam
NonlinearClusterTree.h
Go to the documentation of this file.
1
7#pragma once
8
11#include <gtsam/inference/ClusterTree.h>
12
13namespace gtsam {
14class 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 }
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);
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
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:165
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:189
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition: ClusterTree.h:25
FastVector< sharedNode > roots_
concept check
Definition: ClusterTree.h:116
Definition: Ordering.h:34
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
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
A Bayes net made from linear-Gaussian densities.
Definition: GaussianBayesNet.h:31
VectorValues optimize() const
Solve the GaussianBayesNet, i.e. return , by back-substitution.
Definition: GaussianBayesNet.cpp:41
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:75
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition: NonlinearFactorGraph.h:163
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:101
Definition: NonlinearClusterTree.h:14
Definition: NonlinearClusterTree.h:18