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

Eliminated some copypasta with lambda

release/4.3a0
Frank Dellaert 2018-09-30 15:13:47 -04:00
parent 2fe2f1ad19
commit c50b3cd6be
1 changed files with 44 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
gtsam/nonlinear/ISAM2.cpp
View File

@ -141,8 +141,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
gttoc(affectedKeysSet); gttoc(affectedKeysSet);
gttic(check_candidates_and_linearize); gttic(check_candidates_and_linearize);
GaussianFactorGraph::shared_ptr linearized = auto linearized = boost::make_shared<GaussianFactorGraph>();
boost::make_shared<GaussianFactorGraph>();
for (Key idx : candidates) { for (Key idx : candidates) {
bool inside = true; bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors; bool useCachedLinear = params_.cacheLinearizedFactors;
@ -162,8 +161,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
#endif #endif
linearized->push_back(linearFactors_[idx]); linearized->push_back(linearFactors_[idx]);
} else { } else {
GaussianFactor::shared_ptr linearFactor = auto linearFactor = nonlinearFactors_[idx]->linearize(theta_);
nonlinearFactors_[idx]->linearize(theta_);
linearized->push_back(linearFactor); linearized->push_back(linearFactor);
if (params_.cacheLinearizedFactors) { if (params_.cacheLinearizedFactors) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
@ -268,7 +266,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
// path to root included // path to root included
gttic(affectedKeys); gttic(affectedKeys);
FastList<Key> affectedKeys; FastList<Key> affectedKeys;
for (const ConditionalType::shared_ptr& conditional : affectedBayesNet) for (const auto& conditional : affectedBayesNet)
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(),
conditional->endFrontals()); conditional->endFrontals());
gttoc(affectedKeys); gttoc(affectedKeys);
@ -758,8 +756,7 @@ ISAM2Result ISAM2::update(
// 7. Linearize new factors // 7. Linearize new factors
if (params_.cacheLinearizedFactors) { if (params_.cacheLinearizedFactors) {
gttic(linearize); gttic(linearize);
GaussianFactorGraph::shared_ptr linearFactors = auto linearFactors = newFactors.linearize(theta_);
newFactors.linearize(theta_);
if (params_.findUnusedFactorSlots) { if (params_.findUnusedFactorSlots) {
linearFactors_.resize(nonlinearFactors_.size()); linearFactors_.resize(nonlinearFactors_.size());
for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI)
@ -822,11 +819,35 @@ void ISAM2::marginalizeLeaves(
// multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors; // multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors; map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors;
// Keep track of variables removed in subtrees
KeySet leafKeysRemoved;
// Keep track of factors that get summarized by removing cliques // Keep track of factors that get summarized by removing cliques
KeySet factorIndicesToRemove; KeySet factorIndicesToRemove;
// Keep track of variables removed in subtrees // Remove the subtree and throw away the cliques
KeySet leafKeysRemoved; auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) {
const Cliques removedCliques = this->removeSubtree(subtreeRoot);
for (const sharedClique& removedClique : removedCliques) {
auto cg = removedClique->conditional();
marginalFactors.erase(cg->front());
leafKeysRemoved.insert(cg->beginFrontals(), cg->endFrontals());
for (Key frontal : cg->frontals()) {
// Add to factors to remove
const auto& involved = variableIndex_[frontal];
factorIndicesToRemove.insert(involved.begin(), involved.end());
#if !defined(NDEBUG)
// Check for non-leaf keys
if (!leafKeys.exists(frontal))
throw std::runtime_error(
"Requesting to marginalize variables that are not leaves, "
"the ISAM2 object is now in an inconsistent state so should "
"no longer be used.");
#endif
}
}
return removedCliques;
};
// Remove each variable and its subtrees // Remove each variable and its subtrees
for (Key j : leafKeys) { for (Key j : leafKeys) {
@ -858,7 +879,7 @@ void ISAM2::marginalizeLeaves(
if (marginalizeEntireClique) { if (marginalizeEntireClique) {
// Remove the whole clique and its subtree, and keep the marginal // Remove the whole clique and its subtree, and keep the marginal
// factor. // factor.
GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); auto marginalFactor = clique->cachedFactor();
// We do not need the marginal factors associated with this clique // We do not need the marginal factors associated with this clique
// because their information is already incorporated in the new // because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the // marginal factor. So, now associate this marginal factor with the
@ -867,27 +888,7 @@ void ISAM2::marginalizeLeaves(
marginalFactor); marginalFactor);
// Now remove this clique and its subtree - all of its marginal // Now remove this clique and its subtree - all of its marginal
// information has been stored in marginalFactors. // information has been stored in marginalFactors.
const Cliques removedCliques = this->removeSubtree( trackingRemoveSubtree(clique);
clique); // Remove the subtree and throw away the cliques
for (const sharedClique& removedClique : removedCliques) {
marginalFactors.erase(removedClique->conditional()->front());
leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(),
removedClique->conditional()->endFrontals());
for (Key frontal : removedClique->conditional()->frontals()) {
// Add to factors to remove
const VariableIndex::Factors& involvedFactors =
variableIndex_[frontal];
factorIndicesToRemove.insert(involvedFactors.begin(),
involvedFactors.end());
// Check for non-leaf keys
if (!leafKeys.exists(frontal))
throw runtime_error(
"Requesting to marginalize variables that are not leaves, "
"the ISAM2 object is now in an inconsistent state so should "
"no longer be used.");
}
}
} else { } else {
// Reeliminate the current clique and the marginals from its children, // Reeliminate the current clique and the marginals from its children,
// then keep only the marginal on the non-marginalized variables. We // then keep only the marginal on the non-marginalized variables. We
@ -910,31 +911,10 @@ void ISAM2::marginalizeLeaves(
} }
} }
Cliques childrenRemoved; Cliques childrenRemoved;
for (const sharedClique& childToRemove : subtreesToRemove) { for (const sharedClique& subtree : subtreesToRemove) {
const Cliques removedCliques = this->removeSubtree( const Cliques removed = trackingRemoveSubtree(subtree);
childToRemove); // Remove the subtree and throw away the cliques childrenRemoved.insert(childrenRemoved.end(), removed.begin(),
childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removed.end());
removedCliques.end());
for (const sharedClique& removedClique : removedCliques) {
marginalFactors.erase(removedClique->conditional()->front());
leafKeysRemoved.insert(
removedClique->conditional()->beginFrontals(),
removedClique->conditional()->endFrontals());
for (Key frontal : removedClique->conditional()->frontals()) {
// Add to factors to remove
const VariableIndex::Factors& involvedFactors =
variableIndex_[frontal];
factorIndicesToRemove.insert(involvedFactors.begin(),
involvedFactors.end());
// Check for non-leaf keys
if (!leafKeys.exists(frontal))
throw runtime_error(
"Requesting to marginalize variables that are not leaves, "
"the ISAM2 object is now in an inconsistent state so "
"should no longer be used.");
}
}
} }
// Add the factors that are pulled into the current clique by the // Add the factors that are pulled into the current clique by the
@ -969,9 +949,8 @@ void ISAM2::marginalizeLeaves(
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
leafKeys.begin(), leafKeys.end(), leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate)); std::back_inserter(cliqueFrontalsToEliminate));
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> auto eliminationResult1 = params_.getEliminationFunction()(
eliminationResult1 = params_.getEliminationFunction()( graph, Ordering(cliqueFrontalsToEliminate));
graph, Ordering(cliqueFrontalsToEliminate));
// Add the resulting marginal // Add the resulting marginal
if (eliminationResult1.second) if (eliminationResult1.second)
@ -997,13 +976,11 @@ void ISAM2::marginalizeLeaves(
originalKeys.end()); originalKeys.end());
clique->conditional()->nrFrontals() -= nToRemove; clique->conditional()->nrFrontals() -= nToRemove;
// Add to factors to remove factors involved in frontals of current // Add to factorIndicesToRemove any factors involved in frontals of
// clique // current clique
for (Key frontal : cliqueFrontalsToEliminate) { for (Key frontal : cliqueFrontalsToEliminate) {
const VariableIndex::Factors& involvedFactors = const auto& involved = variableIndex_[frontal];
variableIndex_[frontal]; factorIndicesToRemove.insert(involved.begin(), involved.end());
factorIndicesToRemove.insert(involvedFactors.begin(),
involvedFactors.end());
} }
// Add removed keys // Add removed keys
@ -1018,9 +995,8 @@ void ISAM2::marginalizeLeaves(
// Gather factors to add - the new marginal factors // Gather factors to add - the new marginal factors
GaussianFactorGraph factorsToAdd; GaussianFactorGraph factorsToAdd;
typedef pair<Key, vector<GaussianFactor::shared_ptr> > Key_Factors; for (const auto& key_factors : marginalFactors) {
for (const Key_Factors& key_factors : marginalFactors) { for (const auto& factor : key_factors.second) {
for (const GaussianFactor::shared_ptr& factor : key_factors.second) {
if (factor) { if (factor) {
factorsToAdd.push_back(factor); factorsToAdd.push_back(factor);
if (marginalFactorsIndices) if (marginalFactorsIndices)