31 template class BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph>;
34 void ISAM2Clique::setEliminationResult(
36 conditional_ = eliminationResult.first;
37 cachedFactor_ = eliminationResult.second;
39 gradientContribution_.resize(conditional_->cols() - 1);
42 gradientContribution_ << -conditional_->R().transpose() *
44 -conditional_->S().transpose() * conditional_->d();
49 return Base::equals(
other) &&
50 ((!cachedFactor_ && !
other.cachedFactor_) ||
51 (cachedFactor_ &&
other.cachedFactor_ &&
52 cachedFactor_->equals(*
other.cachedFactor_,
tol)));
59 cachedFactor_->print(
s +
"Cached: ",
formatter);
61 cout <<
s <<
"Cached empty" << endl;
62 if (gradientContribution_.rows() != 0)
63 gtsam::print(gradientContribution_,
"Gradient contribution: ");
67 bool ISAM2Clique::isDirty(
const KeySet& replaced,
const KeySet& changed)
const {
73 bool dirty = replaced.
exists(conditional_->frontals().front());
74 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
75 for (
Key frontal : conditional_->frontals()) {
76 assert(dirty == replaced.
exists(frontal));
82 for (
Key parent : conditional_->parents()) {
83 if (changed.
exists(parent)) {
98 #ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
106 if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) {
107 for (
Key frontal : conditional_->frontals())
108 solnPointers_.emplace(frontal,
delta->find(frontal));
109 for (
Key parentKey : conditional_->parents()) {
110 assert(parent->solnPointers_.exists(parentKey));
111 solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey));
117 if (!solnPointers_.empty()) {
125 parentPointers.reserve(conditional_->nrParents());
126 for (
Key parent : conditional_->parents()) {
127 parentPointers.push_back(solnPointers_.at(parent));
128 dim += parentPointers.back()->second.size();
135 const Vector& parentVector = parentPointer->second;
136 xS.block(vectorPos, 0, parentVector.size(), 1) =
137 parentVector.block(0, 0, parentVector.size(), 1);
138 vectorPos += parentVector.size();
146 const Vector rhs =
c.getb() -
c.S() * xS;
150 if (solution.hasNaN())
156 frontal !=
c.endFrontals(); ++frontal) {
157 solnPointers_.at(*frontal)->second =
158 solution.segment(vectorPosition,
c.getDim(frontal));
159 vectorPosition +=
c.getDim(frontal);
171 bool ISAM2Clique::valuesChanged(
const KeySet& replaced,
172 const Vector& originalValues,
174 double threshold)
const {
175 auto frontals = conditional_->frontals();
176 if (replaced.
exists(frontals.front()))
return true;
177 Vector diff = originalValues -
delta.vector(frontals);
183 void ISAM2Clique::markFrontalsAsChanged(
KeySet* changed)
const {
184 for (
Key frontal : conditional_->frontals()) {
185 changed->insert(frontal);
190 void ISAM2Clique::restoreFromOriginals(
const Vector& originalValues,
193 for (
Key frontal : conditional_->frontals()) {
194 auto v =
delta->at(frontal);
195 v = originalValues.segment(
pos,
v.size());
204 size_t* count)
const {
205 if (isDirty(replaced, *changed)) {
207 auto originalValues =
delta->vector(conditional_->frontals());
210 fastBackSubstitute(
delta);
211 count += conditional_->nrFrontals();
213 if (valuesChanged(replaced, originalValues, *
delta, threshold)) {
214 markFrontalsAsChanged(changed);
216 restoreFromOriginals(originalValues,
delta);
220 for (
const auto& child : children) {
221 child->optimizeWildfire(replaced, threshold, changed,
delta, count);
231 if (root) root->optimizeWildfire(
keys, threshold, &changed,
delta, &count);
236 bool ISAM2Clique::optimizeWildfireNode(
const KeySet& replaced,
double threshold,
238 size_t* count)
const {
241 bool dirty = isDirty(replaced, *changed);
244 auto originalValues =
delta->vector(conditional_->frontals());
247 fastBackSubstitute(
delta);
248 *count += conditional_->nrFrontals();
250 if (valuesChanged(replaced, originalValues, *
delta, threshold)) {
251 markFrontalsAsChanged(changed);
253 restoreFromOriginals(originalValues,
delta);
267 std::stack<ISAM2Clique::shared_ptr> travStack;
268 travStack.push(root);
270 while (!travStack.empty()) {
271 currentNode = travStack.top();
273 bool dirty = currentNode->optimizeWildfireNode(
keys, threshold, &changed,
276 for (
const auto& child : currentNode->children) {
277 travStack.push(child);
287 void ISAM2Clique::nnz_internal(
size_t*
result)
const {
288 size_t dimR = conditional_->rows();
289 size_t dimSep = conditional_->S().cols();
290 *
result += ((dimR + 1) * dimR) / 2 + dimSep * dimR;
292 for (
const auto& child : children) {
293 child->nnz_internal(
result);
298 size_t ISAM2Clique::calculate_nnz()
const {
306 static const bool debug =
false;
309 for (
Key key : conditional_->parents()) {
317 keys->insert(conditional_->beginFrontals(), conditional_->endFrontals());
319 if (
debug) cout <<
"so marking key " << conditional_->front() << endl;
321 for (
const auto& child : children) {
322 child->findAll(markedMask,
keys);
330 for (
auto it = conditional_->begin(); it != conditional_->end(); ++it) {
331 const DenseIndex dim = conditional_->getDim(it);
332 const Vector contribution = gradientContribution_.segment(
position, dim);
333 auto [values_it, success] =
g->tryInsert(*it, contribution);
334 if (!success) values_it->second += contribution;
339 for (
const auto& child : children) {
340 child->addGradientAtZero(
g);