-
Notifications
You must be signed in to change notification settings - Fork 765
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1309 from borglab/varun/pruning-fix
- Loading branch information
Showing
15 changed files
with
798 additions
and
320 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,112 @@ | ||
/* ---------------------------------------------------------------------------- | ||
* GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||
* Atlanta, Georgia 30332-0415 | ||
* All Rights Reserved | ||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||
* See LICENSE for the license information | ||
* -------------------------------------------------------------------------- */ | ||
|
||
/** | ||
* @file HybridSmoother.cpp | ||
* @brief An incremental smoother for hybrid factor graphs | ||
* @author Varun Agrawal | ||
* @date October 2022 | ||
*/ | ||
|
||
#include <gtsam/hybrid/HybridSmoother.h> | ||
|
||
#include <algorithm> | ||
#include <unordered_set> | ||
|
||
namespace gtsam { | ||
|
||
/* ************************************************************************* */ | ||
void HybridSmoother::update(HybridGaussianFactorGraph graph, | ||
const Ordering &ordering, | ||
boost::optional<size_t> maxNrLeaves) { | ||
// Add the necessary conditionals from the previous timestep(s). | ||
std::tie(graph, hybridBayesNet_) = | ||
addConditionals(graph, hybridBayesNet_, ordering); | ||
|
||
// Eliminate. | ||
auto bayesNetFragment = graph.eliminateSequential(ordering); | ||
|
||
/// Prune | ||
if (maxNrLeaves) { | ||
// `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in | ||
// all the conditionals with the same keys in bayesNetFragment. | ||
HybridBayesNet prunedBayesNetFragment = | ||
bayesNetFragment->prune(*maxNrLeaves); | ||
// Set the bayes net fragment to the pruned version | ||
bayesNetFragment = | ||
boost::make_shared<HybridBayesNet>(prunedBayesNetFragment); | ||
} | ||
|
||
// Add the partial bayes net to the posterior bayes net. | ||
hybridBayesNet_.push_back<HybridBayesNet>(*bayesNetFragment); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
std::pair<HybridGaussianFactorGraph, HybridBayesNet> | ||
HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, | ||
const HybridBayesNet &originalHybridBayesNet, | ||
const Ordering &ordering) const { | ||
HybridGaussianFactorGraph graph(originalGraph); | ||
HybridBayesNet hybridBayesNet(originalHybridBayesNet); | ||
|
||
// If we are not at the first iteration, means we have conditionals to add. | ||
if (!hybridBayesNet.empty()) { | ||
// We add all relevant conditional mixtures on the last continuous variable | ||
// in the previous `hybridBayesNet` to the graph | ||
|
||
// Conditionals to remove from the bayes net | ||
// since the conditional will be updated. | ||
std::vector<HybridConditional::shared_ptr> conditionals_to_erase; | ||
|
||
// New conditionals to add to the graph | ||
gtsam::HybridBayesNet newConditionals; | ||
|
||
// NOTE(Varun) Using a for-range loop doesn't work since some of the | ||
// conditionals are invalid pointers | ||
for (size_t i = 0; i < hybridBayesNet.size(); i++) { | ||
auto conditional = hybridBayesNet.at(i); | ||
|
||
for (auto &key : conditional->frontals()) { | ||
if (std::find(ordering.begin(), ordering.end(), key) != | ||
ordering.end()) { | ||
newConditionals.push_back(conditional); | ||
conditionals_to_erase.push_back(conditional); | ||
|
||
break; | ||
} | ||
} | ||
} | ||
// Remove conditionals at the end so we don't affect the order in the | ||
// original bayes net. | ||
for (auto &&conditional : conditionals_to_erase) { | ||
auto it = find(hybridBayesNet.begin(), hybridBayesNet.end(), conditional); | ||
hybridBayesNet.erase(it); | ||
} | ||
|
||
graph.push_back(newConditionals); | ||
// newConditionals.print("\n\n\nNew Conditionals to add back"); | ||
} | ||
return {graph, hybridBayesNet}; | ||
} | ||
|
||
/* ************************************************************************* */ | ||
GaussianMixture::shared_ptr HybridSmoother::gaussianMixture( | ||
size_t index) const { | ||
return boost::dynamic_pointer_cast<GaussianMixture>( | ||
hybridBayesNet_.at(index)); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
const HybridBayesNet &HybridSmoother::hybridBayesNet() const { | ||
return hybridBayesNet_; | ||
} | ||
|
||
} // namespace gtsam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,73 @@ | ||
/* ---------------------------------------------------------------------------- | ||
* GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||
* Atlanta, Georgia 30332-0415 | ||
* All Rights Reserved | ||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||
* See LICENSE for the license information | ||
* -------------------------------------------------------------------------- */ | ||
|
||
/** | ||
* @file HybridSmoother.h | ||
* @brief An incremental smoother for hybrid factor graphs | ||
* @author Varun Agrawal | ||
* @date October 2022 | ||
*/ | ||
|
||
#include <gtsam/discrete/DiscreteFactorGraph.h> | ||
#include <gtsam/hybrid/HybridBayesNet.h> | ||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> | ||
|
||
namespace gtsam { | ||
|
||
class HybridSmoother { | ||
private: | ||
HybridBayesNet hybridBayesNet_; | ||
HybridGaussianFactorGraph remainingFactorGraph_; | ||
|
||
public: | ||
/** | ||
* Given new factors, perform an incremental update. | ||
* The relevant densities in the `hybridBayesNet` will be added to the input | ||
* graph (fragment), and then eliminated according to the `ordering` | ||
* presented. The remaining factor graph contains Gaussian mixture factors | ||
* that are not connected to the variables in the ordering, or a single | ||
* discrete factor on all discrete keys, plus all discrete factors in the | ||
* original graph. | ||
* | ||
* \note If maxComponents is given, we look at the discrete factor resulting | ||
* from this elimination, and prune it and the Gaussian components | ||
* corresponding to the pruned choices. | ||
* | ||
* @param graph The new factors, should be linear only | ||
* @param ordering The ordering for elimination, only continuous vars are | ||
* allowed | ||
* @param maxNrLeaves The maximum number of leaves in the new discrete factor, | ||
* if applicable | ||
*/ | ||
void update(HybridGaussianFactorGraph graph, const Ordering& ordering, | ||
boost::optional<size_t> maxNrLeaves = boost::none); | ||
|
||
/** | ||
* @brief Add conditionals from previous timestep as part of liquefication. | ||
* | ||
* @param graph The new factor graph for the current time step. | ||
* @param hybridBayesNet The hybrid bayes net containing all conditionals so | ||
* far. | ||
* @param ordering The elimination ordering. | ||
* @return std::pair<HybridGaussianFactorGraph, HybridBayesNet> | ||
*/ | ||
std::pair<HybridGaussianFactorGraph, HybridBayesNet> addConditionals( | ||
const HybridGaussianFactorGraph& graph, | ||
const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const; | ||
|
||
/// Get the Gaussian Mixture from the Bayes Net posterior at `index`. | ||
GaussianMixture::shared_ptr gaussianMixture(size_t index) const; | ||
|
||
/// Return the Bayes Net posterior. | ||
const HybridBayesNet& hybridBayesNet() const; | ||
}; | ||
|
||
}; // namespace gtsam |
Oops, something went wrong.