HybridNonlinearISAM.cpp
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 
22 
23 #include <iostream>
24 
25 using namespace std;
26 
27 namespace gtsam {
28 
29 /* ************************************************************************* */
30 void HybridNonlinearISAM::saveGraph(const string& s,
31  const KeyFormatter& keyFormatter) const {
32  isam_.saveGraph(s, keyFormatter);
33 }
34 
35 /* ************************************************************************* */
37  const Values& initialValues,
38  const std::optional<size_t>& maxNrLeaves,
39  const std::optional<Ordering>& ordering) {
40  if (newFactors.size() > 0) {
41  // Reorder and relinearize every reorderInterval updates
42  if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
43  reorderRelinearize();
44  reorderCounter_ = 0;
45  }
46 
47  factors_.push_back(newFactors);
48 
49  // Linearize new factors and insert them
50  // TODO: optimize for whole config?
51  linPoint_.insert(initialValues);
52 
53  std::shared_ptr<HybridGaussianFactorGraph> linearizedNewFactors =
54  newFactors.linearize(linPoint_);
55 
56  // Update ISAM
57  isam_.update(*linearizedNewFactors, maxNrLeaves, ordering,
58  eliminationFunction_);
59  }
60 }
61 
62 /* ************************************************************************* */
63 void HybridNonlinearISAM::reorderRelinearize() {
64  if (factors_.size() > 0) {
65  // Obtain the new linearization point
66  const Values newLinPoint = estimate();
67 
68  auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete());
69 
70  isam_.clear();
71 
72  // Prune nonlinear factors based on discrete conditional probabilities
73  HybridNonlinearFactorGraph pruned_factors;
74  for (auto&& factor : factors_) {
75  if (auto nf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
76  pruned_factors.push_back(nf->prune(discreteProbs));
77  } else {
78  pruned_factors.push_back(factor);
79  }
80  }
81  factors_ = pruned_factors;
82 
83  // Just recreate the whole BayesTree
84  // TODO: allow for constrained ordering here
85  // TODO: decouple re-linearization and reordering to avoid
86  isam_.update(*factors_.linearize(newLinPoint), {}, {},
87  eliminationFunction_);
88 
89  // Update linearization point
90  linPoint_ = newLinPoint;
91  }
92 }
93 
94 /* ************************************************************************* */
95 Values HybridNonlinearISAM::estimate() {
96  Values result;
97  if (isam_.size() > 0) {
98  HybridValues values = isam_.optimize();
99  assignment_ = values.discrete();
100  return linPoint_.retract(values.continuous());
101  } else {
102  return linPoint_;
103  }
104 }
105 
106 // /* *************************************************************************
107 // */ Matrix HybridNonlinearISAM::marginalCovariance(Key key) const {
108 // return isam_.marginalCovariance(key);
109 // }
110 
111 /* ************************************************************************* */
112 void HybridNonlinearISAM::print(const string& s,
113  const KeyFormatter& keyFormatter) const {
114  cout << s << "ReorderInterval: " << reorderInterval_
115  << " Current Count: " << reorderCounter_ << endl;
116  std::cout << "HybridGaussianISAM:" << std::endl;
117  isam_.print("", keyFormatter);
118  linPoint_.print("Linearization Point:\n", keyFormatter);
119  std::cout << "Nonlinear Graph:" << std::endl;
120  factors_.print("", keyFormatter);
121 }
122 
123 /* ************************************************************************* */
124 void HybridNonlinearISAM::printStats() const {
125  isam_.getCliqueData().getStats().print();
126 }
127 
128 /* ************************************************************************* */
129 
130 } // namespace gtsam
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
relicense.update
def update(text)
Definition: relicense.py:46
gtsam::HybridValues
Definition: HybridValues.h:37
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::HybridNonlinearFactorGraph
Definition: HybridNonlinearFactorGraph.h:33
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
Ordering.h
Variable ordering for the elimination algorithm.
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
ordering
static enum @1096 ordering
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
HybridNonlinearFactor.h
A set of nonlinear factors indexed by a set of discrete keys.
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: SFMdata.h:40
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::HybridNonlinearFactorGraph::linearize
std::shared_ptr< HybridGaussianFactorGraph > linearize(const Values &continuousValues) const
Linearize all the continuous factors in the HybridNonlinearFactorGraph.
Definition: HybridNonlinearFactorGraph.cpp:139
HybridNonlinearISAM.h


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:26