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

made markedKeys explicit

release/4.3a0
Frank Dellaert 2019-06-02 16:36:36 -04:00
parent 1757198463
commit 3b51bae5c1
3 changed files with 42 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/nonlinear/ISAM2-impl.h
View File

@ -140,14 +140,15 @@ struct GTSAM_EXPORT UpdateImpl {
NonlinearFactorGraph* nonlinearFactors, NonlinearFactorGraph* nonlinearFactors,
GaussianFactorGraph* linearFactors, GaussianFactorGraph* linearFactors,
VariableIndex* variableIndex, VariableIndex* variableIndex,
ISAM2Result* result) const { FactorIndices* newFactorsIndices,
KeySet* keysWithRemovedFactors) const {
gttic(pushBackFactors); gttic(pushBackFactors);
// Perform the first part of the bookkeeping updates for adding new factors. // Perform the first part of the bookkeeping updates for adding new factors.
// Adds them to the complete list of nonlinear factors, and populates the // Adds them to the complete list of nonlinear factors, and populates the
// list of new factor indices, both optionally finding and reusing empty // list of new factor indices, both optionally finding and reusing empty
// factor slots. // factor slots.
result->newFactorsIndices = nonlinearFactors->add_factors( *newFactorsIndices = nonlinearFactors->add_factors(
newFactors, params_.findUnusedFactorSlots); newFactors, params_.findUnusedFactorSlots);
// Remove the removed factors // Remove the removed factors
@ -164,37 +165,41 @@ struct GTSAM_EXPORT UpdateImpl {
variableIndex->remove(updateParams_.removeFactorIndices.begin(), variableIndex->remove(updateParams_.removeFactorIndices.begin(),
updateParams_.removeFactorIndices.end(), updateParams_.removeFactorIndices.end(),
removedFactors); removedFactors);
result->keysWithRemovedFactors = removedFactors.keys(); *keysWithRemovedFactors = removedFactors.keys();
}
// Compute unused keys and indices // Get keys from removed factors and new factors, and compute unused keys,
// Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors.
// i.e., keys that are empty now and do not appear in the new factors. void computeUnusedKeys(const NonlinearFactorGraph& newFactors,
const VariableIndex& variableIndex,
const KeySet& keysWithRemovedFactors,
KeySet* unusedKeys) const {
gttic(computeUnusedKeys);
KeySet removedAndEmpty; KeySet removedAndEmpty;
for (Key key : result->keysWithRemovedFactors) { for (Key key : keysWithRemovedFactors) {
if (variableIndex->empty(key)) if (variableIndex.empty(key))
removedAndEmpty.insert(removedAndEmpty.end(), key); removedAndEmpty.insert(removedAndEmpty.end(), key);
} }
KeySet newFactorSymbKeys = newFactors.keys(); KeySet newFactorSymbKeys = newFactors.keys();
std::set_difference( std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
removedAndEmpty.begin(), removedAndEmpty.end(), newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(*unusedKeys, unusedKeys->end()));
std::inserter(result->unusedKeys, result->unusedKeys.end()));
} }
// Mark linear update // Mark linear update
void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
const NonlinearFactorGraph& nonlinearFactors, const NonlinearFactorGraph& nonlinearFactors,
ISAM2Result* result) const { KeySet* markedKeys, ISAM2Result* result) const {
gttic(gatherInvolvedKeys); gttic(gatherInvolvedKeys);
result->markedKeys = newFactors.keys(); // Get keys from new factors *markedKeys = newFactors.keys(); // Get keys from new factors
// Also mark keys involved in removed factors // Also mark keys involved in removed factors
result->markedKeys.insert(result->keysWithRemovedFactors.begin(), markedKeys->insert(result->keysWithRemovedFactors.begin(),
result->keysWithRemovedFactors.end()); result->keysWithRemovedFactors.end());
// Also mark any provided extra re-eliminate keys // Also mark any provided extra re-eliminate keys
if (updateParams_.extraReelimKeys) { if (updateParams_.extraReelimKeys) {
for (Key key : *updateParams_.extraReelimKeys) { for (Key key : *updateParams_.extraReelimKeys) {
result->markedKeys.insert(key); markedKeys->insert(key);
} }
} }
@ -204,18 +209,18 @@ struct GTSAM_EXPORT UpdateImpl {
for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
const auto factorIdx = factorAddedKeys.first; const auto factorIdx = factorAddedKeys.first;
const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys(); const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys();
result->markedKeys.insert(affectedKeys.begin(), affectedKeys.end()); markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
} }
} }
// Observed keys for detailed results // Observed keys for detailed results
if (params_.enableDetailedResults) { if (params_.enableDetailedResults) {
for (Key key : result->markedKeys) { for (Key key : *markedKeys) {
result->detail->variableStatus[key].isObserved = true; result->detail->variableStatus[key].isObserved = true;
} }
} }
for (Key index : result->markedKeys) { for (Key index : *markedKeys) {
// Only add if not unused // Only add if not unused
if (result->unusedKeys.find(index) == result->unusedKeys.end()) if (result->unusedKeys.find(index) == result->unusedKeys.end())
// Make a copy of these, as we'll soon add to them // Make a copy of these, as we'll soon add to them
@ -347,7 +352,7 @@ struct GTSAM_EXPORT UpdateImpl {
// Mark keys in \Delta above threshold \beta: // Mark keys in \Delta above threshold \beta:
KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
const VectorValues& delta, const VectorValues& delta,
const KeySet& fixedVariables, const KeySet& fixedVariables, KeySet* markedKeys,
ISAM2Result* result) const { ISAM2Result* result) const {
gttic(gatherRelinearizeKeys); gttic(gatherRelinearizeKeys);
// J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
@ -378,18 +383,18 @@ struct GTSAM_EXPORT UpdateImpl {
} }
// Add the variables being relinearized to the marked keys // Add the variables being relinearized to the marked keys
result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); markedKeys->insert(relinKeys.begin(), relinKeys.end());
return relinKeys; return relinKeys;
} }
// Mark all cliques that involve marked variables \Theta_{J} and all // Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors. // their ancestors.
void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys,
ISAM2Result* result) const { KeySet* markedKeys, ISAM2Result* result) const {
gttic(fluidFindAll); gttic(fluidFindAll);
for (const auto& root : roots) for (const auto& root : roots)
// add other cliques that have the marked ones in the separator // add other cliques that have the marked ones in the separator
root->findAll(relinKeys, &result->markedKeys); root->findAll(relinKeys, markedKeys);
// Relinearization-involved keys for detailed results // Relinearization-involved keys for detailed results
if (params_.enableDetailedResults) { if (params_.enableDetailedResults) {

22
gtsam/nonlinear/ISAM2.cpp
View File

@ -350,11 +350,9 @@ void ISAM2::addVariables(
const Values& newTheta, const Values& newTheta,
ISAM2Result::DetailedResults::StatusMap* variableStatus) { ISAM2Result::DetailedResults::StatusMap* variableStatus) {
gttic(addNewVariables); gttic(addNewVariables);
// \Theta:=\Theta\cup\Theta_{new}.
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta_.insert(newTheta); theta_.insert(newTheta);
if (debug) newTheta.print("The new variables are: "); if (ISDEBUG("ISAM2 AddVariables")) newTheta.print("The new variables are: ");
// Add zeros into the VectorValues // Add zeros into the VectorValues
delta_.insert(newTheta.zeroVectors()); delta_.insert(newTheta.zeroVectors());
deltaNewton_.insert(newTheta.zeroVectors()); deltaNewton_.insert(newTheta.zeroVectors());
@ -425,9 +423,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
// 1. Add any new factors \Factors:=\Factors\cup\Factors'. // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_,
&variableIndex_, &result); &variableIndex_, &result.newFactorsIndices,
&result.keysWithRemovedFactors);
update.computeUnusedKeys(newFactors, variableIndex_,
result.keysWithRemovedFactors, &result.unusedKeys);
// 2. Initialize any new variables \Theta_{new} and add // 2. Initialize any new variables \Theta_{new} and add
// \Theta:=\Theta\cup\Theta_{new}.
addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0);
gttic(evaluate_error_before); gttic(evaluate_error_before);
@ -436,19 +438,20 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
gttoc(evaluate_error_before); gttoc(evaluate_error_before);
// 3. Mark linear update // 3. Mark linear update
update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result); update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result.markedKeys,
&result);
// Check relinearization if we're at the nth step, or we are using a looser // Check relinearization if we're at the nth step, or we are using a looser
// loop relinerization threshold. // loop relinerization threshold.
KeySet relinKeys; KeySet relinKeys;
if (relinearizeThisStep) { if (relinearizeThisStep) {
// 4. Mark keys in \Delta above threshold \beta: // 4. Mark keys in \Delta above threshold \beta:
relinKeys = relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_,
update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result); &result.markedKeys, &result);
if (!relinKeys.empty()) { if (!relinKeys.empty()) {
// 5. Mark all cliques that involve marked variables \Theta_{J} and all // 5. Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors. // their ancestors.
update.fluidFindAll(roots_, relinKeys, &result); update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result);
// 6. Update linearization point for marked variables: // 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}. // \Theta_{J}:=\Theta_{J}+\Delta_{J}.
UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_);
@ -479,8 +482,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet, KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet,
relinKeys, &orphans, &result); relinKeys, &orphans, &result);
// Update replaced keys mask (accumulates until back-substitution takes // Update replaced keys mask (accumulates until back-substitution happens)
// place)
deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end());
} }

2
gtsam/nonlinear/ISAM2.h
View File

@ -281,7 +281,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
/// @} /// @}
protected: protected:
// Do a batch step - reorder and relinearize all variables // Do a batch step - reorder and relinearize all variables
void recalculateBatch(const ISAM2UpdateParams& updateParams, void recalculateBatch(const ISAM2UpdateParams& updateParams,
KeySet* affectedKeysSet, ISAM2Result* result); KeySet* affectedKeysSet, ISAM2Result* result);