1. Refactor code in BayesTreeMarginalizationHelper;
2. And avoid the unnecessary re-elimination of subtrees
that only contain marginalizable variables;
Add BayesTreeMarginalizationHelper.h and use the new helper
to gather the additional keys to re-eliminate when marginalizing
variables in IncrementalFixedLagSmoother.