39 template class BayesTree<ISAM2Clique>;
73 gttic(affectedKeysSet);
76 affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
77 gttoc(affectedKeysSet);
79 gttic(check_candidates_and_linearize);
85 if (affectedKeysSet.find(
key) == affectedKeysSet.end()) {
89 if (useCachedLinear && relinKeys.find(
key) != relinKeys.end())
90 useCachedLinear =
false;
93 if (useCachedLinear) {
94 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
103 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
111 gttoc(check_candidates_and_linearize);
122 if (!
result->markedKeys.empty() || !
result->observedKeys.empty()) {
131 &affectedBayesNet, &orphans);
150 for (
const auto& conditional : affectedBayesNet)
151 affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(),
152 conditional->endFrontals());
156 static const double kBatchThreshold = 0.65;
157 if (affectedKeys.size() >=
theta_.
size() * kBatchThreshold) {
162 &affectedKeysSet, &orphans,
result);
167 for (
const auto& root :
roots_)
168 for (
Key var : *root->conditional())
169 result->detail->variableStatus[var].inRootClique =
true;
186 affectedKeysSet->insert(
key);
192 result->unusedKeys.end());
195 affectedKeysSet->erase(
key);
208 for (
Key var :
result->observedKeys) constraintGroups[var] = 1;
233 bayesTree->roots().end());
235 nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
238 result->variablesReeliminated = affectedKeysSet->size();
244 result->detail->variableStatus[
key].isReeliminated =
true;
260 affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(),
262 affectedAndNewKeys.insert(affectedAndNewKeys.end(),
263 result->observedKeys.begin(),
264 result->observedKeys.end());
269 factors.print(
"Relinearized factors: ");
270 std::cout <<
"Affected keys: ";
271 for (
const Key key : affectedKeys) {
272 std::cout <<
key <<
" ";
274 std::cout << std::endl;
279 for (
Key key : affectedAndNewKeys) {
280 result->detail->variableStatus[
key].isReeliminated =
true;
284 result->variablesReeliminated = affectedAndNewKeys.size();
291 if (
debug) cachedBoundary.
print(
"Boundary factors: ");
292 factors.push_back(cachedBoundary);
297 for (
const auto& orphan : *orphans)
305 gttic(reorder_and_eliminate);
311 affectedKeysSet->insert(
result->markedKeys.begin(),
result->markedKeys.end());
312 affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
317 gttic(ordering_constraints);
325 result->observedKeys.size() < affectedFactorsVarIndex.
size() ? 1 : 0;
327 constraintGroups.emplace(var, group);
332 iter != constraintGroups.end();
336 constraintGroups.erase(
iter++);
340 gttoc(ordering_constraints);
353 gttoc(reorder_and_eliminate);
357 bayesTree->roots().end());
358 nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
367 gttic(addNewVariables);
370 if (
ISDEBUG(
"ISAM2 AddVariables")) newTheta.
print(
"The new variables are: ");
379 detail->variableStatus[
key].isNew =
true;
389 for (
Key key : unusedKeys) {
407 bool force_relinearize) {
409 params.constrainedKeys = constrainedKeys;
410 params.extraReelimKeys = extraReelimKeys;
411 params.force_relinearize = force_relinearize;
412 params.noRelinKeys = noRelinKeys;
413 params.removeFactorIndices = removeFactorIndices;
440 &
result.keysWithRemovedFactors);
454 result.variablesRelinearized = 0;
459 update.recordRelinearizeDetail(relinKeys,
result.details());
460 if (!relinKeys.empty()) {
467 result.variablesRelinearized =
result.markedKeys.size();
473 update.augmentVariableIndex(newFactors,
result.newFactorsIndices,
492 KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());
497 map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors;
506 auto trackingRemoveSubtree = [&](
const sharedClique& subtreeRoot) {
508 for (
const sharedClique& removedClique : removedCliques) {
509 auto cg = removedClique->conditional();
510 marginalFactors.erase(cg->front());
511 leafKeysRemoved.insert(cg->beginFrontals(), cg->endFrontals());
512 for (
Key frontal : cg->frontals()) {
515 factorIndicesToRemove.insert(involved.begin(), involved.end());
518 if (!leafKeys.
exists(frontal))
519 throw std::runtime_error(
520 "Requesting to marginalize variables that are not leaves, "
521 "the ISAM2 object is now in an inconsistent state so should "
522 "no longer be used.");
526 return removedCliques;
530 for (
Key j : leafKeys) {
531 if (!leafKeysRemoved.
exists(
j)) {
536 while (
clique->parent_.use_count() != 0) {
540 if (leafKeys.exists(parent->conditional()->front()))
547 bool marginalizeEntireClique =
true;
548 for (
Key frontal :
clique->conditional()->frontals()) {
549 if (!leafKeys.exists(frontal)) {
550 marginalizeEntireClique =
false;
556 if (marginalizeEntireClique) {
566 marginalFactors[
clique->parent()->conditional()->front()].push_back(
571 trackingRemoveSubtree(
clique);
581 KeySet factorsInSubtreeRoot;
585 for (
Key parent : child->conditional()->parents()) {
586 if (leafKeys.exists(parent)) {
587 subtreesToRemove.push_back(child);
588 graph.push_back(child->cachedFactor());
595 const Cliques removed = trackingRemoveSubtree(subtree);
596 childrenRemoved.insert(childrenRemoved.end(), removed.begin(),
605 KeySet factorsFromMarginalizedInClique_step1;
606 for (
Key frontal :
clique->conditional()->frontals()) {
607 if (leafKeys.exists(frontal))
608 factorsFromMarginalizedInClique_step1.insert(
612 for (
const sharedClique& removedChild : childrenRemoved) {
613 for (
Key indexInClique : removedChild->conditional()->frontals()) {
615 factorsFromMarginalizedInClique_step1.erase(factorInvolving);
620 for (
const auto index : factorsFromMarginalizedInClique_step1) {
626 auto cg =
clique->conditional();
627 const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
629 std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
630 leafKeys.begin(), leafKeys.end(),
631 std::back_inserter(cliqueFrontalsToEliminate));
636 if (eliminationResult1.second)
637 marginalFactors[cg->front()].push_back(eliminationResult1.second);
642 while (leafKeys.exists(cg->keys()[nToRemove])) ++nToRemove;
645 const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove);
646 cg->matrixObject().firstBlock() += nToRemove;
647 cg->matrixObject().rowStart() = dimToRemove;
651 originalKeys.swap(cg->keys());
652 cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
653 cg->nrFrontals() -= nToRemove;
657 for (
Key frontal : cliqueFrontalsToEliminate) {
659 factorIndicesToRemove.insert(involved.begin(), involved.end());
663 leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(),
664 cliqueFrontalsToEliminate.end());
674 for (
const auto index : factorIndicesToRemove) {
682 factorIndicesToRemove.end(), removedFactors);
687 for (
const auto& key_factors : marginalFactors) {
688 for (
const auto&
factor : key_factors.second) {
714 if (deletedFactorsIndices) {
715 deletedFactorsIndices->assign(factorIndicesToRemove.begin(),
716 factorIndicesToRemove.end());
718 if (marginalFactorsIndices){
719 *marginalFactorsIndices = std::move(newFactorIndices);
731 const double effectiveWildfireThreshold =
733 gttic(Wildfire_update);
735 effectiveWildfireThreshold, &
delta_);
737 gttoc(Wildfire_update);
743 const double effectiveWildfireThreshold =
747 gttic(Dogleg_Iterate);
750 gttic(Wildfire_update);
753 gttoc(Wildfire_update);
772 gttoc(Dogleg_Iterate);
781 throw std::runtime_error(
"iSAM2: unknown ISAM2Params type");
787 gttic(ISAM2_calculateEstimate);
830 for (
const auto& root : this->
roots()) root->addGradientAtZero(&
g);