Rename files so that everything is HybridX
Varun Agrawal
parent
852a9b9ef6
commit
7c7b5dd030
|
|
@ -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
|
||||
*/
|
||||
|
|
@ -26,7 +27,7 @@ namespace gtsam {
|
|||
|
||||
// Instantiate base class
|
||||
template class BayesTreeCliqueBase<HybridBayesTreeClique,
|
||||
GaussianHybridFactorGraph>;
|
||||
HybridGaussianFactorGraph>;
|
||||
template class BayesTree<HybridBayesTreeClique>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -11,15 +11,16 @@
|
|||
|
||||
/**
|
||||
* @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
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
|
|
@ -38,10 +39,10 @@ class VectorValues;
|
|||
*/
|
||||
class GTSAM_EXPORT HybridBayesTreeClique
|
||||
: public BayesTreeCliqueBase<HybridBayesTreeClique,
|
||||
GaussianHybridFactorGraph> {
|
||||
HybridGaussianFactorGraph> {
|
||||
public:
|
||||
typedef HybridBayesTreeClique This;
|
||||
typedef BayesTreeCliqueBase<HybridBayesTreeClique, GaussianHybridFactorGraph>
|
||||
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
|
||||
Base;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
|
|
|
|||
|
|
@ -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/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
|
@ -34,7 +34,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class GaussianHybridFactorGraph;
|
||||
class HybridGaussianFactorGraph;
|
||||
|
||||
/**
|
||||
* Hybrid Conditional Density
|
||||
|
|
|
|||
|
|
@ -21,17 +21,17 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
template class EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>;
|
||||
template class EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridEliminationTree::HybridEliminationTree(
|
||||
const GaussianHybridFactorGraph& factorGraph,
|
||||
const HybridGaussianFactorGraph& factorGraph,
|
||||
const VariableIndex& structure, const Ordering& order)
|
||||
: Base(factorGraph, structure, order) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridEliminationTree::HybridEliminationTree(
|
||||
const GaussianHybridFactorGraph& factorGraph, const Ordering& order)
|
||||
const HybridGaussianFactorGraph& factorGraph, const Ordering& order)
|
||||
: Base(factorGraph, order) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -17,8 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.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, GaussianHybridFactorGraph> {
|
||||
: public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
|
||||
private:
|
||||
friend class ::EliminationTreeTester;
|
||||
|
||||
public:
|
||||
typedef EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>
|
||||
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 GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph& factorGraph,
|
||||
HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
|
||||
const Ordering& order);
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianHybridFactorGraph.cpp
|
||||
* @file HybridGaussianFactorGraph.cpp
|
||||
* @brief Hybrid factor graph that uses type erasure
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -23,7 +23,6 @@
|
|||
#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>
|
||||
|
|
@ -31,7 +30,8 @@
|
|||
#include <gtsam/hybrid/HybridEliminationTree.h>
|
||||
#include <gtsam/hybrid/HybridFactor.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<GaussianHybridFactorGraph>;
|
||||
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 GaussianHybridFactorGraph &factors,
|
||||
continuousElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
GaussianFactorGraph gfg;
|
||||
for (auto &fp : factors) {
|
||||
|
|
@ -103,7 +103,7 @@ continuousElimination(const GaussianHybridFactorGraph &factors,
|
|||
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
|
||||
discreteElimination(const GaussianHybridFactorGraph &factors,
|
||||
discreteElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
DiscreteFactorGraph dfg;
|
||||
for (auto &fp : factors) {
|
||||
|
|
@ -129,7 +129,7 @@ discreteElimination(const GaussianHybridFactorGraph &factors,
|
|||
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
|
||||
hybridElimination(const GaussianHybridFactorGraph &factors,
|
||||
hybridElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys,
|
||||
const KeySet &continuousSeparator,
|
||||
const std::set<DiscreteKey> &discreteSeparatorSet) {
|
||||
|
|
@ -236,7 +236,7 @@ hybridElimination(const GaussianHybridFactorGraph &factors,
|
|||
}
|
||||
/* ************************************************************************ */
|
||||
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> //
|
||||
EliminateHybrid(const GaussianHybridFactorGraph &factors,
|
||||
EliminateHybrid(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
// NOTE: Because we are in the Conditional Gaussian regime there are only
|
||||
// a few cases:
|
||||
|
|
@ -345,22 +345,22 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors,
|
|||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void GaussianHybridFactorGraph::add(JacobianFactor &&factor) {
|
||||
void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
|
||||
void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) {
|
||||
void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
|
||||
void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
|
||||
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
|
||||
}
|
||||
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianHybridFactorGraph.h
|
||||
* @file HybridGaussianFactorGraph.h
|
||||
* @brief Linearized Hybrid factor graph that uses type erasure
|
||||
* @author Fan Jiang
|
||||
* @date Mar 11, 2022
|
||||
|
|
@ -26,36 +26,37 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class GaussianHybridFactorGraph;
|
||||
class HybridGaussianFactorGraph;
|
||||
class HybridConditional;
|
||||
class HybridBayesNet;
|
||||
class HybridEliminationTree;
|
||||
class HybridBayesTree;
|
||||
class HybridJunctionTree;
|
||||
class HybridGaussianJunctionTree;
|
||||
class DecisionTreeFactor;
|
||||
|
||||
class JacobianFactor;
|
||||
|
||||
/** Main elimination function for GaussianHybridFactorGraph */
|
||||
/** Main elimination function for HybridGaussianFactorGraph */
|
||||
GTSAM_EXPORT
|
||||
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
|
||||
EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys);
|
||||
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <>
|
||||
struct EliminationTraits<GaussianHybridFactorGraph> {
|
||||
struct EliminationTraits<HybridGaussianFactorGraph> {
|
||||
typedef HybridFactor FactorType; ///< Type of factors in factor graph
|
||||
typedef GaussianHybridFactorGraph
|
||||
typedef HybridGaussianFactorGraph
|
||||
FactorGraphType; ///< Type of the factor graph (e.g.
|
||||
///< GaussianHybridFactorGraph)
|
||||
///< 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> >
|
||||
|
|
@ -70,12 +71,12 @@ struct EliminationTraits<GaussianHybridFactorGraph> {
|
|||
* This is the linearized version of a hybrid factor graph.
|
||||
* Everything inside needs to be hybrid factor or hybrid conditional.
|
||||
*/
|
||||
class GaussianHybridFactorGraph
|
||||
class HybridGaussianFactorGraph
|
||||
: public FactorGraph<HybridFactor>,
|
||||
public EliminateableFactorGraph<GaussianHybridFactorGraph> {
|
||||
public EliminateableFactorGraph<HybridGaussianFactorGraph> {
|
||||
public:
|
||||
using Base = FactorGraph<HybridFactor>;
|
||||
using This = GaussianHybridFactorGraph; ///< this class
|
||||
using This = HybridGaussianFactorGraph; ///< this class
|
||||
using BaseEliminateable =
|
||||
EliminateableFactorGraph<This>; ///< for elimination
|
||||
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
|
||||
|
|
@ -86,7 +87,7 @@ class GaussianHybridFactorGraph
|
|||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
GaussianHybridFactorGraph() = default;
|
||||
HybridGaussianFactorGraph() = default;
|
||||
|
||||
/**
|
||||
* Implicit copy/downcast constructor to override explicit template container
|
||||
|
|
@ -94,7 +95,7 @@ class GaussianHybridFactorGraph
|
|||
* `cachedSeparatorMarginal_.reset(*separatorMarginal)`
|
||||
* */
|
||||
template <class DERIVEDFACTOR>
|
||||
GaussianHybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
|
||||
HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
|
||||
: Base(graph) {}
|
||||
|
||||
/// @}
|
||||
|
|
@ -10,16 +10,16 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridISAM.h
|
||||
* @file HybridGaussianISAM.h
|
||||
* @date March 31, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.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 GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph& newFactors,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HybridISAM::update(const GaussianHybridFactorGraph& 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
|
||||
|
|
@ -20,33 +20,33 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.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 GaussianHybridFactorGraph& newFactors,
|
||||
const HybridGaussianFactorGraph& newFactors,
|
||||
HybridBayesTree::Cliques* orphans,
|
||||
const HybridBayesTree::Eliminate& function =
|
||||
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
|
||||
|
|
@ -58,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 GaussianHybridFactorGraph& 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/GaussianHybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridEliminationTree.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>
|
||||
|
||||
|
|
@ -27,19 +27,19 @@ namespace gtsam {
|
|||
|
||||
// Instantiate base classes
|
||||
template class EliminatableClusterTree<HybridBayesTree,
|
||||
GaussianHybridFactorGraph>;
|
||||
template class JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>;
|
||||
HybridGaussianFactorGraph>;
|
||||
template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
|
||||
|
||||
struct HybridConstructorTraversalData {
|
||||
typedef
|
||||
typename JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>::Node
|
||||
typename JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>::Node
|
||||
Node;
|
||||
typedef
|
||||
typename JunctionTree<HybridBayesTree,
|
||||
GaussianHybridFactorGraph>::sharedNode sharedNode;
|
||||
HybridGaussianFactorGraph>::sharedNode sharedNode;
|
||||
|
||||
HybridConstructorTraversalData* const parentData;
|
||||
sharedNode myJTNode;
|
||||
sharedNode junctionTreeNode;
|
||||
FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
|
||||
FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
|
||||
KeySet discreteKeys;
|
||||
|
|
@ -57,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
|
||||
|
|
@ -87,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;
|
||||
}
|
||||
}
|
||||
|
|
@ -142,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,
|
||||
|
|
@ -156,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,
|
||||
|
|
@ -164,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,15 +10,15 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file HybridJunctionTree.h
|
||||
* @file HybridGaussianJunctionTree.h
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.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, GaussianHybridFactorGraph> {
|
||||
class GTSAM_EXPORT HybridGaussianJunctionTree
|
||||
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
|
||||
public:
|
||||
typedef JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>
|
||||
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
|
||||
|
|
@ -98,15 +98,15 @@ class HybridBayesNet {
|
|||
const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
|
||||
};
|
||||
|
||||
#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
|
||||
class GaussianHybridFactorGraph {
|
||||
GaussianHybridFactorGraph();
|
||||
GaussianHybridFactorGraph(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::GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph {
|
|||
const gtsam::HybridFactor* at(size_t i) const;
|
||||
|
||||
void print(string s = "") const;
|
||||
bool equals(const gtsam::GaussianHybridFactorGraph& 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::GaussianHybridFactorGraph*>
|
||||
pair<gtsam::HybridBayesNet*, gtsam::HybridGaussianFactorGraph*>
|
||||
eliminatePartialSequential(const gtsam::Ordering& ordering);
|
||||
|
||||
gtsam::HybridBayesTree* eliminateMultifrontal();
|
||||
|
|
@ -134,7 +134,7 @@ class GaussianHybridFactorGraph {
|
|||
gtsam::Ordering::OrderingType type);
|
||||
gtsam::HybridBayesTree* eliminateMultifrontal(
|
||||
const gtsam::Ordering& ordering);
|
||||
pair<gtsam::HybridBayesTree*, gtsam::GaussianHybridFactorGraph*>
|
||||
pair<gtsam::HybridBayesTree*, gtsam::HybridGaussianFactorGraph*>
|
||||
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
|
||||
|
||||
string dot(
|
||||
|
|
|
|||
|
|
@ -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/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 GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
|
||||
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
|
||||
size_t n, std::function<Key(int)> keyFunc = X,
|
||||
std::function<Key(int)> dKeyFunc = C) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
|
||||
|
||||
|
|
@ -51,7 +51,7 @@ inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
|
|||
}
|
||||
}
|
||||
|
||||
return boost::make_shared<GaussianHybridFactorGraph>(std::move(hfg));
|
||||
return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
|
||||
}
|
||||
|
||||
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testGaussianHybridFactorGraph.cpp
|
||||
* @file testHybridGaussianFactorGraph.cpp
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
*/
|
||||
|
|
@ -20,7 +20,6 @@
|
|||
#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>
|
||||
|
|
@ -29,7 +28,8 @@
|
|||
#include <gtsam/hybrid/HybridDiscreteFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactor.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,25 +57,11 @@ 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(GaussianHybridFactorGraph, creation) {
|
||||
TEST(HybridGaussianFactorGraph, creation) {
|
||||
HybridConditional test;
|
||||
|
||||
GaussianHybridFactorGraph hfg;
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
|
||||
|
||||
|
|
@ -91,8 +77,8 @@ TEST(GaussianHybridFactorGraph, creation) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, eliminate) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminate) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
|
||||
|
||||
|
|
@ -102,8 +88,8 @@ TEST(GaussianHybridFactorGraph, eliminate) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c(C(1), 2);
|
||||
|
||||
|
|
@ -119,8 +105,8 @@ TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c1(C(1), 2);
|
||||
|
||||
|
|
@ -143,8 +129,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c1(C(1), 2);
|
||||
|
||||
|
|
@ -171,8 +157,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c1(C(1), 2);
|
||||
|
||||
|
|
@ -204,8 +190,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
|
||||
GaussianHybridFactorGraph hfg;
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
DiscreteKey c(C(1), 2);
|
||||
|
||||
|
|
@ -240,8 +226,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
|
|||
* This test is about how to assemble the Bayes Tree roots after we do partial
|
||||
* elimination
|
||||
*/
|
||||
TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||
GaussianHybridFactorGraph 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(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
GaussianHybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
GTSAM_PRINT(*hbt);
|
||||
|
|
@ -309,7 +295,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(GaussianHybridFactorGraph, Switching) {
|
||||
TEST(HybridGaussianFactorGraph, Switching) {
|
||||
auto N = 12;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
|
|
@ -381,7 +367,7 @@ TEST(GaussianHybridFactorGraph, Switching) {
|
|||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
GaussianHybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// GTSAM_PRINT(*hbt);
|
||||
|
|
@ -417,7 +403,7 @@ TEST(GaussianHybridFactorGraph, Switching) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(GaussianHybridFactorGraph, SwitchingISAM) {
|
||||
TEST(HybridGaussianFactorGraph, SwitchingISAM) {
|
||||
auto N = 11;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
|
|
@ -473,7 +459,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) {
|
|||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
GaussianHybridFactorGraph::shared_ptr remaining;
|
||||
HybridGaussianFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// GTSAM_PRINT(*hbt);
|
||||
|
|
@ -499,10 +485,10 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) {
|
|||
}
|
||||
|
||||
auto new_fg = makeSwitchingChain(12);
|
||||
auto isam = HybridISAM(*hbt);
|
||||
auto isam = HybridGaussianISAM(*hbt);
|
||||
|
||||
{
|
||||
GaussianHybridFactorGraph 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(GaussianHybridFactorGraph, SwitchingISAM) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianHybridFactorGraph, 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(GaussianHybridFactorGraph, SwitchingTwoVar) {
|
|||
}
|
||||
{
|
||||
HybridBayesNet::shared_ptr hbn;
|
||||
GaussianHybridFactorGraph::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/GaussianHybridFactorGraph.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::GaussianHybridFactorGraph}>
|
||||
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::GaussianHybridFactorGraph}>
|
||||
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::GaussianHybridFactorGraph}>
|
||||
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::GaussianHybridFactorGraph}>
|
||||
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::GaussianHybridFactorGraph}>
|
||||
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::GaussianHybridFactorGraph}>
|
||||
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 TestGaussianHybridFactorGraph(GtsamTestCase):
|
||||
"""Unit tests for GaussianHybridFactorGraph."""
|
||||
class TestHybridGaussianFactorGraph(GtsamTestCase):
|
||||
"""Unit tests for HybridGaussianFactorGraph."""
|
||||
|
||||
def test_create(self):
|
||||
"""Test contruction of hybrid factor graph."""
|
||||
|
|
@ -36,13 +36,13 @@ class TestGaussianHybridFactorGraph(GtsamTestCase):
|
|||
|
||||
gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
|
||||
|
||||
hfg = gtsam.GaussianHybridFactorGraph()
|
||||
hfg = gtsam.HybridGaussianFactorGraph()
|
||||
hfg.add(jf1)
|
||||
hfg.add(jf2)
|
||||
hfg.push_back(gmf)
|
||||
|
||||
hbn = hfg.eliminateSequential(
|
||||
gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph(
|
||||
gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
|
||||
hfg, [C(0)]))
|
||||
|
||||
# print("hbn = ", hbn)
|
||||
|
|
|
|||
Loading…
Reference in New Issue