Merge pull request #47 from varunagrawal/feature/GaussianHybridFactorGraph
Varun Agrawal
GitHub
commit
709bbb0c35
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianMixtureConditional.cpp
|
||||
* @file GaussianMixture.cpp
|
||||
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -20,41 +20,41 @@
|
|||
|
||||
#include <gtsam/base/utilities.h>
|
||||
#include <gtsam/discrete/DecisionTree-inl.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/inference/Conditional-inst.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
GaussianMixtureConditional::GaussianMixtureConditional(
|
||||
GaussianMixture::GaussianMixture(
|
||||
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const GaussianMixtureConditional::Conditionals &conditionals)
|
||||
const GaussianMixture::Conditionals &conditionals)
|
||||
: BaseFactor(CollectKeys(continuousFrontals, continuousParents),
|
||||
discreteParents),
|
||||
BaseConditional(continuousFrontals.size()),
|
||||
conditionals_(conditionals) {}
|
||||
|
||||
/* *******************************************************************************/
|
||||
const GaussianMixtureConditional::Conditionals &
|
||||
GaussianMixtureConditional::conditionals() {
|
||||
const GaussianMixture::Conditionals &
|
||||
GaussianMixture::conditionals() {
|
||||
return conditionals_;
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixtureConditional GaussianMixtureConditional::FromConditionals(
|
||||
GaussianMixture GaussianMixture::FromConditionals(
|
||||
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
|
||||
Conditionals dt(discreteParents, conditionalsList);
|
||||
|
||||
return GaussianMixtureConditional(continuousFrontals, continuousParents,
|
||||
return GaussianMixture(continuousFrontals, continuousParents,
|
||||
discreteParents, dt);
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
|
||||
const GaussianMixtureConditional::Sum &sum) const {
|
||||
GaussianMixture::Sum GaussianMixture::add(
|
||||
const GaussianMixture::Sum &sum) const {
|
||||
using Y = GaussianFactorGraph;
|
||||
auto add = [](const Y &graph1, const Y &graph2) {
|
||||
auto result = graph1;
|
||||
|
|
@ -66,8 +66,8 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixtureConditional::Sum
|
||||
GaussianMixtureConditional::asGaussianFactorGraphTree() const {
|
||||
GaussianMixture::Sum
|
||||
GaussianMixture::asGaussianFactorGraphTree() const {
|
||||
auto lambda = [](const GaussianFactor::shared_ptr &factor) {
|
||||
GaussianFactorGraph result;
|
||||
result.push_back(factor);
|
||||
|
|
@ -77,14 +77,14 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
bool GaussianMixtureConditional::equals(const HybridFactor &lf,
|
||||
bool GaussianMixture::equals(const HybridFactor &lf,
|
||||
double tol) const {
|
||||
const This *e = dynamic_cast<const This *>(&lf);
|
||||
return e != nullptr && BaseFactor::equals(*e, tol);
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
void GaussianMixtureConditional::print(const std::string &s,
|
||||
void GaussianMixture::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << s;
|
||||
if (isContinuous()) std::cout << "Continuous ";
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianMixtureConditional.h
|
||||
* @file GaussianMixture.h
|
||||
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -27,20 +27,26 @@
|
|||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief A conditional of gaussian mixtures indexed by discrete variables.
|
||||
* @brief A conditional of gaussian mixtures indexed by discrete variables, as
|
||||
* part of a Bayes Network.
|
||||
*
|
||||
* Represents the conditional density P(X | M, Z) where X is a continuous random
|
||||
* variable, M is the discrete variable and Z is the set of measurements.
|
||||
* variable, M is the selection of discrete variables corresponding to a subset
|
||||
* of the Gaussian variables and Z is parent of this node
|
||||
*
|
||||
* The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2}
|
||||
* |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing
|
||||
* coefficient.
|
||||
*
|
||||
*/
|
||||
class GaussianMixtureConditional
|
||||
class GTSAM_EXPORT GaussianMixture
|
||||
: public HybridFactor,
|
||||
public Conditional<HybridFactor, GaussianMixtureConditional> {
|
||||
public Conditional<HybridFactor, GaussianMixture> {
|
||||
public:
|
||||
using This = GaussianMixtureConditional;
|
||||
using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
|
||||
using This = GaussianMixture;
|
||||
using shared_ptr = boost::shared_ptr<GaussianMixture>;
|
||||
using BaseFactor = HybridFactor;
|
||||
using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
|
||||
using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
|
||||
|
||||
/// Alias for DecisionTree of GaussianFactorGraphs
|
||||
using Sum = DecisionTree<Key, GaussianFactorGraph>;
|
||||
|
|
@ -61,16 +67,20 @@ class GaussianMixtureConditional
|
|||
/// @{
|
||||
|
||||
/// Defaut constructor, mainly for serialization.
|
||||
GaussianMixtureConditional() = default;
|
||||
GaussianMixture() = default;
|
||||
|
||||
/**
|
||||
* @brief Construct a new GaussianMixtureConditional object
|
||||
* @brief Construct a new GaussianMixture object.
|
||||
*
|
||||
* @param continuousFrontals the continuous frontals.
|
||||
* @param continuousParents the continuous parents.
|
||||
* @param discreteParents the discrete parents. Will be placed last.
|
||||
* @param conditionals a decision tree of GaussianConditionals.
|
||||
* @param conditionals a decision tree of GaussianConditionals. The number of
|
||||
* conditionals should be C^(number of discrete parents), where C is the
|
||||
* cardinality of the DiscreteKeys in discreteParents, since the
|
||||
* discreteParents will be used as the labels in the decision tree.
|
||||
*/
|
||||
GaussianMixtureConditional(const KeyVector &continuousFrontals,
|
||||
GaussianMixture(const KeyVector &continuousFrontals,
|
||||
const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const Conditionals &conditionals);
|
||||
|
|
@ -97,7 +107,7 @@ class GaussianMixtureConditional
|
|||
|
||||
/* print utility */
|
||||
void print(
|
||||
const std::string &s = "GaussianMixtureConditional\n",
|
||||
const std::string &s = "GaussianMixture\n",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// @}
|
||||
|
|
@ -11,7 +11,8 @@
|
|||
|
||||
/**
|
||||
* @file HybridBayesTree.cpp
|
||||
* @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
|
||||
* @brief Hybrid Bayes Tree, the result of eliminating a
|
||||
* HybridGaussianJunctionTree
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
|
@ -25,7 +26,8 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
template class BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>;
|
||||
template class BayesTreeCliqueBase<HybridBayesTreeClique,
|
||||
HybridGaussianFactorGraph>;
|
||||
template class BayesTree<HybridBayesTreeClique>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -11,7 +11,8 @@
|
|||
|
||||
/**
|
||||
* @file HybridBayesTree.h
|
||||
* @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
|
||||
* @brief Hybrid Bayes Tree, the result of eliminating a
|
||||
* HybridGaussianJunctionTree
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
|
@ -19,7 +20,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
|
|
@ -37,10 +38,12 @@ class VectorValues;
|
|||
* which is a HybridConditional internally.
|
||||
*/
|
||||
class GTSAM_EXPORT HybridBayesTreeClique
|
||||
: public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
|
||||
: public BayesTreeCliqueBase<HybridBayesTreeClique,
|
||||
HybridGaussianFactorGraph> {
|
||||
public:
|
||||
typedef HybridBayesTreeClique This;
|
||||
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> Base;
|
||||
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
|
||||
Base;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
HybridBayesTreeClique() {}
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ HybridConditional::HybridConditional(
|
|||
|
||||
/* ************************************************************************ */
|
||||
HybridConditional::HybridConditional(
|
||||
boost::shared_ptr<GaussianMixtureConditional> gaussianMixture)
|
||||
boost::shared_ptr<GaussianMixture> gaussianMixture)
|
||||
: BaseFactor(KeyVector(gaussianMixture->keys().begin(),
|
||||
gaussianMixture->keys().begin() +
|
||||
gaussianMixture->nrContinuous()),
|
||||
|
|
|
|||
|
|
@ -18,9 +18,9 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
|
@ -34,7 +34,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class HybridFactorGraph;
|
||||
class HybridGaussianFactorGraph;
|
||||
|
||||
/**
|
||||
* Hybrid Conditional Density
|
||||
|
|
@ -42,7 +42,7 @@ class HybridFactorGraph;
|
|||
* As a type-erased variant of:
|
||||
* - DiscreteConditional
|
||||
* - GaussianConditional
|
||||
* - GaussianMixtureConditional
|
||||
* - GaussianMixture
|
||||
*
|
||||
* The reason why this is important is that `Conditional<T>` is a CRTP class.
|
||||
* CRTP is static polymorphism such that all CRTP classes, while bearing the
|
||||
|
|
@ -128,16 +128,16 @@ class GTSAM_EXPORT HybridConditional
|
|||
* HybridConditional.
|
||||
*/
|
||||
HybridConditional(
|
||||
boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
|
||||
boost::shared_ptr<GaussianMixture> gaussianMixture);
|
||||
|
||||
/**
|
||||
* @brief Return HybridConditional as a GaussianMixtureConditional
|
||||
* @brief Return HybridConditional as a GaussianMixture
|
||||
*
|
||||
* @return GaussianMixtureConditional::shared_ptr
|
||||
* @return GaussianMixture::shared_ptr
|
||||
*/
|
||||
GaussianMixtureConditional::shared_ptr asMixture() {
|
||||
GaussianMixture::shared_ptr asMixture() {
|
||||
if (!isHybrid()) throw std::invalid_argument("Not a mixture");
|
||||
return boost::static_pointer_cast<GaussianMixtureConditional>(inner_);
|
||||
return boost::static_pointer_cast<GaussianMixture>(inner_);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -146,7 +146,8 @@ class GTSAM_EXPORT HybridConditional
|
|||
* @return DiscreteConditional::shared_ptr
|
||||
*/
|
||||
DiscreteConditional::shared_ptr asDiscreteConditional() {
|
||||
if (!isDiscrete()) throw std::invalid_argument("Not a discrete conditional");
|
||||
if (!isDiscrete())
|
||||
throw std::invalid_argument("Not a discrete conditional");
|
||||
return boost::static_pointer_cast<DiscreteConditional>(inner_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -21,17 +21,17 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
template class EliminationTree<HybridBayesNet, HybridFactorGraph>;
|
||||
template class EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridEliminationTree::HybridEliminationTree(
|
||||
const HybridFactorGraph& factorGraph, const VariableIndex& structure,
|
||||
const Ordering& order)
|
||||
const HybridGaussianFactorGraph& factorGraph,
|
||||
const VariableIndex& structure, const Ordering& order)
|
||||
: Base(factorGraph, structure, order) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridEliminationTree::HybridEliminationTree(
|
||||
const HybridFactorGraph& factorGraph, const Ordering& order)
|
||||
const HybridGaussianFactorGraph& factorGraph, const Ordering& order)
|
||||
: Base(factorGraph, order) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -27,12 +27,12 @@ namespace gtsam {
|
|||
* Elimination Tree type for Hybrid
|
||||
*/
|
||||
class GTSAM_EXPORT HybridEliminationTree
|
||||
: public EliminationTree<HybridBayesNet, HybridFactorGraph> {
|
||||
: public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
|
||||
private:
|
||||
friend class ::EliminationTreeTester;
|
||||
|
||||
public:
|
||||
typedef EliminationTree<HybridBayesNet, HybridFactorGraph>
|
||||
typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
|
||||
Base; ///< Base class
|
||||
typedef HybridEliminationTree This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
|
@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree
|
|||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
HybridEliminationTree(const HybridFactorGraph& factorGraph,
|
||||
HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
|
||||
const VariableIndex& structure, const Ordering& order);
|
||||
|
||||
/** Build the elimination tree of a factor graph. Note that this has to
|
||||
|
|
@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree
|
|||
* this precomputed, use the other constructor instead.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
*/
|
||||
HybridEliminationTree(const HybridFactorGraph& factorGraph,
|
||||
HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
|
||||
const Ordering& order);
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridFactorGraph.cpp
|
||||
* @file HybridGaussianFactorGraph.cpp
|
||||
* @brief Hybrid factor graph that uses type erasure
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -23,15 +23,15 @@
|
|||
#include <gtsam/discrete/DiscreteEliminationTree.h>
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteJunctionTree.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridDiscreteFactor.h>
|
||||
#include <gtsam/hybrid/HybridEliminationTree.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridJunctionTree.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianJunctionTree.h>
|
||||
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
|
@ -53,7 +53,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
template class EliminateableFactorGraph<HybridFactorGraph>;
|
||||
template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
|
||||
|
||||
/* ************************************************************************ */
|
||||
static GaussianMixtureFactor::Sum &addGaussian(
|
||||
|
|
@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian(
|
|||
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
|
||||
continuousElimination(const HybridFactorGraph &factors,
|
||||
continuousElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
GaussianFactorGraph gfg;
|
||||
for (auto &fp : factors) {
|
||||
|
|
@ -103,7 +103,7 @@ continuousElimination(const HybridFactorGraph &factors,
|
|||
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
|
||||
discreteElimination(const HybridFactorGraph &factors,
|
||||
discreteElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
DiscreteFactorGraph dfg;
|
||||
for (auto &fp : factors) {
|
||||
|
|
@ -129,7 +129,8 @@ discreteElimination(const HybridFactorGraph &factors,
|
|||
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
|
||||
hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
|
||||
hybridElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys,
|
||||
const KeySet &continuousSeparator,
|
||||
const std::set<DiscreteKey> &discreteSeparatorSet) {
|
||||
// NOTE: since we use the special JunctionTree,
|
||||
|
|
@ -206,8 +207,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
|
|||
|
||||
const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
|
||||
|
||||
// Create the GaussianMixtureConditional from the conditionals
|
||||
auto conditional = boost::make_shared<GaussianMixtureConditional>(
|
||||
// Create the GaussianMixture from the conditionals
|
||||
auto conditional = boost::make_shared<GaussianMixture>(
|
||||
frontalKeys, keysOfSeparator, discreteSeparator, pair.first);
|
||||
|
||||
// If there are no more continuous parents, then we should create here a
|
||||
|
|
@ -235,7 +236,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
|
|||
}
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> //
|
||||
EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
|
||||
EliminateHybrid(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
// NOTE: Because we are in the Conditional Gaussian regime there are only
|
||||
// a few cases:
|
||||
// 1. continuous variable, make a Gaussian Mixture if there are hybrid
|
||||
|
|
@ -260,7 +262,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
|
|||
// Because of all these reasons, we carefully consider how to
|
||||
// implement the hybrid factors so that we do not get poor performance.
|
||||
|
||||
// The first thing is how to represent the GaussianMixtureConditional.
|
||||
// The first thing is how to represent the GaussianMixture.
|
||||
// A very possible scenario is that the incoming factors will have different
|
||||
// levels of discrete keys. For example, imagine we are going to eliminate the
|
||||
// fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid.
|
||||
|
|
@ -343,22 +345,22 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
|
|||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridFactorGraph::add(JacobianFactor &&factor) {
|
||||
void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
|
||||
void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridFactorGraph::add(DecisionTreeFactor &&factor) {
|
||||
void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
|
||||
void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
|
||||
}
|
||||
|
||||
|
|
@ -10,8 +10,8 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridFactorGraph.h
|
||||
* @brief Hybrid factor graph that uses type erasure
|
||||
* @file HybridGaussianFactorGraph.h
|
||||
* @brief Linearized Hybrid factor graph that uses type erasure
|
||||
* @author Fan Jiang
|
||||
* @date Mar 11, 2022
|
||||
*/
|
||||
|
|
@ -26,35 +26,37 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class HybridFactorGraph;
|
||||
class HybridGaussianFactorGraph;
|
||||
class HybridConditional;
|
||||
class HybridBayesNet;
|
||||
class HybridEliminationTree;
|
||||
class HybridBayesTree;
|
||||
class HybridJunctionTree;
|
||||
class HybridGaussianJunctionTree;
|
||||
class DecisionTreeFactor;
|
||||
|
||||
class JacobianFactor;
|
||||
|
||||
/** Main elimination function for HybridFactorGraph */
|
||||
/** Main elimination function for HybridGaussianFactorGraph */
|
||||
GTSAM_EXPORT
|
||||
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
|
||||
EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
|
||||
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <>
|
||||
struct EliminationTraits<HybridFactorGraph> {
|
||||
struct EliminationTraits<HybridGaussianFactorGraph> {
|
||||
typedef HybridFactor FactorType; ///< Type of factors in factor graph
|
||||
typedef HybridFactorGraph
|
||||
FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph)
|
||||
typedef HybridGaussianFactorGraph
|
||||
FactorGraphType; ///< Type of the factor graph (e.g.
|
||||
///< HybridGaussianFactorGraph)
|
||||
typedef HybridConditional
|
||||
ConditionalType; ///< Type of conditionals from elimination
|
||||
typedef HybridBayesNet
|
||||
BayesNetType; ///< Type of Bayes net from sequential elimination
|
||||
typedef HybridEliminationTree
|
||||
EliminationTreeType; ///< Type of elimination tree
|
||||
typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree
|
||||
typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree
|
||||
EliminationTreeType; ///< Type of elimination tree
|
||||
typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree
|
||||
typedef HybridGaussianJunctionTree
|
||||
JunctionTreeType; ///< Type of Junction tree
|
||||
/// The default dense elimination function
|
||||
static std::pair<boost::shared_ptr<ConditionalType>,
|
||||
boost::shared_ptr<FactorType> >
|
||||
|
|
@ -64,16 +66,17 @@ struct EliminationTraits<HybridFactorGraph> {
|
|||
};
|
||||
|
||||
/**
|
||||
* Hybrid Factor Graph
|
||||
* Gaussian Hybrid Factor Graph
|
||||
* -----------------------
|
||||
* This is the linear version of a hybrid factor graph. Everything inside needs
|
||||
* to be hybrid factor or hybrid conditional.
|
||||
* This is the linearized version of a hybrid factor graph.
|
||||
* Everything inside needs to be hybrid factor or hybrid conditional.
|
||||
*/
|
||||
class HybridFactorGraph : public FactorGraph<HybridFactor>,
|
||||
public EliminateableFactorGraph<HybridFactorGraph> {
|
||||
class HybridGaussianFactorGraph
|
||||
: public FactorGraph<HybridFactor>,
|
||||
public EliminateableFactorGraph<HybridGaussianFactorGraph> {
|
||||
public:
|
||||
using Base = FactorGraph<HybridFactor>;
|
||||
using This = HybridFactorGraph; ///< this class
|
||||
using This = HybridGaussianFactorGraph; ///< this class
|
||||
using BaseEliminateable =
|
||||
EliminateableFactorGraph<This>; ///< for elimination
|
||||
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
|
||||
|
|
@ -84,7 +87,7 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
|
|||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
HybridFactorGraph() = default;
|
||||
HybridGaussianFactorGraph() = default;
|
||||
|
||||
/**
|
||||
* Implicit copy/downcast constructor to override explicit template container
|
||||
|
|
@ -92,7 +95,8 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
|
|||
* `cachedSeparatorMarginal_.reset(*separatorMarginal)`
|
||||
* */
|
||||
template <class DERIVEDFACTOR>
|
||||
HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
|
||||
HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
|
||||
: Base(graph) {}
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridISAM.h
|
||||
* @file HybridGaussianISAM.h
|
||||
* @date March 31, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -18,8 +18,8 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridISAM.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/inference/ISAM-inst.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
|
|
@ -31,15 +31,17 @@ namespace gtsam {
|
|||
// template class ISAM<HybridBayesTree>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridISAM::HybridISAM() {}
|
||||
HybridGaussianISAM::HybridGaussianISAM() {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
|
||||
HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
|
||||
: Base(bayesTree) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
|
||||
HybridBayesTree::Cliques* orphans,
|
||||
const HybridBayesTree::Eliminate& function) {
|
||||
void HybridGaussianISAM::updateInternal(
|
||||
const HybridGaussianFactorGraph& newFactors,
|
||||
HybridBayesTree::Cliques* orphans,
|
||||
const HybridBayesTree::Eliminate& function) {
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
BayesNetType bn;
|
||||
const KeySet newFactorKeys = newFactors.keys();
|
||||
|
|
@ -90,8 +92,8 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HybridISAM::update(const HybridFactorGraph& newFactors,
|
||||
const HybridBayesTree::Eliminate& function) {
|
||||
void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
|
||||
const HybridBayesTree::Eliminate& function) {
|
||||
Cliques orphans;
|
||||
this->updateInternal(newFactors, &orphans, function);
|
||||
}
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridISAM.h
|
||||
* @file HybridGaussianISAM.h
|
||||
* @date March 31, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -21,32 +21,33 @@
|
|||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/ISAM.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
|
||||
class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
|
||||
public:
|
||||
typedef ISAM<HybridBayesTree> Base;
|
||||
typedef HybridISAM This;
|
||||
typedef HybridGaussianISAM This;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
HybridISAM();
|
||||
HybridGaussianISAM();
|
||||
|
||||
/** Copy constructor */
|
||||
HybridISAM(const HybridBayesTree& bayesTree);
|
||||
HybridGaussianISAM(const HybridBayesTree& bayesTree);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/// Internal method that performs the ISAM update.
|
||||
void updateInternal(
|
||||
const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
|
||||
const HybridGaussianFactorGraph& newFactors,
|
||||
HybridBayesTree::Cliques* orphans,
|
||||
const HybridBayesTree::Eliminate& function =
|
||||
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
|
||||
|
||||
|
|
@ -57,13 +58,13 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
|
|||
* @param newFactors Factor graph of new factors to add and eliminate.
|
||||
* @param function Elimination function.
|
||||
*/
|
||||
void update(const HybridFactorGraph& newFactors,
|
||||
void update(const HybridGaussianFactorGraph& newFactors,
|
||||
const HybridBayesTree::Eliminate& function =
|
||||
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
|
||||
};
|
||||
|
||||
/// traits
|
||||
template <>
|
||||
struct traits<HybridISAM> : public Testable<HybridISAM> {};
|
||||
struct traits<HybridGaussianISAM> : public Testable<HybridGaussianISAM> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -10,14 +10,14 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridJunctionTree.cpp
|
||||
* @file HybridGaussianJunctionTree.cpp
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
||||
#include <gtsam/hybrid/HybridEliminationTree.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridJunctionTree.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianJunctionTree.h>
|
||||
#include <gtsam/inference/JunctionTree-inst.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
|
|
@ -26,16 +26,20 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Instantiate base classes
|
||||
template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>;
|
||||
template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
|
||||
template class EliminatableClusterTree<HybridBayesTree,
|
||||
HybridGaussianFactorGraph>;
|
||||
template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
|
||||
|
||||
struct HybridConstructorTraversalData {
|
||||
typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::Node Node;
|
||||
typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::sharedNode
|
||||
sharedNode;
|
||||
typedef
|
||||
typename JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>::Node
|
||||
Node;
|
||||
typedef
|
||||
typename JunctionTree<HybridBayesTree,
|
||||
HybridGaussianFactorGraph>::sharedNode sharedNode;
|
||||
|
||||
HybridConstructorTraversalData* const parentData;
|
||||
sharedNode myJTNode;
|
||||
sharedNode junctionTreeNode;
|
||||
FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
|
||||
FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
|
||||
KeySet discreteKeys;
|
||||
|
|
@ -53,24 +57,24 @@ struct HybridConstructorTraversalData {
|
|||
// On the pre-order pass, before children have been visited, we just set up
|
||||
// a traversal data structure with its own JT node, and create a child
|
||||
// pointer in its parent.
|
||||
HybridConstructorTraversalData myData =
|
||||
HybridConstructorTraversalData data =
|
||||
HybridConstructorTraversalData(&parentData);
|
||||
myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
|
||||
parentData.myJTNode->addChild(myData.myJTNode);
|
||||
data.junctionTreeNode = boost::make_shared<Node>(node->key, node->factors);
|
||||
parentData.junctionTreeNode->addChild(data.junctionTreeNode);
|
||||
|
||||
for (HybridFactor::shared_ptr& f : node->factors) {
|
||||
for (auto& k : f->discreteKeys()) {
|
||||
myData.discreteKeys.insert(k.first);
|
||||
data.discreteKeys.insert(k.first);
|
||||
}
|
||||
}
|
||||
|
||||
return myData;
|
||||
return data;
|
||||
}
|
||||
|
||||
// Post-order visitor function
|
||||
static void ConstructorTraversalVisitorPostAlg2(
|
||||
const boost::shared_ptr<HybridEliminationTree::Node>& ETreeNode,
|
||||
const HybridConstructorTraversalData& myData) {
|
||||
const HybridConstructorTraversalData& data) {
|
||||
// In this post-order visitor, we combine the symbolic elimination results
|
||||
// from the elimination tree children and symbolically eliminate the current
|
||||
// elimination tree node. We then check whether each of our elimination
|
||||
|
|
@ -83,50 +87,50 @@ struct HybridConstructorTraversalData {
|
|||
// Do symbolic elimination for this node
|
||||
SymbolicFactors symbolicFactors;
|
||||
symbolicFactors.reserve(ETreeNode->factors.size() +
|
||||
myData.childSymbolicFactors.size());
|
||||
data.childSymbolicFactors.size());
|
||||
// Add ETree node factors
|
||||
symbolicFactors += ETreeNode->factors;
|
||||
// Add symbolic factors passed up from children
|
||||
symbolicFactors += myData.childSymbolicFactors;
|
||||
symbolicFactors += data.childSymbolicFactors;
|
||||
|
||||
Ordering keyAsOrdering;
|
||||
keyAsOrdering.push_back(ETreeNode->key);
|
||||
SymbolicConditional::shared_ptr myConditional;
|
||||
SymbolicFactor::shared_ptr mySeparatorFactor;
|
||||
boost::tie(myConditional, mySeparatorFactor) =
|
||||
SymbolicConditional::shared_ptr conditional;
|
||||
SymbolicFactor::shared_ptr separatorFactor;
|
||||
boost::tie(conditional, separatorFactor) =
|
||||
internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
|
||||
|
||||
// Store symbolic elimination results in the parent
|
||||
myData.parentData->childSymbolicConditionals.push_back(myConditional);
|
||||
myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
|
||||
myData.parentData->discreteKeys.merge(myData.discreteKeys);
|
||||
data.parentData->childSymbolicConditionals.push_back(conditional);
|
||||
data.parentData->childSymbolicFactors.push_back(separatorFactor);
|
||||
data.parentData->discreteKeys.merge(data.discreteKeys);
|
||||
|
||||
sharedNode node = myData.myJTNode;
|
||||
sharedNode node = data.junctionTreeNode;
|
||||
const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
|
||||
myData.childSymbolicConditionals;
|
||||
node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size());
|
||||
data.childSymbolicConditionals;
|
||||
node->problemSize_ = (int)(conditional->size() * symbolicFactors.size());
|
||||
|
||||
// Merge our children if they are in our clique - if our conditional has
|
||||
// exactly one fewer parent than our child's conditional.
|
||||
const size_t myNrParents = myConditional->nrParents();
|
||||
const size_t nrParents = conditional->nrParents();
|
||||
const size_t nrChildren = node->nrChildren();
|
||||
assert(childConditionals.size() == nrChildren);
|
||||
|
||||
// decide which children to merge, as index into children
|
||||
std::vector<size_t> nrFrontals = node->nrFrontalsOfChildren();
|
||||
std::vector<size_t> nrChildrenFrontals = node->nrFrontalsOfChildren();
|
||||
std::vector<bool> merge(nrChildren, false);
|
||||
size_t myNrFrontals = 1;
|
||||
size_t nrFrontals = 1;
|
||||
for (size_t i = 0; i < nrChildren; i++) {
|
||||
// Check if we should merge the i^th child
|
||||
if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
|
||||
if (nrParents + nrFrontals == childConditionals[i]->nrParents()) {
|
||||
const bool myType =
|
||||
myData.discreteKeys.exists(myConditional->frontals()[0]);
|
||||
data.discreteKeys.exists(conditional->frontals()[0]);
|
||||
const bool theirType =
|
||||
myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
|
||||
data.discreteKeys.exists(childConditionals[i]->frontals()[0]);
|
||||
|
||||
if (myType == theirType) {
|
||||
// Increment number of frontal variables
|
||||
myNrFrontals += nrFrontals[i];
|
||||
nrFrontals += nrChildrenFrontals[i];
|
||||
merge[i] = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -138,7 +142,7 @@ struct HybridConstructorTraversalData {
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridJunctionTree::HybridJunctionTree(
|
||||
HybridGaussianJunctionTree::HybridGaussianJunctionTree(
|
||||
const HybridEliminationTree& eliminationTree) {
|
||||
gttic(JunctionTree_FromEliminationTree);
|
||||
// Here we rely on the BayesNet having been produced by this elimination tree,
|
||||
|
|
@ -152,7 +156,7 @@ HybridJunctionTree::HybridJunctionTree(
|
|||
// as we go. Gather the created junction tree roots in a dummy Node.
|
||||
typedef HybridConstructorTraversalData Data;
|
||||
Data rootData(0);
|
||||
rootData.myJTNode =
|
||||
rootData.junctionTreeNode =
|
||||
boost::make_shared<typename Base::Node>(); // Make a dummy node to gather
|
||||
// the junction tree roots
|
||||
treeTraversal::DepthFirstForest(eliminationTree, rootData,
|
||||
|
|
@ -160,7 +164,7 @@ HybridJunctionTree::HybridJunctionTree(
|
|||
Data::ConstructorTraversalVisitorPostAlg2);
|
||||
|
||||
// Assign roots from the dummy node
|
||||
this->addChildrenAsRoots(rootData.myJTNode);
|
||||
this->addChildrenAsRoots(rootData.junctionTreeNode);
|
||||
|
||||
// Transfer remaining factors from elimination tree
|
||||
Base::remainingFactors_ = eliminationTree.remainingFactors();
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridJunctionTree.h
|
||||
* @file HybridGaussianJunctionTree.h
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -48,12 +48,12 @@ class HybridEliminationTree;
|
|||
* \addtogroup Multifrontal
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT HybridJunctionTree
|
||||
: public JunctionTree<HybridBayesTree, HybridFactorGraph> {
|
||||
class GTSAM_EXPORT HybridGaussianJunctionTree
|
||||
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
|
||||
public:
|
||||
typedef JunctionTree<HybridBayesTree, HybridFactorGraph>
|
||||
typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
|
||||
Base; ///< Base class
|
||||
typedef HybridJunctionTree This; ///< This class
|
||||
typedef HybridGaussianJunctionTree This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
||||
/**
|
||||
|
|
@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridJunctionTree
|
|||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
HybridJunctionTree(const HybridEliminationTree& eliminationTree);
|
||||
HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree);
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
|
|||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
||||
class GaussianMixtureConditional : gtsam::HybridFactor {
|
||||
static GaussianMixtureConditional FromConditionals(
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
class GaussianMixture : gtsam::HybridFactor {
|
||||
static GaussianMixture FromConditionals(
|
||||
const gtsam::KeyVector& continuousFrontals,
|
||||
const gtsam::KeyVector& continuousParents,
|
||||
const gtsam::DiscreteKeys& discreteParents,
|
||||
const std::vector<gtsam::GaussianConditional::shared_ptr>&
|
||||
conditionalsList);
|
||||
|
||||
void print(string s = "GaussianMixtureConditional\n",
|
||||
void print(string s = "GaussianMixture\n",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
|
@ -98,15 +98,15 @@ class HybridBayesNet {
|
|||
const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
|
||||
};
|
||||
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
class HybridFactorGraph {
|
||||
HybridFactorGraph();
|
||||
HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
class HybridGaussianFactorGraph {
|
||||
HybridGaussianFactorGraph();
|
||||
HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet);
|
||||
|
||||
// Building the graph
|
||||
void push_back(const gtsam::HybridFactor* factor);
|
||||
void push_back(const gtsam::HybridConditional* conditional);
|
||||
void push_back(const gtsam::HybridFactorGraph& graph);
|
||||
void push_back(const gtsam::HybridGaussianFactorGraph& graph);
|
||||
void push_back(const gtsam::HybridBayesNet& bayesNet);
|
||||
void push_back(const gtsam::HybridBayesTree& bayesTree);
|
||||
void push_back(const gtsam::GaussianMixtureFactor* gmm);
|
||||
|
|
@ -120,13 +120,13 @@ class HybridFactorGraph {
|
|||
const gtsam::HybridFactor* at(size_t i) const;
|
||||
|
||||
void print(string s = "") const;
|
||||
bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const;
|
||||
bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const;
|
||||
|
||||
gtsam::HybridBayesNet* eliminateSequential();
|
||||
gtsam::HybridBayesNet* eliminateSequential(
|
||||
gtsam::Ordering::OrderingType type);
|
||||
gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
|
||||
pair<gtsam::HybridBayesNet*, gtsam::HybridFactorGraph*>
|
||||
pair<gtsam::HybridBayesNet*, gtsam::HybridGaussianFactorGraph*>
|
||||
eliminatePartialSequential(const gtsam::Ordering& ordering);
|
||||
|
||||
gtsam::HybridBayesTree* eliminateMultifrontal();
|
||||
|
|
@ -134,7 +134,7 @@ class HybridFactorGraph {
|
|||
gtsam::Ordering::OrderingType type);
|
||||
gtsam::HybridBayesTree* eliminateMultifrontal(
|
||||
const gtsam::Ordering& ordering);
|
||||
pair<gtsam::HybridBayesTree*, gtsam::HybridFactorGraph*>
|
||||
pair<gtsam::HybridBayesTree*, gtsam::HybridGaussianFactorGraph*>
|
||||
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
|
||||
|
||||
string dot(
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
|
|
@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C;
|
|||
using gtsam::symbol_shorthand::X;
|
||||
|
||||
namespace gtsam {
|
||||
inline HybridFactorGraph::shared_ptr makeSwitchingChain(
|
||||
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
|
||||
size_t n, std::function<Key(int)> keyFunc = X,
|
||||
std::function<Key(int)> dKeyFunc = C) {
|
||||
HybridFactorGraph hfg;
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
|
||||
|
||||
|
|
@ -51,7 +51,7 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain(
|
|||
}
|
||||
}
|
||||
|
||||
return boost::make_shared<HybridFactorGraph>(std::move(hfg));
|
||||
return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
|
||||
}
|
||||
|
||||
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testHybridFactorGraph.cpp
|
||||
* @file testHybridGaussianFactorGraph.cpp
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
|
@ -20,16 +20,16 @@
|
|||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridDiscreteFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridISAM.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/DotWriter.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
|
@ -57,31 +57,17 @@ using gtsam::symbol_shorthand::D;
|
|||
using gtsam::symbol_shorthand::X;
|
||||
using gtsam::symbol_shorthand::Y;
|
||||
|
||||
#ifdef HYBRID_DEBUG
|
||||
#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED
|
||||
|
||||
#include <signal.h> // ::signal, ::raise
|
||||
|
||||
#include <boost/stacktrace.hpp>
|
||||
|
||||
void my_signal_handler(int signum) {
|
||||
::signal(signum, SIG_DFL);
|
||||
std::cout << boost::stacktrace::stacktrace();
|
||||
::raise(SIGABRT);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, creation) {
|
||||
TEST(HybridGaussianFactorGraph, creation) {
|
||||
HybridConditional test;
|
||||
|
||||
HybridFactorGraph hfg;
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
|
||||
|
||||
GaussianMixtureConditional clgc(
|
||||
GaussianMixture clgc(
|
||||
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
|
||||
GaussianMixtureConditional::Conditionals(
|
||||
GaussianMixture::Conditionals(
|
||||
C(0),
|
||||
boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
|
||||
I_3x3),
|
||||
|
|
@ -91,8 +77,8 @@ TEST(HybridFactorGraph, creation) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, eliminate) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminate) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
|
||||
|
||||
|
|
@ -102,8 +88,8 @@ TEST(HybridFactorGraph, eliminate) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, eliminateMultifrontal) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c(C(1), 2);
|
||||
|
||||
|
|
@ -119,8 +105,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c1(C(1), 2);
|
||||
|
||||
|
|
@ -143,8 +129,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c1(C(1), 2);
|
||||
|
||||
|
|
@ -171,8 +157,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c1(C(1), 2);
|
||||
|
||||
|
|
@ -204,8 +190,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c(C(1), 2);
|
||||
|
||||
|
|
@ -240,8 +226,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
|
|||
* This test is about how to assemble the Bayes Tree roots after we do partial
|
||||
* elimination
|
||||
*/
|
||||
TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||
HybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
|
||||
|
|
@ -290,7 +276,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
HybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
GTSAM_PRINT(*hbt);
|
||||
|
|
@ -309,7 +295,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(HybridFactorGraph, Switching) {
|
||||
TEST(HybridGaussianFactorGraph, Switching) {
|
||||
auto N = 12;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
|
|
@ -381,7 +367,7 @@ TEST(HybridFactorGraph, Switching) {
|
|||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
HybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// GTSAM_PRINT(*hbt);
|
||||
|
|
@ -417,7 +403,7 @@ TEST(HybridFactorGraph, Switching) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(HybridFactorGraph, SwitchingISAM) {
|
||||
TEST(HybridGaussianFactorGraph, SwitchingISAM) {
|
||||
auto N = 11;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
|
|
@ -473,7 +459,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
|
|||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
HybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// GTSAM_PRINT(*hbt);
|
||||
|
|
@ -499,10 +485,10 @@ TEST(HybridFactorGraph, SwitchingISAM) {
|
|||
}
|
||||
|
||||
auto new_fg = makeSwitchingChain(12);
|
||||
auto isam = HybridISAM(*hbt);
|
||||
auto isam = HybridGaussianISAM(*hbt);
|
||||
|
||||
{
|
||||
HybridFactorGraph factorGraph;
|
||||
HybridGaussianFactorGraph factorGraph;
|
||||
factorGraph.push_back(new_fg->at(new_fg->size() - 2));
|
||||
factorGraph.push_back(new_fg->at(new_fg->size() - 1));
|
||||
isam.update(factorGraph);
|
||||
|
|
@ -512,7 +498,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridFactorGraph, SwitchingTwoVar) {
|
||||
TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
|
||||
const int N = 7;
|
||||
auto hfg = makeSwitchingChain(N, X);
|
||||
hfg->push_back(*makeSwitchingChain(N, Y, D));
|
||||
|
|
@ -582,7 +568,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) {
|
|||
}
|
||||
{
|
||||
HybridBayesNet::shared_ptr hbn;
|
||||
HybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbn, remaining) =
|
||||
hfg->eliminatePartialSequential(ordering_partial);
|
||||
|
||||
|
|
@ -9,7 +9,7 @@ namespace gtsam {
|
|||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
|
|
@ -107,36 +107,36 @@ class Ordering {
|
|||
|
||||
template <
|
||||
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
|
||||
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
|
||||
|
||||
template <
|
||||
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
|
||||
static gtsam::Ordering ColamdConstrainedLast(
|
||||
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
|
||||
bool forceOrder = false);
|
||||
|
||||
template <
|
||||
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
|
||||
static gtsam::Ordering ColamdConstrainedFirst(
|
||||
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
|
||||
bool forceOrder = false);
|
||||
|
||||
template <
|
||||
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
|
||||
static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
|
||||
|
||||
template <
|
||||
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
|
||||
static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
|
||||
|
||||
template <
|
||||
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
|
||||
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
|
||||
static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
|
||||
const FACTOR_GRAPH& graph);
|
||||
|
||||
|
|
|
|||
|
|
@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X
|
|||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestHybridFactorGraph(GtsamTestCase):
|
||||
"""Unit tests for HybridFactorGraph."""
|
||||
class TestHybridGaussianFactorGraph(GtsamTestCase):
|
||||
"""Unit tests for HybridGaussianFactorGraph."""
|
||||
|
||||
def test_create(self):
|
||||
"""Test contruction of hybrid factor graph."""
|
||||
|
|
@ -36,13 +36,14 @@ class TestHybridFactorGraph(GtsamTestCase):
|
|||
|
||||
gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
|
||||
|
||||
hfg = gtsam.HybridFactorGraph()
|
||||
hfg = gtsam.HybridGaussianFactorGraph()
|
||||
hfg.add(jf1)
|
||||
hfg.add(jf2)
|
||||
hfg.push_back(gmf)
|
||||
|
||||
hbn = hfg.eliminateSequential(
|
||||
gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)]))
|
||||
gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
|
||||
hfg, [C(0)]))
|
||||
|
||||
# print("hbn = ", hbn)
|
||||
self.assertEqual(hbn.size(), 2)
|
||||
|
|
|
|||
Loading…
Reference in New Issue