1. Skip subtrees that have already been visited when searching for
dependent cliques;
2. Avoid copying shared_ptrs (which needs extra expensive atomic
operations) in the searching. Use const Clique* instead of
sharedClique whenever possible;
3. Use std::unordered_set instead of std::set to improve average
searching speed.
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.
p-zach
Jeffrey