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
* @author Fan Jiang
* @author Varun Agrawal
@ -23,13 +23,13 @@
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.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/inference/EliminateableFactorGraph-inst.h>
@ -53,7 +53,7 @@
namespace gtsam {
template class EliminateableFactorGraph<HybridFactorGraph>;
template class EliminateableFactorGraph<GaussianHybridFactorGraph>;
/* ************************************************************************ */
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 GaussianHybridFactorGraph &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 GaussianHybridFactorGraph &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 GaussianHybridFactorGraph &factors,
const Ordering &frontalKeys,
const KeySet &continuousSeparator,
const std::set<DiscreteKey> &discreteSeparatorSet) {
// 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> //
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
// a few cases:
// 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)));
}
/* ************************************************************************ */
void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr 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)));
}
/* ************************************************************************ */
void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr 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
* @brief Hybrid factor graph that uses type erasure
* @file GaussianHybridFactorGraph.h
* @brief Linearized Hybrid factor graph that uses type erasure
* @author Fan Jiang
* @date Mar 11, 2022
*/
@ -26,7 +26,7 @@
namespace gtsam {
// Forward declarations
class HybridFactorGraph;
class GaussianHybridFactorGraph;
class HybridConditional;
class HybridBayesNet;
class HybridEliminationTree;
@ -36,17 +36,18 @@ class DecisionTreeFactor;
class JacobianFactor;
/** Main elimination function for HybridFactorGraph */
/** Main elimination function for GaussianHybridFactorGraph */
GTSAM_EXPORT
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys);
/* ************************************************************************* */
template <>
struct EliminationTraits<HybridFactorGraph> {
struct EliminationTraits<GaussianHybridFactorGraph> {
typedef HybridFactor FactorType; ///< Type of factors in factor graph
typedef HybridFactorGraph
FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph)
typedef GaussianHybridFactorGraph
FactorGraphType; ///< Type of the factor graph (e.g.
///< GaussianHybridFactorGraph)
typedef HybridConditional
ConditionalType; ///< Type of conditionals from elimination
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
* 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 GaussianHybridFactorGraph
: public FactorGraph<HybridFactor>,
public EliminateableFactorGraph<GaussianHybridFactorGraph> {
public:
using Base = FactorGraph<HybridFactor>;
using This = HybridFactorGraph; ///< this class
using This = GaussianHybridFactorGraph; ///< this class
using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
@ -84,7 +86,7 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
/// @name Constructors
/// @{
HybridFactorGraph() = default;
GaussianHybridFactorGraph() = default;
/**
* Implicit copy/downcast constructor to override explicit template container
@ -92,7 +94,8 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
* `cachedSeparatorMarginal_.reset(*separatorMarginal)`
* */
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 {
// Instantiate base class
template class BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>;
template class BayesTreeCliqueBase<HybridBayesTreeClique,
GaussianHybridFactorGraph>;
template class BayesTree<HybridBayesTreeClique>;
/* ************************************************************************* */

8
gtsam/hybrid/HybridBayesTree.h
View File

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

7
gtsam/hybrid/HybridConditional.h
View File

@ -18,9 +18,9 @@
#pragma once
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.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 GaussianHybridFactorGraph;
/**
* Hybrid Conditional Density
@ -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_);
}

8
gtsam/hybrid/HybridEliminationTree.cpp
View File

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

10
gtsam/hybrid/HybridEliminationTree.h
View File

@ -17,8 +17,8 @@
#pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridFactorGraph.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, GaussianHybridFactorGraph> {
private:
friend class ::EliminationTreeTester;
public:
typedef EliminationTree<HybridBayesNet, HybridFactorGraph>
typedef EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>
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 GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph& factorGraph,
const Ordering& order);
/// @}

6
gtsam/hybrid/HybridISAM.cpp
View File

@ -17,8 +17,8 @@
* @author Richard Roberts
*/
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridISAM.h>
#include <gtsam/inference/ISAM-inst.h>
#include <gtsam/inference/Key.h>
@ -37,7 +37,7 @@ HybridISAM::HybridISAM() {}
HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
/* ************************************************************************* */
void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const HybridBayesTree::Eliminate& function) {
// 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) {
Cliques orphans;
this->updateInternal(newFactors, &orphans, function);

7
gtsam/hybrid/HybridISAM.h
View File

@ -20,8 +20,8 @@
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/ISAM.h>
namespace gtsam {
@ -46,7 +46,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
private:
/// Internal method that performs the ISAM update.
void updateInternal(
const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
const GaussianHybridFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const HybridBayesTree::Eliminate& function =
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 function Elimination function.
*/
void update(const HybridFactorGraph& newFactors,
void update(const GaussianHybridFactorGraph& newFactors,
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
};

16
gtsam/hybrid/HybridJunctionTree.cpp
View File

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

6
gtsam/hybrid/HybridJunctionTree.h
View File

@ -17,8 +17,8 @@
#pragma once
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/JunctionTree.h>
namespace gtsam {
@ -49,9 +49,9 @@ class HybridEliminationTree;
* \nosubgrouping
*/
class GTSAM_EXPORT HybridJunctionTree
: public JunctionTree<HybridBayesTree, HybridFactorGraph> {
: public JunctionTree<HybridBayesTree, GaussianHybridFactorGraph> {
public:
typedef JunctionTree<HybridBayesTree, HybridFactorGraph>
typedef JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>
Base; ///< Base class
typedef HybridJunctionTree This; ///< 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;
};
#include <gtsam/hybrid/HybridFactorGraph.h>
class HybridFactorGraph {
HybridFactorGraph();
HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
class GaussianHybridFactorGraph {
GaussianHybridFactorGraph();
GaussianHybridFactorGraph(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::GaussianHybridFactorGraph& 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::GaussianHybridFactorGraph& 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::GaussianHybridFactorGraph*>
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::GaussianHybridFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
string dot(

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

@ -18,8 +18,8 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.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 GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X,
std::function<Key(int)> dKeyFunc = C) {
HybridFactorGraph hfg;
GaussianHybridFactorGraph 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<GaussianHybridFactorGraph>(std::move(hfg));
}
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
* @author Fan Jiang
*/
@ -20,6 +20,7 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
#include <gtsam/hybrid/GaussianMixtureConditional.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
@ -27,7 +28,6 @@
#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/inference/BayesNet.h>
@ -72,10 +72,10 @@ void my_signal_handler(int signum) {
#endif
/* ************************************************************************* */
TEST(HybridFactorGraph, creation) {
TEST(GaussianHybridFactorGraph, creation) {
HybridConditional test;
HybridFactorGraph hfg;
GaussianHybridFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@ -91,8 +91,8 @@ TEST(HybridFactorGraph, creation) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, eliminate) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminate) {
GaussianHybridFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@ -102,8 +102,8 @@ TEST(HybridFactorGraph, eliminate) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, eliminateMultifrontal) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
GaussianHybridFactorGraph hfg;
DiscreteKey c(C(1), 2);
@ -119,8 +119,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
GaussianHybridFactorGraph hfg;
DiscreteKey c1(C(1), 2);
@ -143,8 +143,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
GaussianHybridFactorGraph hfg;
DiscreteKey c1(C(1), 2);
@ -171,8 +171,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
GaussianHybridFactorGraph hfg;
DiscreteKey c1(C(1), 2);
@ -204,8 +204,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
GaussianHybridFactorGraph hfg;
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
* elimination
*/
TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
HybridFactorGraph hfg;
TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
GaussianHybridFactorGraph 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 +290,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridFactorGraph::shared_ptr remaining;
GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
GTSAM_PRINT(*hbt);
@ -309,7 +309,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
/* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one
TEST(HybridFactorGraph, Switching) {
TEST(GaussianHybridFactorGraph, Switching) {
auto N = 12;
auto hfg = makeSwitchingChain(N);
@ -381,7 +381,7 @@ TEST(HybridFactorGraph, Switching) {
GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridFactorGraph::shared_ptr remaining;
GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt);
@ -417,7 +417,7 @@ TEST(HybridFactorGraph, Switching) {
/* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one
TEST(HybridFactorGraph, SwitchingISAM) {
TEST(GaussianHybridFactorGraph, SwitchingISAM) {
auto N = 11;
auto hfg = makeSwitchingChain(N);
@ -473,7 +473,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridFactorGraph::shared_ptr remaining;
GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt);
@ -502,7 +502,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
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() - 1));
isam.update(factorGraph);
@ -512,7 +512,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
}
/* ************************************************************************* */
TEST(HybridFactorGraph, SwitchingTwoVar) {
TEST(GaussianHybridFactorGraph, SwitchingTwoVar) {
const int N = 7;
auto hfg = makeSwitchingChain(N, X);
hfg->push_back(*makeSwitchingChain(N, Y, D));
@ -582,7 +582,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) {
}
{
HybridBayesNet::shared_ptr hbn;
HybridFactorGraph::shared_ptr remaining;
GaussianHybridFactorGraph::shared_ptr remaining;
std::tie(hbn, remaining) =
hfg->eliminatePartialSequential(ordering_partial);

14
gtsam/inference/inference.i
View File

@ -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/GaussianHybridFactorGraph.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::GaussianHybridFactorGraph}>
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::GaussianHybridFactorGraph}>
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::GaussianHybridFactorGraph}>
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::GaussianHybridFactorGraph}>
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::GaussianHybridFactorGraph}>
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::GaussianHybridFactorGraph}>
static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
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
class TestHybridFactorGraph(GtsamTestCase):
"""Unit tests for HybridFactorGraph."""
class TestGaussianHybridFactorGraph(GtsamTestCase):
"""Unit tests for GaussianHybridFactorGraph."""
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.GaussianHybridFactorGraph()
hfg.add(jf1)
hfg.add(jf2)
hfg.push_back(gmf)
hbn = hfg.eliminateSequential(
gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)]))
gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph(
hfg, [C(0)]))
# print("hbn = ", hbn)
self.assertEqual(hbn.size(), 2)