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

Removed unusedIndices

release/4.3a0
Frank Dellaert 2019-06-01 23:55:50 -04:00
parent 42b5f81633
commit 696d8076d1
2 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -179,11 +179,6 @@ struct GTSAM_EXPORT UpdateImpl {
removedAndEmpty.begin(), removedAndEmpty.end(), removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
std::inserter(result->unusedKeys, result->unusedKeys.end())); std::inserter(result->unusedKeys, result->unusedKeys.end()));
// Get indices for unused keys
for (Key key : result->unusedKeys) {
result->unusedIndices.insert(result->unusedIndices.end(), key);
}
} }
// 3. Mark linear update // 3. Mark linear update
@ -221,7 +216,7 @@ struct GTSAM_EXPORT UpdateImpl {
for (Key index : result->markedKeys) { for (Key index : result->markedKeys) {
// Only add if not unused // Only add if not unused
if (result->unusedIndices.find(index) == result->unusedIndices.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
result->observedKeys.push_back(index); result->observedKeys.push_back(index);
} }
@ -631,9 +626,9 @@ struct GTSAM_EXPORT UpdateImpl {
VariableIndex affectedFactorsVarIndex = variableIndex; VariableIndex affectedFactorsVarIndex = variableIndex;
affectedFactorsVarIndex.removeUnusedVariables( affectedFactorsVarIndex.removeUnusedVariables(
result->unusedIndices.begin(), result->unusedIndices.end()); result->unusedKeys.begin(), result->unusedKeys.end());
for (const Key key : result->unusedIndices) { for (const Key key : result->unusedKeys) {
affectedKeysSet.erase(key); affectedKeysSet.erase(key);
} }
gttoc(add_keys); gttoc(add_keys);
@ -777,7 +772,7 @@ struct GTSAM_EXPORT UpdateImpl {
for (FastMap<Key, int>::iterator iter = constraintGroups.begin(); for (FastMap<Key, int>::iterator iter = constraintGroups.begin();
iter != constraintGroups.end(); iter != constraintGroups.end();
/*Incremented in loop ++iter*/) { /*Incremented in loop ++iter*/) {
if (result->unusedIndices.exists(iter->first) || if (result->unusedKeys.exists(iter->first) ||
!affectedKeysSet.exists(iter->first)) !affectedKeysSet.exists(iter->first))
constraintGroups.erase(iter++); constraintGroups.erase(iter++);
else else

4
gtsam/nonlinear/ISAM2Result.h
View File

@ -96,10 +96,10 @@ struct GTSAM_EXPORT ISAM2Result {
*/ */
FactorIndices newFactorsIndices; FactorIndices newFactorsIndices;
/** Unused keys, and indices for unused keys. TODO(frank): the same?? /** Unused keys, and indices for 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.
*/ */
KeySet unusedKeys, unusedIndices; KeySet unusedKeys;
/** keys for variables that were observed, i.e., not unused. */ /** keys for variables that were observed, i.e., not unused. */
KeyVector observedKeys; KeyVector observedKeys;