12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small cleanup of copy/paste call

release/4.3a0
Frank Dellaert 2018-09-30 15:26:36 -04:00
parent c50b3cd6be
commit 3e0ee2052d
1 changed files with 10 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/nonlinear/ISAM2.cpp
View File

@ -943,8 +943,8 @@ void ISAM2::marginalizeLeaves(
// Reeliminate the linear graph to get the marginal and discard the // Reeliminate the linear graph to get the marginal and discard the
// conditional // conditional
const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), auto cg = clique->conditional();
clique->conditional()->endFrontals()); const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
FastVector<Key> cliqueFrontalsToEliminate; FastVector<Key> cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
leafKeys.begin(), leafKeys.end(), leafKeys.begin(), leafKeys.end(),
@ -954,27 +954,23 @@ void ISAM2::marginalizeLeaves(
// Add the resulting marginal // Add the resulting marginal
if (eliminationResult1.second) if (eliminationResult1.second)
marginalFactors[clique->conditional()->front()].push_back( marginalFactors[cg->front()].push_back(eliminationResult1.second);
eliminationResult1.second);
// Split the current clique // Split the current clique
// Find the position of the last leaf key in this clique // Find the position of the last leaf key in this clique
DenseIndex nToRemove = 0; DenseIndex nToRemove = 0;
while (leafKeys.exists(clique->conditional()->keys()[nToRemove])) while (leafKeys.exists(cg->keys()[nToRemove])) ++nToRemove;
++nToRemove;
// Make the clique's matrix appear as a subset // Make the clique's matrix appear as a subset
const DenseIndex dimToRemove = const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove);
clique->conditional()->matrixObject().offset(nToRemove); cg->matrixObject().firstBlock() = nToRemove;
clique->conditional()->matrixObject().firstBlock() = nToRemove; cg->matrixObject().rowStart() = dimToRemove;
clique->conditional()->matrixObject().rowStart() = dimToRemove;
// Change the keys in the clique // Change the keys in the clique
FastVector<Key> originalKeys; FastVector<Key> originalKeys;
originalKeys.swap(clique->conditional()->keys()); originalKeys.swap(cg->keys());
clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
originalKeys.end()); cg->nrFrontals() -= nToRemove;
clique->conditional()->nrFrontals() -= nToRemove;
// Add to factorIndicesToRemove any factors involved in frontals of // Add to factorIndicesToRemove any factors involved in frontals of
// current clique // current clique