ISAM2-impl.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 
20 #include <gtsam/base/debug.h>
21 #include <gtsam/config.h> // for GTSAM_USE_TBB
22 #include <gtsam/inference/Symbol.h> // for selective linearization thresholds
24 
25 #include <functional>
26 #include <limits>
27 #include <string>
28 
29 using namespace std;
30 
31 namespace gtsam {
32 
33 /* ************************************************************************* */
34 namespace internal {
35 inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
37  // parents are assumed to already be solved and available in result
38  result->update(clique->conditional()->solve(*result));
39 
40  // starting from the root, call optimize on each conditional
41  for (const ISAM2::sharedClique& child : clique->children)
42  optimizeInPlace(child, result);
43 }
44 } // namespace internal
45 
46 /* ************************************************************************* */
47 size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
48  const KeySet& replacedKeys,
49  double wildfireThreshold,
51  size_t lastBacksubVariableCount;
52 
53  if (wildfireThreshold <= 0.0) {
54  // Threshold is zero or less, so do a full recalculation
55  for (const ISAM2::sharedClique& root : roots)
57  lastBacksubVariableCount = delta->size();
58 
59  } else {
60  // Optimize with wildfire
61  lastBacksubVariableCount = 0;
62  for (const ISAM2::sharedClique& root : roots)
63  lastBacksubVariableCount += optimizeWildfireNonRecursive(
64  root, wildfireThreshold, replacedKeys, delta); // modifies delta
65 
66 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
67  for (VectorValues::const_iterator key_delta = delta->begin();
68  key_delta != delta->end(); ++key_delta) {
69  assert((*delta)[key_delta->first].allFinite());
70  }
71 #endif
72  }
73 
74  return lastBacksubVariableCount;
75 }
76 
77 /* ************************************************************************* */
78 namespace internal {
79 void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
80  const VectorValues& grad, VectorValues* RgProd,
81  size_t* varsUpdated) {
82  // Check if any frontal or separator keys were recalculated, if so, we need
83  // update deltas and recurse to children, but if not, we do not need to
84  // recurse further because of the running separator property.
85  bool anyReplaced = false;
86  for (Key j : *clique->conditional()) {
87  if (replacedKeys.exists(j)) {
88  anyReplaced = true;
89  break;
90  }
91  }
92 
93  if (anyReplaced) {
94  // Update the current variable
95  // Get VectorValues slice corresponding to current variables
96  Vector gR =
97  grad.vector(KeyVector(clique->conditional()->beginFrontals(),
98  clique->conditional()->endFrontals()));
99  Vector gS =
100  grad.vector(KeyVector(clique->conditional()->beginParents(),
101  clique->conditional()->endParents()));
102 
103  // Compute R*g and S*g for this clique
104  Vector RSgProd = clique->conditional()->R() * gR +
105  clique->conditional()->S() * gS;
106 
107  // Write into RgProd vector
108  DenseIndex vectorPosition = 0;
109  for (Key frontal : clique->conditional()->frontals()) {
110  Vector& RgProdValue = (*RgProd)[frontal];
111  RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
112  vectorPosition += RgProdValue.size();
113  }
114 
115  // Now solve the part of the Newton's method point for this clique
116  // (back-substitution)
117  // (*clique)->solveInPlace(deltaNewton);
118 
119  *varsUpdated += clique->conditional()->nrFrontals();
120 
121  // Recurse to children
122  for (const ISAM2::sharedClique& child : clique->children) {
123  updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated);
124  }
125  }
126 }
127 } // namespace internal
128 
129 /* ************************************************************************* */
130 size_t DeltaImpl::UpdateRgProd(const ISAM2::Roots& roots,
131  const KeySet& replacedKeys,
132  const VectorValues& gradAtZero,
133  VectorValues* RgProd) {
134  // Update variables
135  size_t varsUpdated = 0;
136  for (const ISAM2::sharedClique& root : roots) {
137  internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd,
138  &varsUpdated);
139  }
140 
141  return varsUpdated;
142 }
143 
144 /* ************************************************************************* */
145 VectorValues DeltaImpl::ComputeGradientSearch(const VectorValues& gradAtZero,
146  const VectorValues& RgProd) {
147  // Compute gradient squared-magnitude
148  const double gradientSqNorm = gradAtZero.dot(gradAtZero);
149 
150  // Compute minimizing step size
151  double RgNormSq = RgProd.vector().squaredNorm();
152  double step = -gradientSqNorm / RgNormSq;
153 
154  // Compute steepest descent point
155  return step * gradAtZero;
156 }
157 
158 } // namespace gtsam
gtsam::FastSet::exists
bool exists(const VALUE &e) const
Definition: FastSet.h:98
ISAM2-impl.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::VectorValues::dot
double dot(const VectorValues &v) const
Definition: VectorValues.cpp:235
gtsam::FastSet< Key >
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
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::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:85
gtsam::optimizeWildfireNonRecursive
size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
Definition: ISAM2Clique.cpp:260
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::utils.visual_isam.step
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
Definition: visual_isam.py:82
Symbol.h
gtsam::BayesTree< ISAM2Clique >::Roots
FastVector< sharedClique > Roots
Definition: BayesTree.h:95
gtsam
traits
Definition: SFMdata.h:40
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
std
Definition: BFloat16.h:88
gtsam::internal::updateRgProd
void updateRgProd(const ISAM2::sharedClique &clique, const KeySet &replacedKeys, const VectorValues &grad, VectorValues *RgProd, size_t *varsUpdated)
Definition: ISAM2-impl.cpp:79
internal
Definition: BandTriangularSolver.h:13
gtsam::internal::optimizeInPlace
static void optimizeInPlace(const ISAM2::sharedClique &clique, VectorValues *result)
Definition: ISAM2-impl.cpp:35
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
debug.h
Global debugging flags.
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:52