21 #include <gtsam/config.h>
47 size_t DeltaImpl::UpdateGaussNewtonDelta(
const ISAM2::Roots& roots,
48 const KeySet& replacedKeys,
49 double wildfireThreshold,
51 size_t lastBacksubVariableCount;
53 if (wildfireThreshold <= 0.0) {
57 lastBacksubVariableCount =
delta->size();
61 lastBacksubVariableCount = 0;
64 root, wildfireThreshold, replacedKeys,
delta);
66 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
68 key_delta !=
delta->end(); ++key_delta) {
69 assert((*
delta)[key_delta->first].allFinite());
74 return lastBacksubVariableCount;
81 size_t* varsUpdated) {
85 bool anyReplaced =
false;
86 for (
Key j : *clique->conditional()) {
98 clique->conditional()->endFrontals()));
101 clique->conditional()->endParents()));
104 Vector RSgProd = clique->conditional()->R() * gR +
105 clique->conditional()->S() * gS;
109 for (
Key frontal : clique->conditional()->frontals()) {
110 Vector& RgProdValue = (*RgProd)[frontal];
111 RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
112 vectorPosition += RgProdValue.size();
119 *varsUpdated += clique->conditional()->nrFrontals();
123 updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated);
131 const KeySet& replacedKeys,
135 size_t varsUpdated = 0;
148 const double gradientSqNorm = gradAtZero.
dot(gradAtZero);
151 double RgNormSq = RgProd.
vector().squaredNorm();
152 double step = -gradientSqNorm / RgNormSq;
155 return step * gradAtZero;