12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

rename HybridFactorGraph to GaussianHybridFactorGraph

release/4.3a0
Varun Agrawal 2022-05-28 14:24:03 -04:00
parent c3a59cfd62
commit 852a9b9ef6
16 changed files with 122 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/hybrid/HybridFactorGraph.cpp → gtsam/hybrid/GaussianHybridFactorGraph.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file HybridFactorGraph.cpp * @file GaussianHybridFactorGraph.cpp
* @brief Hybrid factor graph that uses type erasure * @brief Hybrid factor graph that uses type erasure
* @author Fan Jiang * @author Fan Jiang
* @author Varun Agrawal * @author Varun Agrawal
@ -23,13 +23,13 @@
#include <gtsam/discrete/DiscreteEliminationTree.h> #include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h> #include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h> #include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h> #include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridEliminationTree.h> #include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridJunctionTree.h> #include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h> #include <gtsam/inference/EliminateableFactorGraph-inst.h>
@ -53,7 +53,7 @@
namespace gtsam { namespace gtsam {
template class EliminateableFactorGraph<HybridFactorGraph>; template class EliminateableFactorGraph<GaussianHybridFactorGraph>;
/* ************************************************************************ */ /* ************************************************************************ */
static GaussianMixtureFactor::Sum &addGaussian( static GaussianMixtureFactor::Sum &addGaussian(
@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian(
/* ************************************************************************ */ /* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
continuousElimination(const HybridFactorGraph &factors, continuousElimination(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
for (auto &fp : factors) { for (auto &fp : factors) {
@ -103,7 +103,7 @@ continuousElimination(const HybridFactorGraph &factors,
/* ************************************************************************ */ /* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
discreteElimination(const HybridFactorGraph &factors, discreteElimination(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto &fp : factors) { for (auto &fp : factors) {
@ -129,7 +129,8 @@ discreteElimination(const HybridFactorGraph &factors,
/* ************************************************************************ */ /* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, hybridElimination(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys,
const KeySet &continuousSeparator, const KeySet &continuousSeparator,
const std::set<DiscreteKey> &discreteSeparatorSet) { const std::set<DiscreteKey> &discreteSeparatorSet) {
// NOTE: since we use the special JunctionTree, // NOTE: since we use the special JunctionTree,
@ -235,7 +236,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
} }
/* ************************************************************************ */ /* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> // std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> //
EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { EliminateHybrid(const GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys) {
// NOTE: Because we are in the Conditional Gaussian regime there are only // NOTE: Because we are in the Conditional Gaussian regime there are only
// a few cases: // a few cases:
// 1. continuous variable, make a Gaussian Mixture if there are hybrid // 1. continuous variable, make a Gaussian Mixture if there are hybrid
@ -343,22 +345,22 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
} }
/* ************************************************************************ */ /* ************************************************************************ */
void HybridFactorGraph::add(JacobianFactor &&factor) { void GaussianHybridFactorGraph::add(JacobianFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor))); FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
} }
/* ************************************************************************ */ /* ************************************************************************ */
void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) { void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor)); FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
} }
/* ************************************************************************ */ /* ************************************************************************ */
void HybridFactorGraph::add(DecisionTreeFactor &&factor) { void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor))); FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
} }
/* ************************************************************************ */ /* ************************************************************************ */
void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor)); FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
} }

35
gtsam/hybrid/HybridFactorGraph.h → gtsam/hybrid/GaussianHybridFactorGraph.h
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file HybridFactorGraph.h * @file GaussianHybridFactorGraph.h
* @brief Hybrid factor graph that uses type erasure * @brief Linearized Hybrid factor graph that uses type erasure
* @author Fan Jiang * @author Fan Jiang
* @date Mar 11, 2022 * @date Mar 11, 2022
*/ */
@ -26,7 +26,7 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class HybridFactorGraph; class GaussianHybridFactorGraph;
class HybridConditional; class HybridConditional;
class HybridBayesNet; class HybridBayesNet;
class HybridEliminationTree; class HybridEliminationTree;
@ -36,17 +36,18 @@ class DecisionTreeFactor;
class JacobianFactor; class JacobianFactor;
/** Main elimination function for HybridFactorGraph */ /** Main elimination function for GaussianHybridFactorGraph */
GTSAM_EXPORT GTSAM_EXPORT
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr> std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys);
/* ************************************************************************* */ /* ************************************************************************* */
template <> template <>
struct EliminationTraits<HybridFactorGraph> { struct EliminationTraits<GaussianHybridFactorGraph> {
typedef HybridFactor FactorType; ///< Type of factors in factor graph typedef HybridFactor FactorType; ///< Type of factors in factor graph
typedef HybridFactorGraph typedef GaussianHybridFactorGraph
FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) FactorGraphType; ///< Type of the factor graph (e.g.
///< GaussianHybridFactorGraph)
typedef HybridConditional typedef HybridConditional
ConditionalType; ///< Type of conditionals from elimination ConditionalType; ///< Type of conditionals from elimination
typedef HybridBayesNet typedef HybridBayesNet
@ -64,16 +65,17 @@ struct EliminationTraits<HybridFactorGraph> {
}; };
/** /**
* Hybrid Factor Graph * Gaussian Hybrid Factor Graph
* ----------------------- * -----------------------
* This is the linear version of a hybrid factor graph. Everything inside needs * This is the linearized version of a hybrid factor graph.
* to be hybrid factor or hybrid conditional. * Everything inside needs to be hybrid factor or hybrid conditional.
*/ */
class HybridFactorGraph : public FactorGraph<HybridFactor>, class GaussianHybridFactorGraph
public EliminateableFactorGraph<HybridFactorGraph> { : public FactorGraph<HybridFactor>,
public EliminateableFactorGraph<GaussianHybridFactorGraph> {
public: public:
using Base = FactorGraph<HybridFactor>; using Base = FactorGraph<HybridFactor>;
using This = HybridFactorGraph; ///< this class using This = GaussianHybridFactorGraph; ///< this class
using BaseEliminateable = using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination EliminateableFactorGraph<This>; ///< for elimination
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
@ -84,7 +86,7 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
/// @name Constructors /// @name Constructors
/// @{ /// @{
HybridFactorGraph() = default; GaussianHybridFactorGraph() = default;
/** /**
* Implicit copy/downcast constructor to override explicit template container * Implicit copy/downcast constructor to override explicit template container
@ -92,7 +94,8 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
* `cachedSeparatorMarginal_.reset(*separatorMarginal)` * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
* */ * */
template <class DERIVEDFACTOR> template <class DERIVEDFACTOR>
HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} GaussianHybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
: Base(graph) {}
/// @} /// @}

3
gtsam/hybrid/HybridBayesTree.cpp
View File

@ -25,7 +25,8 @@
namespace gtsam { namespace gtsam {
// Instantiate base class // Instantiate base class
template class BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>; template class BayesTreeCliqueBase<HybridBayesTreeClique,
GaussianHybridFactorGraph>;
template class BayesTree<HybridBayesTreeClique>; template class BayesTree<HybridBayesTreeClique>;
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/hybrid/HybridBayesTree.h
View File

@ -18,8 +18,8 @@
#pragma once #pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/BayesTreeCliqueBase.h> #include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>
@ -37,10 +37,12 @@ class VectorValues;
* which is a HybridConditional internally. * which is a HybridConditional internally.
*/ */
class GTSAM_EXPORT HybridBayesTreeClique class GTSAM_EXPORT HybridBayesTreeClique
: public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> { : public BayesTreeCliqueBase<HybridBayesTreeClique,
GaussianHybridFactorGraph> {
public: public:
typedef HybridBayesTreeClique This; typedef HybridBayesTreeClique This;
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> Base; typedef BayesTreeCliqueBase<HybridBayesTreeClique, GaussianHybridFactorGraph>
Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr; typedef boost::weak_ptr<This> weak_ptr;
HybridBayesTreeClique() {} HybridBayesTreeClique() {}

7
gtsam/hybrid/HybridConditional.h
View File

@ -18,9 +18,9 @@
#pragma once #pragma once
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h> #include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
@ -34,7 +34,7 @@
namespace gtsam { namespace gtsam {
class HybridFactorGraph; class GaussianHybridFactorGraph;
/** /**
* Hybrid Conditional Density * Hybrid Conditional Density
@ -146,7 +146,8 @@ class GTSAM_EXPORT HybridConditional
* @return DiscreteConditional::shared_ptr * @return DiscreteConditional::shared_ptr
*/ */
DiscreteConditional::shared_ptr asDiscreteConditional() { 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_); return boost::static_pointer_cast<DiscreteConditional>(inner_);
} }

8
gtsam/hybrid/HybridEliminationTree.cpp
View File

@ -21,17 +21,17 @@
namespace gtsam { namespace gtsam {
// Instantiate base class // Instantiate base class
template class EliminationTree<HybridBayesNet, HybridFactorGraph>; template class EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>;
/* ************************************************************************* */ /* ************************************************************************* */
HybridEliminationTree::HybridEliminationTree( HybridEliminationTree::HybridEliminationTree(
const HybridFactorGraph& factorGraph, const VariableIndex& structure, const GaussianHybridFactorGraph& factorGraph,
const Ordering& order) const VariableIndex& structure, const Ordering& order)
: Base(factorGraph, structure, order) {} : Base(factorGraph, structure, order) {}
/* ************************************************************************* */ /* ************************************************************************* */
HybridEliminationTree::HybridEliminationTree( HybridEliminationTree::HybridEliminationTree(
const HybridFactorGraph& factorGraph, const Ordering& order) const GaussianHybridFactorGraph& factorGraph, const Ordering& order)
: Base(factorGraph, order) {} : Base(factorGraph, order) {}
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/hybrid/HybridEliminationTree.h
View File

@ -17,8 +17,8 @@
#pragma once #pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTree.h>
namespace gtsam { namespace gtsam {
@ -27,12 +27,12 @@ namespace gtsam {
* Elimination Tree type for Hybrid * Elimination Tree type for Hybrid
*/ */
class GTSAM_EXPORT HybridEliminationTree class GTSAM_EXPORT HybridEliminationTree
: public EliminationTree<HybridBayesNet, HybridFactorGraph> { : public EliminationTree<HybridBayesNet, GaussianHybridFactorGraph> {
private: private:
friend class ::EliminationTreeTester; friend class ::EliminationTreeTester;
public: public:
typedef EliminationTree<HybridBayesNet, HybridFactorGraph> typedef EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>
Base; ///< Base class Base; ///< Base class
typedef HybridEliminationTree This; ///< This class typedef HybridEliminationTree This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to 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. * named constructor instead.
* @return The elimination tree * @return The elimination tree
*/ */
HybridEliminationTree(const HybridFactorGraph& factorGraph, HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph,
const VariableIndex& structure, const Ordering& order); const VariableIndex& structure, const Ordering& order);
/** Build the elimination tree of a factor graph. Note that this has to /** 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. * this precomputed, use the other constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree * @param factorGraph The factor graph for which to build the elimination tree
*/ */
HybridEliminationTree(const HybridFactorGraph& factorGraph, HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph,
const Ordering& order); const Ordering& order);
/// @} /// @}

6
gtsam/hybrid/HybridISAM.cpp
View File

@ -17,8 +17,8 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridISAM.h> #include <gtsam/hybrid/HybridISAM.h>
#include <gtsam/inference/ISAM-inst.h> #include <gtsam/inference/ISAM-inst.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -37,7 +37,7 @@ HybridISAM::HybridISAM() {}
HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
/* ************************************************************************* */ /* ************************************************************************* */
void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans, HybridBayesTree::Cliques* orphans,
const HybridBayesTree::Eliminate& function) { const HybridBayesTree::Eliminate& function) {
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
@ -90,7 +90,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HybridISAM::update(const HybridFactorGraph& newFactors, void HybridISAM::update(const GaussianHybridFactorGraph& newFactors,
const HybridBayesTree::Eliminate& function) { const HybridBayesTree::Eliminate& function) {
Cliques orphans; Cliques orphans;
this->updateInternal(newFactors, &orphans, function); this->updateInternal(newFactors, &orphans, function);

7
gtsam/hybrid/HybridISAM.h
View File

@ -20,8 +20,8 @@
#pragma once #pragma once
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/ISAM.h> #include <gtsam/inference/ISAM.h>
namespace gtsam { namespace gtsam {
@ -46,7 +46,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
private: private:
/// Internal method that performs the ISAM update. /// Internal method that performs the ISAM update.
void updateInternal( void updateInternal(
const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const GaussianHybridFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const HybridBayesTree::Eliminate& function = const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate); HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@ -57,7 +58,7 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
* @param newFactors Factor graph of new factors to add and eliminate. * @param newFactors Factor graph of new factors to add and eliminate.
* @param function Elimination function. * @param function Elimination function.
*/ */
void update(const HybridFactorGraph& newFactors, void update(const GaussianHybridFactorGraph& newFactors,
const HybridBayesTree::Eliminate& function = const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate); HybridBayesTree::EliminationTraitsType::DefaultEliminate);
}; };

16
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -15,8 +15,8 @@
* @author Fan Jiang * @author Fan Jiang
*/ */
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridEliminationTree.h> #include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridJunctionTree.h> #include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/JunctionTree-inst.h> #include <gtsam/inference/JunctionTree-inst.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -26,13 +26,17 @@
namespace gtsam { namespace gtsam {
// Instantiate base classes // Instantiate base classes
template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>; template class EliminatableClusterTree<HybridBayesTree,
template class JunctionTree<HybridBayesTree, HybridFactorGraph>; GaussianHybridFactorGraph>;
template class JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>;
struct HybridConstructorTraversalData { struct HybridConstructorTraversalData {
typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::Node Node; typedef
typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::sharedNode typename JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>::Node
sharedNode; Node;
typedef
typename JunctionTree<HybridBayesTree,
GaussianHybridFactorGraph>::sharedNode sharedNode;
HybridConstructorTraversalData* const parentData; HybridConstructorTraversalData* const parentData;
sharedNode myJTNode; sharedNode myJTNode;

6
gtsam/hybrid/HybridJunctionTree.h
View File

@ -17,8 +17,8 @@
#pragma once #pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTree.h>
namespace gtsam { namespace gtsam {
@ -49,9 +49,9 @@ class HybridEliminationTree;
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT HybridJunctionTree class GTSAM_EXPORT HybridJunctionTree
: public JunctionTree<HybridBayesTree, HybridFactorGraph> { : public JunctionTree<HybridBayesTree, GaussianHybridFactorGraph> {
public: public:
typedef JunctionTree<HybridBayesTree, HybridFactorGraph> typedef JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>
Base; ///< Base class Base; ///< Base class
typedef HybridJunctionTree This; ///< This class typedef HybridJunctionTree This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class

16
gtsam/hybrid/hybrid.i
View File

@ -98,15 +98,15 @@ class HybridBayesNet {
const gtsam::DotWriter& writer = gtsam::DotWriter()) const; const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
}; };
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/GaussianHybridFactorGraph.h>
class HybridFactorGraph { class GaussianHybridFactorGraph {
HybridFactorGraph(); GaussianHybridFactorGraph();
HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
// Building the graph // Building the graph
void push_back(const gtsam::HybridFactor* factor); void push_back(const gtsam::HybridFactor* factor);
void push_back(const gtsam::HybridConditional* conditional); void push_back(const gtsam::HybridConditional* conditional);
void push_back(const gtsam::HybridFactorGraph& graph); void push_back(const gtsam::GaussianHybridFactorGraph& graph);
void push_back(const gtsam::HybridBayesNet& bayesNet); void push_back(const gtsam::HybridBayesNet& bayesNet);
void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::HybridBayesTree& bayesTree);
void push_back(const gtsam::GaussianMixtureFactor* gmm); void push_back(const gtsam::GaussianMixtureFactor* gmm);
@ -120,13 +120,13 @@ class HybridFactorGraph {
const gtsam::HybridFactor* at(size_t i) const; const gtsam::HybridFactor* at(size_t i) const;
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const; bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const;
gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential();
gtsam::HybridBayesNet* eliminateSequential( gtsam::HybridBayesNet* eliminateSequential(
gtsam::Ordering::OrderingType type); gtsam::Ordering::OrderingType type);
gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
pair<gtsam::HybridBayesNet*, gtsam::HybridFactorGraph*> pair<gtsam::HybridBayesNet*, gtsam::GaussianHybridFactorGraph*>
eliminatePartialSequential(const gtsam::Ordering& ordering); eliminatePartialSequential(const gtsam::Ordering& ordering);
gtsam::HybridBayesTree* eliminateMultifrontal(); gtsam::HybridBayesTree* eliminateMultifrontal();
@ -134,7 +134,7 @@ class HybridFactorGraph {
gtsam::Ordering::OrderingType type); gtsam::Ordering::OrderingType type);
gtsam::HybridBayesTree* eliminateMultifrontal( gtsam::HybridBayesTree* eliminateMultifrontal(
const gtsam::Ordering& ordering); const gtsam::Ordering& ordering);
pair<gtsam::HybridBayesTree*, gtsam::HybridFactorGraph*> pair<gtsam::HybridBayesTree*, gtsam::GaussianHybridFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering); eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
string dot( string dot(

8
gtsam/hybrid/tests/Switching.h
View File

@ -18,8 +18,8 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h> #include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C;
using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::X;
namespace gtsam { namespace gtsam {
inline HybridFactorGraph::shared_ptr makeSwitchingChain( inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X, size_t n, std::function<Key(int)> keyFunc = X,
std::function<Key(int)> dKeyFunc = C) { std::function<Key(int)> dKeyFunc = C) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); 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<GaussianHybridFactorGraph>(std::move(hfg));
} }
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering( inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(

52
gtsam/hybrid/tests/testHybridFactorGraph.cpp → gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* /*
* @file testHybridFactorGraph.cpp * @file testGaussianHybridFactorGraph.cpp
* @date Mar 11, 2022 * @date Mar 11, 2022
* @author Fan Jiang * @author Fan Jiang
*/ */
@ -20,6 +20,7 @@
#include <gtsam/discrete/DecisionTreeFactor.h> #include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h> #include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
@ -27,7 +28,6 @@
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h> #include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridISAM.h> #include <gtsam/hybrid/HybridISAM.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>
@ -72,10 +72,10 @@ void my_signal_handler(int signum) {
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, creation) { TEST(GaussianHybridFactorGraph, creation) {
HybridConditional test; HybridConditional test;
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@ -91,8 +91,8 @@ TEST(HybridFactorGraph, creation) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, eliminate) { TEST(GaussianHybridFactorGraph, eliminate) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@ -102,8 +102,8 @@ TEST(HybridFactorGraph, eliminate) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, eliminateMultifrontal) { TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
DiscreteKey c(C(1), 2); DiscreteKey c(C(1), 2);
@ -119,8 +119,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
DiscreteKey c1(C(1), 2); DiscreteKey c1(C(1), 2);
@ -143,8 +143,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullSequentialSimple) { TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
DiscreteKey c1(C(1), 2); DiscreteKey c1(C(1), 2);
@ -171,8 +171,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
DiscreteKey c1(C(1), 2); DiscreteKey c1(C(1), 2);
@ -204,8 +204,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
DiscreteKey c(C(1), 2); DiscreteKey c(C(1), 2);
@ -240,8 +240,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
* This test is about how to assemble the Bayes Tree roots after we do partial * This test is about how to assemble the Bayes Tree roots after we do partial
* elimination * elimination
*/ */
TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
HybridFactorGraph hfg; GaussianHybridFactorGraph hfg;
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); 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)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
@ -290,7 +290,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
GTSAM_PRINT(ordering_full); GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt; HybridBayesTree::shared_ptr hbt;
HybridFactorGraph::shared_ptr remaining; GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
GTSAM_PRINT(*hbt); GTSAM_PRINT(*hbt);
@ -309,7 +309,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one // TODO(fan): make a graph like Varun's paper one
TEST(HybridFactorGraph, Switching) { TEST(GaussianHybridFactorGraph, Switching) {
auto N = 12; auto N = 12;
auto hfg = makeSwitchingChain(N); auto hfg = makeSwitchingChain(N);
@ -381,7 +381,7 @@ TEST(HybridFactorGraph, Switching) {
GTSAM_PRINT(ordering_full); GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt; HybridBayesTree::shared_ptr hbt;
HybridFactorGraph::shared_ptr remaining; GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt); // GTSAM_PRINT(*hbt);
@ -417,7 +417,7 @@ TEST(HybridFactorGraph, Switching) {
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one // TODO(fan): make a graph like Varun's paper one
TEST(HybridFactorGraph, SwitchingISAM) { TEST(GaussianHybridFactorGraph, SwitchingISAM) {
auto N = 11; auto N = 11;
auto hfg = makeSwitchingChain(N); auto hfg = makeSwitchingChain(N);
@ -473,7 +473,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
GTSAM_PRINT(ordering_full); GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt; HybridBayesTree::shared_ptr hbt;
HybridFactorGraph::shared_ptr remaining; GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt); // GTSAM_PRINT(*hbt);
@ -502,7 +502,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
auto isam = HybridISAM(*hbt); auto isam = HybridISAM(*hbt);
{ {
HybridFactorGraph factorGraph; GaussianHybridFactorGraph factorGraph;
factorGraph.push_back(new_fg->at(new_fg->size() - 2)); factorGraph.push_back(new_fg->at(new_fg->size() - 2));
factorGraph.push_back(new_fg->at(new_fg->size() - 1)); factorGraph.push_back(new_fg->at(new_fg->size() - 1));
isam.update(factorGraph); isam.update(factorGraph);
@ -512,7 +512,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridFactorGraph, SwitchingTwoVar) { TEST(GaussianHybridFactorGraph, SwitchingTwoVar) {
const int N = 7; const int N = 7;
auto hfg = makeSwitchingChain(N, X); auto hfg = makeSwitchingChain(N, X);
hfg->push_back(*makeSwitchingChain(N, Y, D)); hfg->push_back(*makeSwitchingChain(N, Y, D));
@ -582,7 +582,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) {
} }
{ {
HybridBayesNet::shared_ptr hbn; HybridBayesNet::shared_ptr hbn;
HybridFactorGraph::shared_ptr remaining; GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbn, remaining) = std::tie(hbn, remaining) =
hfg->eliminatePartialSequential(ordering_partial); hfg->eliminatePartialSequential(ordering_partial);

14
gtsam/inference/inference.i
View File

@ -9,7 +9,7 @@ namespace gtsam {
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h> #include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -107,36 +107,36 @@ class Ordering {
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
static gtsam::Ordering ColamdConstrainedLast( static gtsam::Ordering ColamdConstrainedLast(
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
bool forceOrder = false); bool forceOrder = false);
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
static gtsam::Ordering ColamdConstrainedFirst( static gtsam::Ordering ColamdConstrainedFirst(
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
bool forceOrder = false); bool forceOrder = false);
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
const FACTOR_GRAPH& graph); const FACTOR_GRAPH& graph);

9
python/gtsam/tests/test_HybridFactorGraph.py
View File

@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
class TestHybridFactorGraph(GtsamTestCase): class TestGaussianHybridFactorGraph(GtsamTestCase):
"""Unit tests for HybridFactorGraph.""" """Unit tests for GaussianHybridFactorGraph."""
def test_create(self): def test_create(self):
"""Test contruction of hybrid factor graph.""" """Test contruction of hybrid factor graph."""
@ -36,13 +36,14 @@ class TestHybridFactorGraph(GtsamTestCase):
gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
hfg = gtsam.HybridFactorGraph() hfg = gtsam.GaussianHybridFactorGraph()
hfg.add(jf1) hfg.add(jf1)
hfg.add(jf2) hfg.add(jf2)
hfg.push_back(gmf) hfg.push_back(gmf)
hbn = hfg.eliminateSequential( hbn = hfg.eliminateSequential(
gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)])) gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph(
hfg, [C(0)]))
# print("hbn = ", hbn) # print("hbn = ", hbn)
self.assertEqual(hbn.size(), 2) self.assertEqual(hbn.size(), 2)