21 #include <gtsam/config.h>
48 size_t DeltaImpl::UpdateGaussNewtonDelta(
const ISAM2::Roots& roots,
49 const KeySet& replacedKeys,
50 double wildfireThreshold,
52 size_t lastBacksubVariableCount;
54 if (wildfireThreshold <= 0.0) {
58 lastBacksubVariableCount =
delta->size();
62 lastBacksubVariableCount = 0;
65 root, wildfireThreshold, replacedKeys,
delta);
67 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
69 key_delta !=
delta->end(); ++key_delta) {
70 assert((*
delta)[key_delta->first].allFinite());
75 return lastBacksubVariableCount;
82 size_t* varsUpdated) {
86 bool anyReplaced =
false;
87 for (
Key j : *clique->conditional()) {
99 clique->conditional()->endFrontals()));
102 clique->conditional()->endParents()));
105 Vector RSgProd = clique->conditional()->R() * gR +
106 clique->conditional()->S() * gS;
110 for (
Key frontal : clique->conditional()->frontals()) {
111 Vector& RgProdValue = (*RgProd)[frontal];
112 RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
113 vectorPosition += RgProdValue.size();
120 *varsUpdated += clique->conditional()->nrFrontals();
124 updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated);
132 const KeySet& replacedKeys,
136 size_t varsUpdated = 0;
149 const double gradientSqNorm = gradAtZero.
dot(gradAtZero);
152 double RgNormSq = RgProd.
vector().squaredNorm();
153 double step = -gradientSqNorm / RgNormSq;
156 return step * gradAtZero;