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

Merge branch 'hybrid/hybrid-factor-graph' into hybrid/hybrid-optimize

release/4.3a0
sjxue 2022-08-14 20:42:15 -04:00
parent 58068503f4 91da5209fe
commit 36d6097d2b
8 changed files with 540 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -108,7 +108,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
void print(
const std::string &s = "HybridFactor\n",
const std::string &s = "GaussianMixtureFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}

25
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -10,6 +10,7 @@
* @file HybridBayesNet.cpp
* @brief A bayes net of Gaussian Conditionals indexed by discrete keys.
* @author Fan Jiang
* @author Varun Agrawal
* @author Shangjie Xue
* @date January 2022
*/
@ -20,10 +21,32 @@
namespace gtsam {
/* ************************************************************************* */
GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) const {
return boost::dynamic_pointer_cast<GaussianMixture>(factors_.at(i)->inner());
}
/* ************************************************************************* */
DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const {
return boost::dynamic_pointer_cast<DiscreteConditional>(
factors_.at(i)->inner());
}
/* ************************************************************************* */
GaussianBayesNet HybridBayesNet::choose(
const DiscreteValues &assignment) const {
GaussianBayesNet gbn;
for (size_t idx = 0; idx < size(); idx++) {
GaussianMixture gm = *this->atGaussian(idx);
gbn.push_back(gm(assignment));
}
return gbn;
}
/* *******************************************************************************/
HybridValues HybridBayesNet::optimize() const {
auto dag = HybridLookupDAG::FromBayesNet(*this);
return dag.argmax();
}
} // namespace gtsam
} // namespace gtsam

28
gtsam/hybrid/HybridBayesNet.h
View File

@ -20,6 +20,7 @@
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/linear/GaussianBayesNet.h>
namespace gtsam {
@ -35,11 +36,32 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
using shared_ptr = boost::shared_ptr<HybridBayesNet>;
using sharedConditional = boost::shared_ptr<ConditionalType>;
/// Construct empty bayes net
/** Construct empty bayes net */
HybridBayesNet() = default;
/// Destructor
virtual ~HybridBayesNet() {}
/// Add HybridConditional to Bayes Net
using Base::add;
/// Add a discrete conditional to the Bayes Net.
void add(const DiscreteKey &key, const std::string &table) {
push_back(
HybridConditional(boost::make_shared<DiscreteConditional>(key, table)));
}
/// Get a specific Gaussian mixture by index `i`.
GaussianMixture::shared_ptr atGaussian(size_t i) const;
/// Get a specific discrete conditional by index `i`.
DiscreteConditional::shared_ptr atDiscrete(size_t i) const;
/**
* @brief Get the Gaussian Bayes Net which corresponds to a specific discrete
* value assignment.
*
* @param assignment The discrete value assignment for the discrete keys.
* @return GaussianBayesNet
*/
GaussianBayesNet choose(const DiscreteValues &assignment) const;
/// Solve the HybridBayesNet by back-substitution.
HybridValues optimize() const;

140
gtsam/hybrid/HybridFactorGraph.h Normal file
View File

@ -0,0 +1,140 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HybridFactorGraph.h
* @brief Hybrid factor graph base class that uses type erasure
* @author Varun Agrawal
* @date May 28, 2022
*/
#pragma once
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <boost/format.hpp>
namespace gtsam {
using SharedFactor = boost::shared_ptr<Factor>;
/**
* Hybrid Factor Graph
* -----------------------
* This is the base hybrid factor graph.
* Everything inside needs to be hybrid factor or hybrid conditional.
*/
class HybridFactorGraph : public FactorGraph<HybridFactor> {
public:
using Base = FactorGraph<HybridFactor>;
using This = HybridFactorGraph; ///< this class
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
using Values = gtsam::Values; ///< backwards compatibility
using Indices = KeyVector; ///> map from keys to values
protected:
/// Check if FACTOR type is derived from DiscreteFactor.
template <typename FACTOR>
using IsDiscrete = typename std::enable_if<
std::is_base_of<DiscreteFactor, FACTOR>::value>::type;
/// Check if FACTOR type is derived from HybridFactor.
template <typename FACTOR>
using IsHybrid = typename std::enable_if<
std::is_base_of<HybridFactor, FACTOR>::value>::type;
public:
/// @name Constructors
/// @{
/// Default constructor
HybridFactorGraph() = default;
/**
* Implicit copy/downcast constructor to override explicit template container
* constructor. In BayesTree this is used for:
* `cachedSeparatorMarginal_.reset(*separatorMarginal)`
* */
template <class DERIVEDFACTOR>
HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// @}
// Allow use of selected FactorGraph methods:
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::push_back;
using Base::resize;
/**
* Add a discrete factor *pointer* to the internal discrete graph
* @param discreteFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsDiscrete<FACTOR> push_discrete(
const boost::shared_ptr<FACTOR>& discreteFactor) {
Base::push_back(boost::make_shared<HybridDiscreteFactor>(discreteFactor));
}
/**
* Add a discrete-continuous (Hybrid) factor *pointer* to the graph
* @param hybridFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsHybrid<FACTOR> push_hybrid(const boost::shared_ptr<FACTOR>& hybridFactor) {
Base::push_back(hybridFactor);
}
/// delete emplace_shared.
template <class FACTOR, class... Args>
void emplace_shared(Args&&... args) = delete;
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_discrete(factor);
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsHybrid<FACTOR> emplace_hybrid(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_hybrid(factor);
}
/**
* @brief Add a single factor shared pointer to the hybrid factor graph.
* Dynamically handles the factor type and assigns it to the correct
* underlying container.
*
* @param sharedFactor The factor to add to this factor graph.
*/
void push_back(const SharedFactor& sharedFactor) {
if (auto p = boost::dynamic_pointer_cast<DiscreteFactor>(sharedFactor)) {
push_discrete(p);
}
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(sharedFactor)) {
push_hybrid(p);
}
}
};
} // namespace gtsam

60
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -19,6 +19,8 @@
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
@ -53,10 +55,9 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
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 HybridJunctionTree JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function
static std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> >
@ -72,10 +73,16 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
* Everything inside needs to be hybrid factor or hybrid conditional.
*/
class GTSAM_EXPORT HybridGaussianFactorGraph
: public FactorGraph<HybridFactor>,
: public HybridFactorGraph,
public EliminateableFactorGraph<HybridGaussianFactorGraph> {
protected:
/// Check if FACTOR type is derived from GaussianFactor.
template <typename FACTOR>
using IsGaussian = typename std::enable_if<
std::is_base_of<GaussianFactor, FACTOR>::value>::type;
public:
using Base = FactorGraph<HybridFactor>;
using Base = HybridFactorGraph;
using This = HybridGaussianFactorGraph; ///< this class
using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination
@ -100,7 +107,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @}
using FactorGraph::add;
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::push_back;
using Base::resize;
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
@ -113,6 +126,39 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// Add a DecisionTreeFactor as a shared ptr.
void add(boost::shared_ptr<DecisionTreeFactor> factor);
/**
* Add a gaussian factor *pointer* to the internal gaussian factor graph
* @param gaussianFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsGaussian<FACTOR> push_gaussian(
const boost::shared_ptr<FACTOR>& gaussianFactor) {
Base::push_back(boost::make_shared<HybridGaussianFactor>(gaussianFactor));
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsGaussian<FACTOR> emplace_gaussian(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_gaussian(factor);
}
/**
* @brief Add a single factor shared pointer to the hybrid factor graph.
* Dynamically handles the factor type and assigns it to the correct
* underlying container.
*
* @param sharedFactor The factor to add to this factor graph.
*/
void push_back(const SharedFactor& sharedFactor) {
if (auto p = boost::dynamic_pointer_cast<GaussianFactor>(sharedFactor)) {
push_gaussian(p);
} else {
Base::push_back(sharedFactor);
}
}
};
} // namespace gtsam

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

@ -18,17 +18,135 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#pragma once
using gtsam::symbol_shorthand::C;
using gtsam::symbol_shorthand::X;
namespace gtsam {
using symbol_shorthand::C;
using symbol_shorthand::M;
using symbol_shorthand::X;
/* ****************************************************************************/
// Test fixture with switching network.
// TODO(Varun) Currently this is only linear. We need to add nonlinear support
// and then update to
// https://github.com/borglab/gtsam/pull/973/files#diff-58c02b3b197ebf731694946e87762d252e9eaa2f5c6c4ba22d618085b321ca23
struct Switching {
size_t K;
DiscreteKeys modes;
HybridGaussianFactorGraph linearizedFactorGraph;
Values linearizationPoint;
using MotionModel = BetweenFactor<double>;
// using MotionMixture = MixtureFactor<MotionModel>;
/// Create with given number of time steps.
Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1)
: K(K) {
// Create DiscreteKeys for binary K modes, modes[0] will not be used.
modes = addDiscreteModes(K);
// Create hybrid factor graph.
// Add a prior on X(1).
auto prior = boost::make_shared<JacobianFactor>(
X(1), Matrix11::Ones() / prior_sigma, Vector1::Zero());
linearizedFactorGraph.push_back(prior);
// Add "motion models".
linearizedFactorGraph = addMotionModels(K);
// Add measurement factors
for (size_t k = 1; k <= K; k++) {
linearizedFactorGraph.emplace_gaussian<JacobianFactor>(
X(k), Matrix11::Ones() / 0.1, Vector1::Zero());
}
// Add "mode chain"
linearizedFactorGraph = addModeChain(linearizedFactorGraph);
// Create the linearization point.
for (size_t k = 1; k <= K; k++) {
linearizationPoint.insert<double>(X(k), static_cast<double>(k));
}
}
/// Create DiscreteKeys for K binary modes.
DiscreteKeys addDiscreteModes(size_t K) {
DiscreteKeys m;
for (size_t k = 0; k <= K; k++) {
m.emplace_back(M(k), 2);
}
return m;
}
/// Helper function to add motion models for each [k, k+1] interval.
HybridGaussianFactorGraph addMotionModels(size_t K) {
HybridGaussianFactorGraph hgfg;
for (size_t k = 1; k < K; k++) {
auto keys = {X(k), X(k + 1)};
auto components = motionModels(k);
hgfg.emplace_hybrid<GaussianMixtureFactor>(keys, DiscreteKeys{modes[k]},
components);
}
return hgfg;
}
// Create motion models for a given time step
static std::vector<GaussianFactor::shared_ptr> motionModels(
size_t k, double sigma = 1.0) {
auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
auto still = boost::make_shared<JacobianFactor>(
X(k), -Matrix11::Ones() / sigma, X(k + 1),
Matrix11::Ones() / sigma, Vector1::Zero()),
moving = boost::make_shared<JacobianFactor>(
X(k), -Matrix11::Ones() / sigma, X(k + 1),
Matrix11::Ones() / sigma, -Vector1::Ones() / sigma);
return {boost::dynamic_pointer_cast<GaussianFactor>(still),
boost::dynamic_pointer_cast<GaussianFactor>(moving)};
}
// // Add "mode chain" to NonlinearHybridFactorGraph
// void addModeChain(HybridNonlinearFactorGraph& fg) {
// auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
// fg.push_discrete(prior);
// for (size_t k = 1; k < K - 1; k++) {
// auto parents = {modes[k]};
// auto conditional = boost::make_shared<DiscreteConditional>(
// modes[k + 1], parents, "1/2 3/2");
// fg.push_discrete(conditional);
// }
// }
// Add "mode chain" to GaussianHybridFactorGraph
HybridGaussianFactorGraph addModeChain(HybridGaussianFactorGraph& fg) {
auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
fg.push_discrete(prior);
for (size_t k = 1; k < K - 1; k++) {
auto parents = {modes[k]};
auto conditional = boost::make_shared<DiscreteConditional>(
modes[k + 1], parents, "1/2 3/2");
fg.push_discrete(conditional);
}
return fg;
}
};
/**
* @brief Create a switching system chain. A switching system is a continuous
* system which depends on a discrete mode at each time step of the chain.
*
* @param n The number of chain elements.
* @param keyFunc The functional to help specify the continuous key.
* @param dKeyFunc The functional to help specify the discrete key.
* @return HybridGaussianFactorGraph::shared_ptr
*/
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
size_t n, std::function<Key(int)> keyFunc = X,
std::function<Key(int)> dKeyFunc = C) {
@ -54,9 +172,19 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
}
/**
* @brief Return the ordering as a binary tree such that all parent nodes are
* above their children.
*
* This will result in a nested dissection Bayes tree after elimination.
*
* @param input The original ordering.
* @return std::pair<KeyVector, std::vector<int>>
*/
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
std::vector<Key> &input) {
std::vector<Key>& input) {
KeyVector new_order;
std::vector<int> levels(input.size());
std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
int)>
@ -79,7 +207,7 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
bsg(input.begin(), input.end(), 0);
std::reverse(new_order.begin(), new_order.end());
// std::reverse(levels.begin(), levels.end());
return {new_order, levels};
}

226
gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
View File

@ -60,26 +60,30 @@ using gtsam::symbol_shorthand::X;
using gtsam::symbol_shorthand::Y;
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, creation) {
HybridConditional test;
TEST(HybridGaussianFactorGraph, Creation) {
HybridConditional conditional;
HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1)));
GaussianMixture clgc(
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
GaussianMixture::Conditionals(
C(0),
boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
I_3x3),
boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
X(1), I_3x3)));
GTSAM_PRINT(clgc);
// Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
// graph
GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
GaussianMixture::Conditionals(
C(0),
boost::make_shared<GaussianConditional>(
X(0), Z_3x1, I_3x3, X(1), I_3x3),
boost::make_shared<GaussianConditional>(
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size());
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminate) {
TEST(HybridGaussianFactorGraph, EliminateSequential) {
// Test elimination of a single variable.
HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@ -90,11 +94,13 @@ TEST(HybridGaussianFactorGraph, eliminate) {
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
// Test multifrontal elimination
HybridGaussianFactorGraph hfg;
DiscreteKey c(C(1), 2);
// Add priors on x0 and c1
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
@ -112,9 +118,12 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
DiscreteKey c1(C(1), 2);
// Add prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Add a gaussian mixture factor ϕ(x1, c1)
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
@ -125,9 +134,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
auto dc = result->at(2)->asDiscreteConditional();
DiscreteValues dv;
dv[C(1)] = 0;
EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
DiscreteValues mode;
mode[C(1)] = 0;
EXPECT_DOUBLES_EQUAL(0.6225, (*dc)(mode), 1e-3);
}
/* ************************************************************************* */
@ -136,26 +145,26 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
DiscreteKey c1(C(1), 2);
// Add prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
// hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
// Discrete probability table for c1
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
// Joint discrete probability table for c1, c2
hfg.add(HybridDiscreteFactor(
DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
// 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
// {C(1), 2}}, "1 2 2 1")));
auto result = hfg.eliminateSequential(
Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
GTSAM_PRINT(*result);
EXPECT_LONGS_EQUAL(4, result->size());
}
/* ************************************************************************* */
@ -167,28 +176,19 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// DecisionTree<Key, GaussianFactor::shared_ptr> dt(
// C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
// boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
// hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
hfg.add(GaussianMixtureFactor::FromFactors(
{X(1)}, {{C(1), 2}},
{boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
// hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
hfg.add(DecisionTreeFactor(c1, {2, 8}));
hfg.add(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
// 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
// {C(1), 2}}, "1 2 2 1")));
auto result = hfg.eliminateMultifrontal(
Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
GTSAM_PRINT(*result);
GTSAM_PRINT(*result->marginalFactor(C(2)));
// GTSAM_PRINT(*result);
// GTSAM_PRINT(*result->marginalFactor(C(2)));
}
/* ************************************************************************* */
@ -197,30 +197,28 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
DiscreteKey c(C(1), 2);
// Prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Factor between x0-x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Decision tree with different modes on x1
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
// Hybrid factor P(x1|c1)
hfg.add(GaussianMixtureFactor({X(1)}, {c}, dt));
// Prior factor on c1
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
// 2 3 4")));
// Get a constrained ordering keeping c1 last
auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});
// Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
GTSAM_PRINT(*hbt);
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
*/
EXPECT_LONGS_EQUAL(3, hbt->size());
}
/* ************************************************************************* */
@ -235,10 +233,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
{
// DecisionTree<Key, GaussianFactor::shared_ptr> dt(
// C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
// boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
hfg.add(GaussianMixtureFactor::FromFactors(
{X(0)}, {{C(0), 2}},
{boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
@ -251,7 +245,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
}
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
hfg.add(HybridDiscreteFactor(
DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
@ -275,26 +268,37 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
auto ordering_full =
Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*remaining);
GTSAM_PRINT(*remaining);
hbt->dot(std::cout);
// hbt->marginalFactor(X(1))->print("HBT: ");
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
(Fan) Explanation: the Junction tree will need to reeliminate to get to the
marginal on X(1), which is not possible because it involves eliminating
discrete before continuous. The solution to this, however, is in Murphy02.
TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
And I believe that we should do this.
*/
}
void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg,
const HybridBayesTree::shared_ptr &hbt,
const Ordering &ordering) {
DotWriter dw;
dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1;
std::cout << hfg->dot(DefaultKeyFormatter, dw);
std::cout << "\n";
hbt->dot(std::cout);
std::cout << "\n";
std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
}
/* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one
TEST(HybridGaussianFactorGraph, Switching) {
@ -328,9 +332,6 @@ TEST(HybridGaussianFactorGraph, Switching) {
for (auto &l : lvls) {
l = -l;
}
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
std::cout << "\n";
}
{
std::vector<int> naturalC(N - 1);
@ -344,63 +345,19 @@ TEST(HybridGaussianFactorGraph, Switching) {
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
}
auto ordering_full = Ordering(ordering);
// auto ordering_full =
// Ordering();
// for (int i = 1; i <= 9; i++) {
// ordering_full.push_back(X(i));
// }
// for (int i = 1; i < 9; i++) {
// ordering_full.push_back(C(i));
// }
// auto ordering_full =
// Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
// X(5),
// C(1), C(2), C(3), C(4), C(5), C(6), C(7), C(8)});
// GTSAM_PRINT(*hfg);
GTSAM_PRINT(ordering_full);
// GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*remaining);
{
DotWriter dw;
dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1;
std::cout << hfg->dot(DefaultKeyFormatter, dw);
std::cout << "\n";
hbt->dot(std::cout);
}
{
DotWriter dw;
// dw.positionHints['c'] = 2;
// dw.positionHints['x'] = 1;
std::cout << "\n";
std::cout << hfg->eliminateSequential(ordering_full)
->dot(DefaultKeyFormatter, dw);
}
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
*/
hbt->marginalFactor(C(11))->print("HBT: ");
// hbt->marginalFactor(C(11))->print("HBT: ");
}
/* ************************************************************************* */
@ -436,9 +393,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
for (auto &l : lvls) {
l = -l;
}
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
std::cout << "\n";
}
{
std::vector<int> naturalC(N - 1);
@ -452,40 +406,16 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
std::copy(lvls.begin(), lvls.end(),
std::ostream_iterator<int>(std::cout, ","));
}
auto ordering_full = Ordering(ordering);
// GTSAM_PRINT(*hfg);
GTSAM_PRINT(ordering_full);
// GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
// GTSAM_PRINT(*hbt);
// GTSAM_PRINT(*remaining);
{
DotWriter dw;
dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1;
std::cout << hfg->dot(DefaultKeyFormatter, dw);
std::cout << "\n";
hbt->dot(std::cout);
}
{
DotWriter dw;
// dw.positionHints['c'] = 2;
// dw.positionHints['x'] = 1;
std::cout << "\n";
std::cout << hfg->eliminateSequential(ordering_full)
->dot(DefaultKeyFormatter, dw);
}
auto new_fg = makeSwitchingChain(12);
auto isam = HybridGaussianISAM(*hbt);
@ -494,13 +424,14 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
factorGraph.push_back(new_fg->at(new_fg->size() - 2));
factorGraph.push_back(new_fg->at(new_fg->size() - 1));
isam.update(factorGraph);
std::cout << isam.dot();
isam.marginalFactor(C(11))->print();
// std::cout << isam.dot();
// isam.marginalFactor(C(11))->print();
}
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
// TODO(Varun) Actually test something!
TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) {
const int N = 7;
auto hfg = makeSwitchingChain(N, X);
hfg->push_back(*makeSwitchingChain(N, Y, D));
@ -519,21 +450,6 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
ordX.emplace_back(Y(i));
}
// {
// KeyVector ndX;
// std::vector<int> lvls;
// std::tie(ndX, lvls) = makeBinaryOrdering(naturalX);
// std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
// std::copy(lvls.begin(), lvls.end(),
// std::ostream_iterator<int>(std::cout, ","));
// std::cout << "\n";
// for (size_t i = 0; i < N; i++) {
// ordX.emplace_back(X(ndX[i]));
// ordX.emplace_back(Y(ndX[i]));
// }
// }
for (size_t i = 1; i <= N - 1; i++) {
ordX.emplace_back(C(i));
}

93
gtsam/hybrid/tests/testHybridBayesNet.cpp Normal file
View File

@ -0,0 +1,93 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testHybridBayesNet.cpp
* @brief Unit tests for HybridBayesNet
* @author Varun Agrawal
* @author Fan Jiang
* @author Frank Dellaert
* @date December 2021
*/
#include <gtsam/hybrid/HybridBayesNet.h>
#include "Switching.h"
// Include for test suite
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using noiseModel::Isotropic;
using symbol_shorthand::M;
using symbol_shorthand::X;
static const DiscreteKey Asia(0, 2);
/* ****************************************************************************/
// Test creation
TEST(HybridBayesNet, Creation) {
HybridBayesNet bayesNet;
bayesNet.add(Asia, "99/1");
DiscreteConditional expected(Asia, "99/1");
CHECK(bayesNet.atDiscrete(0));
auto& df = *bayesNet.atDiscrete(0);
EXPECT(df.equals(expected));
}
/* ****************************************************************************/
// Test choosing an assignment of conditionals
TEST(HybridBayesNet, Choose) {
Switching s(4);
Ordering ordering;
for (auto&& kvp : s.linearizationPoint) {
ordering += kvp.key;
}
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteValues assignment;
assignment[M(1)] = 1;
assignment[M(2)] = 1;
assignment[M(3)] = 0;
GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
EXPECT_LONGS_EQUAL(4, gbn.size());
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(0)))(assignment),
*gbn.at(0)));
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(1)))(assignment),
*gbn.at(1)));
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(2)))(assignment),
*gbn.at(2)));
EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
hybridBayesNet->atGaussian(3)))(assignment),
*gbn.at(3)));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */