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

made gatherInvolvedKeys more explicit

release/4.3a0
Frank Dellaert 2019-06-03 13:39:16 -04:00
parent a9e955d56e
commit 62233395b2
2 changed files with 26 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -129,7 +129,6 @@ struct GTSAM_EXPORT UpdateImpl {
isam2.print("ISAM2: "); isam2.print("ISAM2: ");
} }
// Add the new factor indices to the result struct
if (debug || verbose) { if (debug || verbose) {
newFactors.print("The new factors are: "); newFactors.print("The new factors are: ");
} }
@ -189,12 +188,13 @@ struct GTSAM_EXPORT UpdateImpl {
// Mark linear update // Mark linear update
void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
const NonlinearFactorGraph& nonlinearFactors, const NonlinearFactorGraph& nonlinearFactors,
KeySet* markedKeys, ISAM2Result* result) const { const KeySet& keysWithRemovedFactors,
KeySet* markedKeys) const {
gttic(gatherInvolvedKeys); gttic(gatherInvolvedKeys);
*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
markedKeys->insert(result->keysWithRemovedFactors.begin(), markedKeys->insert(keysWithRemovedFactors.begin(),
result->keysWithRemovedFactors.end()); keysWithRemovedFactors.end());
// Also mark any provided extra re-eliminate keys // Also mark any provided extra re-eliminate keys
if (updateParams_.extraReelimKeys) { if (updateParams_.extraReelimKeys) {
@ -212,15 +212,19 @@ struct GTSAM_EXPORT UpdateImpl {
markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
} }
} }
}
// Update detail, unused, and observed keys from markedKeys
void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
gttic(updateKeys);
// Observed keys for detailed results // Observed keys for detailed results
if (params_.enableDetailedResults) { if (params_.enableDetailedResults) {
for (Key key : *markedKeys) { for (Key key : markedKeys) {
result->detail->variableStatus[key].isObserved = true; result->detail->variableStatus[key].isObserved = true;
} }
} }
for (Key index : *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
@ -285,13 +289,14 @@ struct GTSAM_EXPORT UpdateImpl {
/** /**
* Find the set of variables to be relinearized according to * Find the set of variables to be relinearized according to
* relinearizeThreshold. This check is performed recursively, starting at the * relinearizeThreshold. This check is performed recursively, starting at
* top of the tree. Once a variable in the tree does not need to be * the top of the tree. Once a variable in the tree does not need to be
* relinearized, no further checks in that branch are performed. This is an * relinearized, no further checks in that branch are performed. This is an
* approximation of the Full version, designed to save time at the expense of * approximation of the Full version, designed to save time at the expense
* accuracy. * of accuracy.
* @param delta The linear delta to check against the threshold * @param delta The linear delta to check against the threshold
* @param keyFormatter Formatter for printing nonlinear keys during debugging * @param keyFormatter Formatter for printing nonlinear keys during
* debugging
* @return The set of variable indices in delta whose magnitude is greater * @return The set of variable indices in delta whose magnitude is greater
* than or equal to relinearizeThreshold * than or equal to relinearizeThreshold
*/ */
@ -313,10 +318,12 @@ struct GTSAM_EXPORT UpdateImpl {
/** /**
* Find the set of variables to be relinearized according to * Find the set of variables to be relinearized according to
* relinearizeThreshold. Any variables in the VectorValues delta whose vector * relinearizeThreshold. Any variables in the VectorValues delta whose
* magnitude is greater than or equal to relinearizeThreshold are returned. * vector magnitude is greater than or equal to relinearizeThreshold are
* returned.
* @param delta The linear delta to check against the threshold * @param delta The linear delta to check against the threshold
* @param keyFormatter Formatter for printing nonlinear keys during debugging * @param keyFormatter Formatter for printing nonlinear keys during
* debugging
* @return The set of variable indices in delta whose magnitude is greater * @return The set of variable indices in delta whose magnitude is greater
* than or equal to relinearizeThreshold * than or equal to relinearizeThreshold
*/ */
@ -504,8 +511,8 @@ struct GTSAM_EXPORT UpdateImpl {
return indices; return indices;
} }
// find intermediate (linearized) factors from cache that are passed into the // find intermediate (linearized) factors from cache that are passed into
// affected area // the affected area
static GaussianFactorGraph GetCachedBoundaryFactors( static GaussianFactorGraph GetCachedBoundaryFactors(
const ISAM2::Cliques& orphans) { const ISAM2::Cliques& orphans) {
GaussianFactorGraph cachedBoundary; GaussianFactorGraph cachedBoundary;

5
gtsam/nonlinear/ISAM2.cpp
View File

@ -438,8 +438,9 @@ 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.markedKeys, update.gatherInvolvedKeys(newFactors, nonlinearFactors_,
&result); result.keysWithRemovedFactors, &result.markedKeys);
update.updateKeys(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.