NonlinearClusterTree.h
Go to the documentation of this file.
1 
7 #pragma once
8 
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,
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 
36  return factors.linearize(values);
37  }
38 
39  static NonlinearCluster* DownCast(const std::shared_ptr<Cluster>& cluster) {
40  auto nonlinearCluster = std::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
49  const Values& values,
50  const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
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
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
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
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
130  GaussianBayesNet bayesNet;
131  for (const auto& root : roots_) {
133  bayesNet.push_back(result.first);
134  }
135  VectorValues delta = bayesNet.optimize();
136  return values.retract(delta);
137  }
138 };
139 } // namespace gtsam
const gtsam::Symbol key('X', 0)
GaussianFactorGraph::shared_ptr linearize(const Values &values)
Factor Graph consisting of non-linear factors.
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
leaf::MyValues values
const GaussianFactorGraph factors
Values updateCholesky(const Values &values)
void remove(size_t i)
Definition: FactorGraph.h:393
NonlinearFactorGraph graph
static enum @1107 ordering
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
HessianFactor::shared_ptr linearizeAndEliminate(const Values &values, GaussianBayesNet *bayesNet, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
static NonlinearCluster * DownCast(const std::shared_ptr< Cluster > &cluster)
VectorValues optimize() const
HessianFactor::shared_ptr linearizeAndEliminate(const Values &values, GaussianBayesNet *bayesNet, const Ordering &ordering, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Values result
NonlinearCluster(const VariableIndex &variableIndex, const KeyVector &keys, NonlinearFactorGraph *graph)
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
std::pair< GaussianBayesNet, HessianFactor::shared_ptr > linearizeAndEliminate(const Values &values, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
HessianFactor::shared_ptr linearizeToHessianFactor(const Values &values, const Ordering &ordering, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
HessianFactor::shared_ptr linearizeToHessianFactor(const Values &values, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
std::pair< GaussianBayesNet, HessianFactor::shared_ptr > linearizeAndEliminate(const Values &values, const HessianFactor::shared_ptr &localFactor) const
std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::pair< GaussianBayesNet, HessianFactor::shared_ptr > linearizeAndEliminate(const Values &values, const Ordering &ordering, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Collects factorgraph fragments defined on variable clusters, arranged in a tree.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:57