-
Notifications
You must be signed in to change notification settings - Fork 766
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix variable marginalization for IncrementalFixedLagSmoother #1890
base: develop
Are you sure you want to change the base?
Fix variable marginalization for IncrementalFixedLagSmoother #1890
Conversation
Add BayesTreeMarginalizationHelper.h and use the new helper to gather the additional keys to re-eliminate when marginalizing variables in IncrementalFixedLagSmoother.
About the fix: In this PR, I created a new header file BayesTreeMarginalizationHelper.h and added a helper function Note the old marginalization code are still kept, but disabled. To compare the effects of old and the new code, you can enable / disable the macro GTSAM_OLD_MARGINALIZATION to switch between the two versions. When the macro |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Wow, this must be one of the most thorough PRs I have seen :-) Many thanks!
Two requests:
- I trust you: just remove the "ifdefery" :-)
- the main function is too long. I asked Claude 3.5 for a re-factor. Could you try that on your end and use that (or something similar) if you're convinced it's correct?
using Clique = typename BayesTree::Clique; | ||
using sharedClique = typename BayesTree::sharedClique; | ||
|
||
/** Get the additional keys that need reelimination when marginalizing |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This function (and its docs) are too long to understand well. In general I'd like to adhere to a rule "no function should be longer than 4 lines and never more than a page" :-) We violate that a lot. I asked Claude 3.5 to refactor (and also return the keys, rather than using a non-const reference) and it came up with this:
class BayesTreeMarginalizationHelper {
private:
/**
* Check if a clique contains variables that need reelimination due to
* marginalization ordering conflicts.
*
* @param[in] clique The clique to check
* @param[in] marginalizableKeys Set of keys to be marginalized
* @return true if any variables in the clique need re-elimination
*/
static bool needsReelimination(
const sharedClique& clique,
const std::set<Key>& marginalizableKeys) {
bool hasNonMarginalizableAhead = false;
// Check each frontal variable in order
for (Key key : clique->conditional()->frontals()) {
if (marginalizableKeys.count(key)) {
// If we've seen non-marginalizable variables before this one,
// we need to reeliminate
if (hasNonMarginalizableAhead) {
return true;
}
// Check if any child depends on this marginalizable key
if (hasChildDependency(clique, key)) {
return true;
}
} else {
hasNonMarginalizableAhead = true;
}
}
return false;
}
/**
* Check if any child clique depends on the given key.
*
* @param[in] clique Parent clique to check
* @param[in] key Key to check for dependencies
* @return true if any child depends on the key
*/
static bool hasChildDependency(
const sharedClique& clique,
Key key) {
for (const sharedClique& child : clique->children) {
auto parents = child->conditional();
if (std::find(parents->beginParents(),
parents->endParents(), key)
!= parents->endParents()) {
return true;
}
}
return false;
}
/**
* Gather all dependent cliques that need re-elimination based on a root clique.
*
* @param[in] rootClique The starting clique
* @param[in] marginalizableKeys Set of keys to be marginalized
* @param[out] dependentCliques Pointer to set storing dependent cliques
* @param[out] additionalKeys Pointer to set storing keys that need reelimination
*/
static void gatherDependentCliques(
const sharedClique& rootClique,
const std::set<Key>& marginalizableKeys,
std::set<sharedClique>* dependentCliques,
std::set<Key>* additionalKeys) {
// Add frontal variables from current clique
for (Key key : rootClique->conditional()->frontals()) {
additionalKeys->insert(key);
// Find children that depend on this key
for (const sharedClique& child : rootClique->children) {
if (!dependentCliques->count(child) &&
hasChildDependency(rootClique, key)) {
dependentCliques->insert(child);
}
}
}
}
/**
* Process all dependent cliques and gather their keys.
*
* @param[in,out] dependentCliques Pointer to set of cliques to process
* @return Set of keys from all dependent cliques
*/
static std::set<Key> processDependentCliques(
std::set<sharedClique>* dependentCliques) {
std::set<Key> additionalKeys;
while (!dependentCliques->empty()) {
auto begin = dependentCliques->begin();
sharedClique clique = *begin;
dependentCliques->erase(begin);
// Add all frontal variables from this clique
for (Key key : clique->conditional()->frontals()) {
additionalKeys.insert(key);
}
// Add all children for processing
for (const sharedClique& child : clique->children) {
dependentCliques->insert(child);
}
}
return additionalKeys;
}
public:
/**
* Get additional keys that need re-elimination when marginalizing variables.
*
* This function identifies variables that need to be re-eliminated to ensure
* proper marginalization order in two cases:
*
* 1. When a child node depends on a variable being marginalized
* 2. When non-marginalizable variables appear before marginalized ones
*
* @param[in] bayesTree The Bayes tree
* @param[in] marginalizableKeys Keys to be marginalized
* @return Set of additional keys that need to be re-eliminated
*/
static std::set<Key> gatherAdditionalKeysToReEliminate(
const BayesTree& bayesTree,
const KeyVector& marginalizableKeys) {
const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
std::set<Key> marginalizableKeySet(marginalizableKeys.begin(),
marginalizableKeys.end());
std::set<sharedClique> checkedCliques;
std::set<sharedClique> dependentCliques;
std::set<Key> additionalKeys;
// Check each marginalizable key's clique
for (const Key& key : marginalizableKeySet) {
sharedClique clique = bayesTree[key];
if (checkedCliques.count(clique)) continue;
checkedCliques.insert(clique);
if (needsReelimination(clique, marginalizableKeySet)) {
gatherDependentCliques(clique, marginalizableKeySet,
&dependentCliques, &additionalKeys);
}
}
// Process remaining dependent cliques
auto dependentKeys = processDependentCliques(&dependentCliques);
additionalKeys.insert(dependentKeys.begin(), dependentKeys.end());
if (debug) {
std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: ";
for (const Key& key : additionalKeys) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << std::endl;
}
return additionalKeys;
}
};
This pull request fixes a bug in the marginalization of IncrementalFixedLagSmoother.
If the variable to be marginalized, say
xi
,xi
,xj
, beforexi
in the same nodethen
IncrementalFixedLagSmoother
can not marginalize the variablexi
from the underlying Bayes tree but it removes the value forxi
, which causes exceptions like:The bug stems from this piece of code,
When a variable in
marginalizableKeys
is in the situations listed above, it needs re-elimination so that it can be eliminated before other variables. But with the code above it won't be added to the setadditionalKeys
, and consequently that variable won't be re-eliminated in the subsequent isam update.For a specific example of the bug, see the Bayes tree below and consider the variable
x42
:In this case, if we marginalize
x42
(put it inmarginalizableKeys
), the code above won't add any variable toadditionalKeys
, andx42
will not be re-eliminated to become a "leaf key". ISAM2::marginalizeLeaves() can't handle this corner case correctly (it doesn't effectively marginalize the variable from the tree, but remove the value and factor indices of the variable).The example above can be reproduced with the modified unit test for
IncrementalFixedLagSmoother
in this PR. See testIncrementalFixedLagSmoother.cpp