-
Notifications
You must be signed in to change notification settings - Fork 789
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
Hybrid Bayes Net/Tree Optimize #1280
Changes from all commits
a6101b2
f0df82a
cb2d2e6
86320ff
74bddd9
129fa68
7272268
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -35,4 +35,48 @@ bool HybridBayesTree::equals(const This& other, double tol) const { | |
return Base::equals(other, tol); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { | ||
GaussianBayesNet gbn; | ||
|
||
KeyVector added_keys; | ||
|
||
// Iterate over all the nodes in the BayesTree | ||
for (auto&& node : nodes()) { | ||
// Check if conditional being added is already in the Bayes net. | ||
if (std::find(added_keys.begin(), added_keys.end(), node.first) == | ||
added_keys.end()) { | ||
// Access the clique and get the underlying hybrid conditional | ||
HybridBayesTreeClique::shared_ptr clique = node.second; | ||
HybridConditional::shared_ptr conditional = clique->conditional(); | ||
|
||
KeyVector frontals(conditional->frontals().begin(), | ||
conditional->frontals().end()); | ||
|
||
// Record the key being added | ||
added_keys.insert(added_keys.end(), frontals.begin(), frontals.end()); | ||
|
||
// If conditional is hybrid (and not discrete-only), we get the Gaussian | ||
// Conditional corresponding to the assignment and add it to the Gaussian | ||
// Bayes Net. | ||
if (conditional->isHybrid()) { | ||
auto gm = conditional->asMixture(); | ||
GaussianConditional::shared_ptr gaussian_conditional = | ||
(*gm)(assignment); | ||
|
||
gbn.push_back(gaussian_conditional); | ||
} | ||
} | ||
} | ||
// If TBB is enabled, the bayes net order gets reversed, | ||
// so we pre-reverse it | ||
#ifdef GTSAM_USE_TBB | ||
auto reversed = boost::adaptors::reverse(gbn); | ||
gbn = GaussianBayesNet(reversed.begin(), reversed.end()); | ||
#endif | ||
|
||
// Return the optimized bayes net. | ||
return gbn.optimize(); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Does this mean that There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The GBN needs to be topologically sorted so that it optimizes parents before children, and for some reason, the GBN is reverse topologically sorted only when TBB is enabled. |
||
} | ||
|
||
} // namespace gtsam |
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.
assert that idx is indeed discrete only
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.
Made the assertion in #1282