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)
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)
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)
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
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
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
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
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
gtsam::NonlinearClusterTree::NonlinearCluster
Definition: NonlinearClusterTree.h:18
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeAndEliminate
HessianFactor::shared_ptr linearizeAndEliminate(const Values &values, GaussianBayesNet *bayesNet, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Definition: NonlinearClusterTree.h:103
gtsam::NonlinearClusterTree::NonlinearCluster::DownCast
static NonlinearCluster * DownCast(const std::shared_ptr< Cluster > &cluster)
Definition: NonlinearClusterTree.h:39
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::NonlinearClusterTree
Definition: NonlinearClusterTree.h:14
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:109
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeToHessianFactor
HessianFactor::shared_ptr linearizeToHessianFactor(const Values &values, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Definition: NonlinearClusterTree.h:48
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeAndEliminate
std::pair< GaussianBayesNet, HessianFactor::shared_ptr > linearizeAndEliminate(const Values &values, const Ordering &ordering, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Definition: NonlinearClusterTree.h:92
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeAndEliminate
std::pair< GaussianBayesNet, HessianFactor::shared_ptr > linearizeAndEliminate(const Values &values, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Definition: NonlinearClusterTree.h:82
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeToHessianFactor
HessianFactor::shared_ptr linearizeToHessianFactor(const Values &values, const Ordering &ordering, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Definition: NonlinearClusterTree.h:58
gtsam::NonlinearFactorGraph::Dampen
std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition: NonlinearFactorGraph.h:133
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::ClusterTree< NonlinearFactorGraph >::roots_
FastVector< sharedNode > roots_
Definition: ClusterTree.h:116
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::Ordering::ColamdConstrainedFirst
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Definition: inference/Ordering.h:139
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearClusterTree::NonlinearClusterTree
NonlinearClusterTree()
Definition: NonlinearClusterTree.h:16
gtsam::VariableIndex
Definition: VariableIndex.h:41
ordering
static enum @1096 ordering
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeAndEliminate
std::pair< GaussianBayesNet, HessianFactor::shared_ptr > linearizeAndEliminate(const Values &values, const HessianFactor::shared_ptr &localFactor) const
Definition: NonlinearClusterTree.h:66
key
const gtsam::Symbol key('X', 0)
gtsam::NonlinearClusterTree::NonlinearCluster::linearize
GaussianFactorGraph::shared_ptr linearize(const Values &values)
Definition: NonlinearClusterTree.h:35
gtsam::NonlinearClusterTree::updateCholesky
Values updateCholesky(const Values &values)
Definition: NonlinearClusterTree.h:129
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::ClusterTree
Definition: ClusterTree.h:25
gtsam::NonlinearClusterTree::NonlinearCluster::linearizeAndEliminate
HessianFactor::shared_ptr linearizeAndEliminate(const Values &values, GaussianBayesNet *bayesNet, const Ordering &ordering, const NonlinearFactorGraph::Dampen &dampen=nullptr) const
Definition: NonlinearClusterTree.h:116
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::NonlinearClusterTree::NonlinearCluster::NonlinearCluster
NonlinearCluster(const VariableIndex &variableIndex, const KeyVector &keys, NonlinearFactorGraph *graph)
Definition: NonlinearClusterTree.h:22
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:50