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

Merge branch 'develop' into feature/Ordering_initializers

release/4.3a0
Frank Dellaert 2023-01-08 09:23:42 -08:00
parent d3a40fbc71 00dc32df0d
commit 9675761f71
37 changed files with 496 additions and 1190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -62,9 +62,17 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree
typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function /// The default dense elimination function
static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > static std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> >
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminateDiscrete(factors, keys); } return EliminateDiscrete(factors, keys);
}
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
return Ordering::Colamd(*variableIndex);
}
}; };
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/hybrid/GaussianMixture.cpp
View File

@ -59,6 +59,8 @@ GaussianMixture::GaussianMixture(
Conditionals(discreteParents, conditionals)) {} Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/ /* *******************************************************************************/
// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from
// GaussianMixtureFactor, no?
GaussianFactorGraphTree GaussianMixture::add( GaussianFactorGraphTree GaussianMixture::add(
const GaussianFactorGraphTree &sum) const { const GaussianFactorGraphTree &sum) const {
using Y = GraphAndConstant; using Y = GraphAndConstant;

12
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -141,8 +141,8 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
/* ************************************************************************* */ /* ************************************************************************* */
void HybridBayesNet::updateDiscreteConditionals( void HybridBayesNet::updateDiscreteConditionals(
const DecisionTreeFactor::shared_ptr &prunedDecisionTree) { const DecisionTreeFactor &prunedDecisionTree) {
KeyVector prunedTreeKeys = prunedDecisionTree->keys(); KeyVector prunedTreeKeys = prunedDecisionTree.keys();
// Loop with index since we need it later. // Loop with index since we need it later.
for (size_t i = 0; i < this->size(); i++) { for (size_t i = 0; i < this->size(); i++) {
@ -154,7 +154,7 @@ void HybridBayesNet::updateDiscreteConditionals(
auto discreteTree = auto discreteTree =
boost::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete); boost::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
DecisionTreeFactor::ADT prunedDiscreteTree = DecisionTreeFactor::ADT prunedDiscreteTree =
discreteTree->apply(prunerFunc(*prunedDecisionTree, *conditional)); discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional));
// Create the new (hybrid) conditional // Create the new (hybrid) conditional
KeyVector frontals(discrete->frontals().begin(), KeyVector frontals(discrete->frontals().begin(),
@ -173,9 +173,7 @@ void HybridBayesNet::updateDiscreteConditionals(
HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
// Get the decision tree of only the discrete keys // Get the decision tree of only the discrete keys
auto discreteConditionals = this->discreteConditionals(); auto discreteConditionals = this->discreteConditionals();
const DecisionTreeFactor::shared_ptr decisionTree = const auto decisionTree = discreteConditionals->prune(maxNrLeaves);
boost::make_shared<DecisionTreeFactor>(
discreteConditionals->prune(maxNrLeaves));
this->updateDiscreteConditionals(decisionTree); this->updateDiscreteConditionals(decisionTree);
@ -194,7 +192,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
if (auto gm = conditional->asMixture()) { if (auto gm = conditional->asMixture()) {
// Make a copy of the Gaussian mixture and prune it! // Make a copy of the Gaussian mixture and prune it!
auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(*gm); auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(*gm);
prunedGaussianMixture->prune(*decisionTree); // imperative :-( prunedGaussianMixture->prune(decisionTree); // imperative :-(
// Type-erase and add to the pruned Bayes Net fragment. // Type-erase and add to the pruned Bayes Net fragment.
prunedBayesNetFragment.push_back(prunedGaussianMixture); prunedBayesNetFragment.push_back(prunedGaussianMixture);

49
gtsam/hybrid/HybridBayesNet.h
View File

@ -51,33 +51,51 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
/// @{ /// @{
/// GTSAM-style printing /// GTSAM-style printing
void print( void print(const std::string &s = "", const KeyFormatter &formatter =
const std::string &s = "", DefaultKeyFormatter) const override;
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// GTSAM-style equals /// GTSAM-style equals
bool equals(const This& fg, double tol = 1e-9) const; bool equals(const This &fg, double tol = 1e-9) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/// Add HybridConditional to Bayes Net /**
using Base::emplace_shared; * @brief Add a hybrid conditional using a shared_ptr.
*
* This is the "native" push back, as this class stores hybrid conditionals.
*/
void push_back(boost::shared_ptr<HybridConditional> conditional) {
factors_.push_back(conditional);
}
/// Add a conditional directly using a pointer. /**
* Preferred: add a conditional directly using a pointer.
*
* Examples:
* hbn.emplace_back(new GaussianMixture(...)));
* hbn.emplace_back(new GaussianConditional(...)));
* hbn.emplace_back(new DiscreteConditional(...)));
*/
template <class Conditional> template <class Conditional>
void emplace_back(Conditional *conditional) { void emplace_back(Conditional *conditional) {
factors_.push_back(boost::make_shared<HybridConditional>( factors_.push_back(boost::make_shared<HybridConditional>(
boost::shared_ptr<Conditional>(conditional))); boost::shared_ptr<Conditional>(conditional)));
} }
/// Add a conditional directly using a shared_ptr. /**
void push_back(boost::shared_ptr<HybridConditional> conditional) { * Add a conditional using a shared_ptr, using implicit conversion to
factors_.push_back(conditional); * a HybridConditional.
} *
* This is useful when you create a conditional shared pointer as you need it
/// Add a conditional directly using implicit conversion. * somewhere else.
*
* Example:
* auto shared_ptr_to_a_conditional =
* boost::make_shared<GaussianMixture>(...);
* hbn.push_back(shared_ptr_to_a_conditional);
*/
void push_back(HybridConditional &&conditional) { void push_back(HybridConditional &&conditional) {
factors_.push_back( factors_.push_back(
boost::make_shared<HybridConditional>(std::move(conditional))); boost::make_shared<HybridConditional>(std::move(conditional)));
@ -214,8 +232,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
* *
* @param prunedDecisionTree * @param prunedDecisionTree
*/ */
void updateDiscreteConditionals( void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree);
const DecisionTreeFactor::shared_ptr &prunedDecisionTree);
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

15
gtsam/hybrid/HybridBayesTree.h
View File

@ -132,18 +132,15 @@ struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};
* This object stores parent keys in our base type factor so that * This object stores parent keys in our base type factor so that
* eliminating those parent keys will pull this subtree into the * eliminating those parent keys will pull this subtree into the
* elimination. * elimination.
* This does special stuff for the hybrid case.
* *
* @tparam CLIQUE * This is a template instantiation for hybrid Bayes tree cliques, storing both
* the regular keys *and* discrete keys in the HybridConditional.
*/ */
template <class CLIQUE> template <>
class BayesTreeOrphanWrapper< class BayesTreeOrphanWrapper<HybridBayesTreeClique> : public HybridConditional {
CLIQUE, typename std::enable_if<
boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
: public CLIQUE::ConditionalType {
public: public:
typedef CLIQUE CliqueType; typedef HybridBayesTreeClique CliqueType;
typedef typename CLIQUE::ConditionalType Base; typedef HybridConditional Base;
boost::shared_ptr<CliqueType> clique; boost::shared_ptr<CliqueType> clique;

61
gtsam/hybrid/HybridDiscreteFactor.cpp
View File

@ -1,61 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridDiscreteFactor.cpp
* @brief Wrapper for a discrete factor
* @date Mar 11, 2022
* @author Fan Jiang
*/
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <boost/make_shared.hpp>
#include "gtsam/discrete/DecisionTreeFactor.h"
namespace gtsam {
/* ************************************************************************ */
HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
: Base(boost::dynamic_pointer_cast<DecisionTreeFactor>(other)
->discreteKeys()),
inner_(other) {}
/* ************************************************************************ */
HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
: Base(dtf.discreteKeys()),
inner_(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {}
/* ************************************************************************ */
bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
const This *e = dynamic_cast<const This *>(&lf);
if (e == nullptr) return false;
if (!Base::equals(*e, tol)) return false;
return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false)
: !(e->inner_);
}
/* ************************************************************************ */
void HybridDiscreteFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
inner_->print("\n", formatter);
};
/* ************************************************************************ */
double HybridDiscreteFactor::error(const HybridValues &values) const {
return -log((*inner_)(values.discrete()));
}
/* ************************************************************************ */
} // namespace gtsam

91
gtsam/hybrid/HybridDiscreteFactor.h
View File

@ -1,91 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridDiscreteFactor.h
* @date Mar 11, 2022
* @author Fan Jiang
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
namespace gtsam {
class HybridValues;
/**
* A HybridDiscreteFactor is a thin container for DiscreteFactor, which
* allows us to hide the implementation of DiscreteFactor and thus avoid
* diamond inheritance.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor {
private:
DiscreteFactor::shared_ptr inner_;
public:
using Base = HybridFactor;
using This = HybridDiscreteFactor;
using shared_ptr = boost::shared_ptr<This>;
/// @name Constructors
/// @{
/// Default constructor - for serialization.
HybridDiscreteFactor() = default;
// Implicit conversion from a shared ptr of DF
HybridDiscreteFactor(DiscreteFactor::shared_ptr other);
// Forwarding constructor from concrete DecisionTreeFactor
HybridDiscreteFactor(DecisionTreeFactor &&dtf);
/// @}
/// @name Testable
/// @{
virtual bool equals(const HybridFactor &lf, double tol) const override;
void print(
const std::string &s = "HybridFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/// Return pointer to the internal discrete factor.
DiscreteFactor::shared_ptr inner() const { return inner_; }
/// Return the error of the underlying Discrete Factor.
double error(const HybridValues &values) const override;
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(inner_);
}
};
// traits
template <>
struct traits<HybridDiscreteFactor> : public Testable<HybridDiscreteFactor> {};
} // namespace gtsam

5
gtsam/hybrid/HybridFactor.h
View File

@ -67,11 +67,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
const DiscreteKeys &key2); const DiscreteKeys &key2);
/** /**
* Base class for hybrid probabilistic factors * Base class for *truly* hybrid probabilistic factors
* *
* Examples: * Examples:
* - HybridGaussianFactor * - MixtureFactor
* - HybridDiscreteFactor
* - GaussianMixtureFactor * - GaussianMixtureFactor
* - GaussianMixture * - GaussianMixture
* *

78
gtsam/hybrid/HybridFactorGraph.cpp Normal file
View File

@ -0,0 +1,78 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2022, 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.cpp
* @brief Factor graph with utilities for hybrid factors.
* @author Varun Agrawal
* @author Frank Dellaert
* @date January, 2023
*/
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <boost/format.hpp>
namespace gtsam {
/* ************************************************************************* */
DiscreteKeys HybridFactorGraph::discreteKeys() const {
DiscreteKeys keys;
for (auto& factor : factors_) {
if (auto p = boost::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.push_back(key);
}
}
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.push_back(key);
}
}
}
return keys;
}
/* ************************************************************************* */
KeySet HybridFactorGraph::discreteKeySet() const {
KeySet keys;
for (const DiscreteKey& k : discreteKeys()) {
keys.insert(k.first);
}
return keys;
}
/* ************************************************************************* */
std::unordered_map<Key, DiscreteKey> HybridFactorGraph::discreteKeyMap() const {
std::unordered_map<Key, DiscreteKey> result;
for (const DiscreteKey& k : discreteKeys()) {
result[k.first] = k;
}
return result;
}
/* ************************************************************************* */
const KeySet HybridFactorGraph::continuousKeySet() const {
KeySet keys;
for (auto& factor : factors_) {
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(factor)) {
for (const Key& key : p->continuousKeys()) {
keys.insert(key);
}
}
}
return keys;
}
/* ************************************************************************* */
} // namespace gtsam

124
gtsam/hybrid/HybridFactorGraph.h
View File

@ -11,50 +11,40 @@
/** /**
* @file HybridFactorGraph.h * @file HybridFactorGraph.h
* @brief Hybrid factor graph base class that uses type erasure * @brief Factor graph with utilities for hybrid factors.
* @author Varun Agrawal * @author Varun Agrawal
* @author Frank Dellaert
* @date May 28, 2022 * @date May 28, 2022
*/ */
#pragma once #pragma once
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <unordered_map>
namespace gtsam { namespace gtsam {
class DiscreteFactor;
class Ordering;
using SharedFactor = boost::shared_ptr<Factor>; using SharedFactor = boost::shared_ptr<Factor>;
/** /**
* Hybrid Factor Graph * Hybrid Factor Graph
* ----------------------- * Factor graph with utilities for hybrid factors.
* This is the base hybrid factor graph.
* Everything inside needs to be hybrid factor or hybrid conditional.
*/ */
class HybridFactorGraph : public FactorGraph<HybridFactor> { class HybridFactorGraph : public FactorGraph<Factor> {
public: public:
using Base = FactorGraph<HybridFactor>; using Base = FactorGraph<Factor>;
using This = HybridFactorGraph; ///< this class using This = HybridFactorGraph; ///< this class
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
using Values = gtsam::Values; ///< backwards compatibility using Values = gtsam::Values; ///< backwards compatibility
using Indices = KeyVector; ///> map from keys to values 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: public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
@ -71,92 +61,22 @@ class HybridFactorGraph : public FactorGraph<HybridFactor> {
HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// @} /// @}
/// @name Extra methods to inspect discrete/continuous keys.
// 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);
}
}
/// Get all the discrete keys in the factor graph. /// Get all the discrete keys in the factor graph.
const KeySet discreteKeys() const { DiscreteKeys discreteKeys() const;
KeySet discrete_keys;
for (auto& factor : factors_) { /// Get all the discrete keys in the factor graph, as a set.
for (const DiscreteKey& k : factor->discreteKeys()) { KeySet discreteKeySet() const;
discrete_keys.insert(k.first);
} /// Get a map from Key to corresponding DiscreteKey.
} std::unordered_map<Key, DiscreteKey> discreteKeyMap() const;
return discrete_keys;
}
/// Get all the continuous keys in the factor graph. /// Get all the continuous keys in the factor graph.
const KeySet continuousKeys() const { const KeySet continuousKeySet() const;
KeySet keys;
for (auto& factor : factors_) { /// @}
for (const Key& key : factor->continuousKeys()) {
keys.insert(key);
}
}
return keys;
}
}; };
} // namespace gtsam } // namespace gtsam

70
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -1,70 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridGaussianFactor.cpp
* @date Mar 11, 2022
* @author Fan Jiang
*/
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/* ************************************************************************* */
HybridGaussianFactor::HybridGaussianFactor(
const boost::shared_ptr<GaussianFactor> &ptr)
: Base(ptr->keys()), inner_(ptr) {}
HybridGaussianFactor::HybridGaussianFactor(
boost::shared_ptr<GaussianFactor> &&ptr)
: Base(ptr->keys()), inner_(std::move(ptr)) {}
HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
: Base(jf.keys()),
inner_(boost::make_shared<JacobianFactor>(std::move(jf))) {}
HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf)
: Base(hf.keys()),
inner_(boost::make_shared<HessianFactor>(std::move(hf))) {}
/* ************************************************************************* */
bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const {
const This *e = dynamic_cast<const This *>(&other);
if (e == nullptr) return false;
if (!Base::equals(*e, tol)) return false;
return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false)
: !(e->inner_);
}
/* ************************************************************************* */
void HybridGaussianFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
if (inner_) {
inner_->print("\n", formatter);
} else {
std::cout << "\nGaussian: nullptr" << std::endl;
}
};
/* ************************************************************************ */
double HybridGaussianFactor::error(const HybridValues &values) const {
return inner_->error(values.continuous());
}
/* ************************************************************************ */
} // namespace gtsam

123
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -1,123 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridGaussianFactor.h
* @date Mar 11, 2022
* @author Fan Jiang
*/
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/linear/GaussianFactor.h>
namespace gtsam {
// Forward declarations
class JacobianFactor;
class HessianFactor;
class HybridValues;
/**
* A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
* a diamond inheritance i.e. an extra factor type that inherits from both
* HybridFactor and GaussianFactor.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
private:
GaussianFactor::shared_ptr inner_;
public:
using Base = HybridFactor;
using This = HybridGaussianFactor;
using shared_ptr = boost::shared_ptr<This>;
/// @name Constructors
/// @{
/// Default constructor - for serialization.
HybridGaussianFactor() = default;
/**
* Constructor from shared_ptr of GaussianFactor.
* Example:
* auto ptr = boost::make_shared<JacobianFactor>(...);
* HybridGaussianFactor factor(ptr);
*/
explicit HybridGaussianFactor(const boost::shared_ptr<GaussianFactor> &ptr);
/**
* Forwarding constructor from shared_ptr of GaussianFactor.
* Examples:
* HybridGaussianFactor factor = boost::make_shared<JacobianFactor>(...);
* HybridGaussianFactor factor(boost::make_shared<JacobianFactor>(...));
*/
explicit HybridGaussianFactor(boost::shared_ptr<GaussianFactor> &&ptr);
/**
* Forwarding constructor from rvalue reference of JacobianFactor.
*
* Examples:
* HybridGaussianFactor factor = JacobianFactor(...);
* HybridGaussianFactor factor(JacobianFactor(...));
*/
explicit HybridGaussianFactor(JacobianFactor &&jf);
/**
* Forwarding constructor from rvalue reference of JacobianFactor.
*
* Examples:
* HybridGaussianFactor factor = HessianFactor(...);
* HybridGaussianFactor factor(HessianFactor(...));
*/
explicit HybridGaussianFactor(HessianFactor &&hf);
/// @}
/// @name Testable
/// @{
/// Check equality.
virtual bool equals(const HybridFactor &lf, double tol) const override;
/// GTSAM print utility.
void print(
const std::string &s = "HybridGaussianFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/// Return pointer to the internal Gaussian factor.
GaussianFactor::shared_ptr inner() const { return inner_; }
/// Return the error of the underlying Gaussian factor.
double error(const HybridValues &values) const override;
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(inner_);
}
};
// traits
template <>
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
} // namespace gtsam

202
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -26,10 +26,8 @@
#include <gtsam/hybrid/GaussianMixture.h> #include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridEliminationTree.h> #include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridJunctionTree.h> #include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h> #include <gtsam/inference/EliminateableFactorGraph-inst.h>
@ -47,7 +45,6 @@
#include <iterator> #include <iterator>
#include <memory> #include <memory>
#include <stdexcept> #include <stdexcept>
#include <unordered_map>
#include <utility> #include <utility>
#include <vector> #include <vector>
@ -58,6 +55,27 @@ namespace gtsam {
/// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph: /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph:
template class EliminateableFactorGraph<HybridGaussianFactorGraph>; template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
using boost::dynamic_pointer_cast;
/* ************************************************************************ */
// Throw a runtime exception for method specified in string s, and factor f:
static void throwRuntimeError(const std::string &s,
const boost::shared_ptr<Factor> &f) {
auto &fr = *f;
throw std::runtime_error(s + " not implemented for factor type " +
demangle(typeid(fr).name()) + ".");
}
/* ************************************************************************ */
const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) {
KeySet discrete_keys = graph.discreteKeySet();
const VariableIndex index(graph);
return Ordering::ColamdConstrainedLast(
index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true);
}
/* ************************************************************************ */ /* ************************************************************************ */
static GaussianFactorGraphTree addGaussian( static GaussianFactorGraphTree addGaussian(
const GaussianFactorGraphTree &gfgTree, const GaussianFactorGraphTree &gfgTree,
@ -67,7 +85,6 @@ static GaussianFactorGraphTree addGaussian(
GaussianFactorGraph result; GaussianFactorGraph result;
result.push_back(factor); result.push_back(factor);
return GaussianFactorGraphTree(GraphAndConstant(result, 0.0)); return GaussianFactorGraphTree(GraphAndConstant(result, 0.0));
} else { } else {
auto add = [&factor](const GraphAndConstant &graph_z) { auto add = [&factor](const GraphAndConstant &graph_z) {
auto result = graph_z.graph; auto result = graph_z.graph;
@ -79,9 +96,8 @@ static GaussianFactorGraphTree addGaussian(
} }
/* ************************************************************************ */ /* ************************************************************************ */
// TODO(dellaert): Implementation-wise, it's probably more efficient to first // TODO(dellaert): it's probably more efficient to first collect the discrete
// collect the discrete keys, and then loop over all assignments to populate a // keys, and then loop over all assignments to populate a vector.
// vector.
GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
gttic(assembleGraphTree); gttic(assembleGraphTree);
@ -89,38 +105,28 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
for (auto &f : factors_) { for (auto &f : factors_) {
// TODO(dellaert): just use a virtual method defined in HybridFactor. // TODO(dellaert): just use a virtual method defined in HybridFactor.
if (f->isHybrid()) { if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
if (auto gm = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) { result = addGaussian(result, gf);
} else if (auto gm = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
result = gm->add(result);
} else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
if (auto gm = hc->asMixture()) {
result = gm->add(result); result = gm->add(result);
} else if (auto g = hc->asGaussian()) {
result = addGaussian(result, g);
} else {
// Has to be discrete.
// TODO(dellaert): in C++20, we can use std::visit.
continue;
} }
if (auto gm = boost::dynamic_pointer_cast<HybridConditional>(f)) { } else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
result = gm->asMixture()->add(result);
}
} else if (f->isContinuous()) {
if (auto gf = boost::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
result = addGaussian(result, gf->inner());
}
if (auto cg = boost::dynamic_pointer_cast<HybridConditional>(f)) {
result = addGaussian(result, cg->asGaussian());
}
} else if (f->isDiscrete()) {
// Don't do anything for discrete-only factors // Don't do anything for discrete-only factors
// since we want to eliminate continuous values only. // since we want to eliminate continuous values only.
continue; continue;
} else { } else {
// We need to handle the case where the object is actually an // TODO(dellaert): there was an unattributed comment here: We need to
// BayesTreeOrphanWrapper! // handle the case where the object is actually an BayesTreeOrphanWrapper!
auto orphan = boost::dynamic_pointer_cast< throwRuntimeError("gtsam::assembleGraphTree", f);
BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
if (!orphan) {
auto &fr = *f;
throw std::invalid_argument(
std::string("factor is discrete in continuous elimination ") +
demangle(typeid(fr).name()));
}
} }
} }
@ -130,49 +136,54 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> static std::pair<HybridConditional::shared_ptr, boost::shared_ptr<Factor>>
continuousElimination(const HybridGaussianFactorGraph &factors, continuousElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
for (auto &fp : factors) { for (auto &f : factors) {
if (auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp)) { if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
gfg.push_back(ptr->inner()); gfg.push_back(gf);
} else if (auto ptr = boost::static_pointer_cast<HybridConditional>(fp)) { } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
gfg.push_back( // Ignore orphaned clique.
boost::static_pointer_cast<GaussianConditional>(ptr->inner())); // TODO(dellaert): is this correct? If so explain here.
} else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
auto gc = hc->asGaussian();
if (!gc) throwRuntimeError("continuousElimination", gc);
gfg.push_back(gc);
} else { } else {
// It is an orphan wrapped conditional throwRuntimeError("continuousElimination", f);
} }
} }
auto result = EliminatePreferCholesky(gfg, frontalKeys); auto result = EliminatePreferCholesky(gfg, frontalKeys);
return {boost::make_shared<HybridConditional>(result.first), return {boost::make_shared<HybridConditional>(result.first), result.second};
boost::make_shared<HybridGaussianFactor>(result.second)};
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> static std::pair<HybridConditional::shared_ptr, boost::shared_ptr<Factor>>
discreteElimination(const HybridGaussianFactorGraph &factors, discreteElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto &factor : factors) { for (auto &f : factors) {
if (auto p = boost::dynamic_pointer_cast<HybridDiscreteFactor>(factor)) { if (auto dtf = dynamic_pointer_cast<DecisionTreeFactor>(f)) {
dfg.push_back(p->inner()); dfg.push_back(dtf);
} else if (auto p = boost::static_pointer_cast<HybridConditional>(factor)) { } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
auto discrete_conditional = // Ignore orphaned clique.
boost::static_pointer_cast<DiscreteConditional>(p->inner()); // TODO(dellaert): is this correct? If so explain here.
dfg.push_back(discrete_conditional); } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
auto dc = hc->asDiscrete();
if (!dc) throwRuntimeError("continuousElimination", dc);
dfg.push_back(hc->asDiscrete());
} else { } else {
// It is an orphan wrapper throwRuntimeError("continuousElimination", f);
} }
} }
// NOTE: This does sum-product. For max-product, use EliminateForMPE. // NOTE: This does sum-product. For max-product, use EliminateForMPE.
auto result = EliminateDiscrete(dfg, frontalKeys); auto result = EliminateDiscrete(dfg, frontalKeys);
return {boost::make_shared<HybridConditional>(result.first), return {boost::make_shared<HybridConditional>(result.first), result.second};
boost::make_shared<HybridDiscreteFactor>(result.second)};
} }
/* ************************************************************************ */ /* ************************************************************************ */
@ -190,7 +201,7 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> static std::pair<HybridConditional::shared_ptr, boost::shared_ptr<Factor>>
hybridElimination(const HybridGaussianFactorGraph &factors, hybridElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys, const Ordering &frontalKeys,
const KeyVector &continuousSeparator, const KeyVector &continuousSeparator,
@ -292,7 +303,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt); boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
return {boost::make_shared<HybridConditional>(gaussianMixture), return {boost::make_shared<HybridConditional>(gaussianMixture),
boost::make_shared<HybridDiscreteFactor>(discreteFactor)}; discreteFactor};
} else { } else {
// Create a resulting GaussianMixtureFactor on the separator. // Create a resulting GaussianMixtureFactor on the separator.
return {boost::make_shared<HybridConditional>(gaussianMixture), return {boost::make_shared<HybridConditional>(gaussianMixture),
@ -315,7 +326,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
* eliminate a discrete variable (as specified in the ordering), the result will * eliminate a discrete variable (as specified in the ordering), the result will
* be INCORRECT and there will be NO error raised. * be INCORRECT and there will be NO error raised.
*/ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> // std::pair<HybridConditional::shared_ptr, boost::shared_ptr<Factor>> //
EliminateHybrid(const HybridGaussianFactorGraph &factors, EliminateHybrid(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
// NOTE: Because we are in the Conditional Gaussian regime there are only // NOTE: Because we are in the Conditional Gaussian regime there are only
@ -375,14 +386,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
} }
// Build a map from keys to DiscreteKeys // Build a map from keys to DiscreteKeys
std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey; auto mapFromKeyToDiscreteKey = factors.discreteKeyMap();
for (auto &&factor : factors) {
if (!factor->isContinuous()) {
for (auto &k : factor->discreteKeys()) {
mapFromKeyToDiscreteKey[k.first] = k;
}
}
}
// Fill in discrete frontals and continuous frontals. // Fill in discrete frontals and continuous frontals.
std::set<DiscreteKey> discreteFrontals; std::set<DiscreteKey> discreteFrontals;
@ -428,74 +432,31 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
} }
} }
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(boost::shared_ptr<JacobianFactor> &factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
}
/* ************************************************************************ */
void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
}
/* ************************************************************************ */
const Ordering HybridGaussianFactorGraph::getHybridOrdering() const {
KeySet discrete_keys = discreteKeys();
for (auto &factor : factors_) {
for (const DiscreteKey &k : factor->discreteKeys()) {
discrete_keys.insert(k.first);
}
}
const VariableIndex index(factors_);
Ordering ordering = Ordering::ColamdConstrainedLast(
index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true);
return ordering;
}
/* ************************************************************************ */ /* ************************************************************************ */
AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error( AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
const VectorValues &continuousValues) const { const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree(0.0); AlgebraicDecisionTree<Key> error_tree(0.0);
// Iterate over each factor. // Iterate over each factor.
for (size_t idx = 0; idx < size(); idx++) { for (auto &f : factors_) {
// TODO(dellaert): just use a virtual method defined in HybridFactor. // TODO(dellaert): just use a virtual method defined in HybridFactor.
AlgebraicDecisionTree<Key> factor_error; AlgebraicDecisionTree<Key> factor_error;
if (factors_.at(idx)->isHybrid()) { if (auto gaussianMixture = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
// If factor is hybrid, select based on assignment.
GaussianMixtureFactor::shared_ptr gaussianMixture =
boost::static_pointer_cast<GaussianMixtureFactor>(factors_.at(idx));
// Compute factor error and add it. // Compute factor error and add it.
error_tree = error_tree + gaussianMixture->error(continuousValues); error_tree = error_tree + gaussianMixture->error(continuousValues);
} else if (auto gaussian = dynamic_pointer_cast<GaussianFactor>(f)) {
} else if (factors_.at(idx)->isContinuous()) {
// If continuous only, get the (double) error // If continuous only, get the (double) error
// and add it to the error_tree // and add it to the error_tree
auto hybridGaussianFactor =
boost::static_pointer_cast<HybridGaussianFactor>(factors_.at(idx));
GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner();
// Compute the error of the gaussian factor.
double error = gaussian->error(continuousValues); double error = gaussian->error(continuousValues);
// Add the gaussian factor error to every leaf of the error tree. // Add the gaussian factor error to every leaf of the error tree.
error_tree = error_tree.apply( error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; }); [error](double leaf_value) { return leaf_value + error; });
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
} else if (factors_.at(idx)->isDiscrete()) {
// If factor at `idx` is discrete-only, we skip. // If factor at `idx` is discrete-only, we skip.
continue; continue;
} else {
throwRuntimeError("HybridGaussianFactorGraph::error(VV)", f);
} }
} }
@ -505,8 +466,17 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
/* ************************************************************************ */ /* ************************************************************************ */
double HybridGaussianFactorGraph::error(const HybridValues &values) const { double HybridGaussianFactorGraph::error(const HybridValues &values) const {
double error = 0.0; double error = 0.0;
for (auto &factor : factors_) { for (auto &f : factors_) {
error += factor->error(values); if (auto hf = dynamic_pointer_cast<GaussianFactor>(f)) {
error += hf->error(values.continuous());
} else if (auto hf = dynamic_pointer_cast<HybridFactor>(f)) {
// TODO(dellaert): needs to change when we discard other wrappers.
error += hf->error(values);
} else if (auto dtf = dynamic_pointer_cast<DecisionTreeFactor>(f)) {
error -= log((*dtf)(values.discrete()));
} else {
throwRuntimeError("HybridGaussianFactorGraph::error(HV)", f);
}
} }
return error; return error;
} }

89
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -21,7 +21,6 @@
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/EliminateableFactorGraph.h> #include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
@ -50,13 +49,22 @@ class HybridValues;
* @ingroup hybrid * @ingroup hybrid
*/ */
GTSAM_EXPORT GTSAM_EXPORT
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr> std::pair<boost::shared_ptr<HybridConditional>, boost::shared_ptr<Factor>>
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
/**
* @brief Return a Colamd constrained ordering where the discrete keys are
* eliminated after the continuous keys.
*
* @return const Ordering
*/
GTSAM_EXPORT const Ordering
HybridOrdering(const HybridGaussianFactorGraph& graph);
/* ************************************************************************* */ /* ************************************************************************* */
template <> template <>
struct EliminationTraits<HybridGaussianFactorGraph> { struct EliminationTraits<HybridGaussianFactorGraph> {
typedef HybridFactor FactorType; ///< Type of factors in factor graph typedef Factor FactorType; ///< Type of factors in factor graph
typedef HybridGaussianFactorGraph typedef HybridGaussianFactorGraph
FactorGraphType; ///< Type of the factor graph (e.g. FactorGraphType; ///< Type of the factor graph (e.g.
///< HybridGaussianFactorGraph) ///< HybridGaussianFactorGraph)
@ -70,17 +78,22 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function /// The default dense elimination function
static std::pair<boost::shared_ptr<ConditionalType>, static std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> > boost::shared_ptr<FactorType>>
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminateHybrid(factors, keys); return EliminateHybrid(factors, keys);
} }
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
return HybridOrdering(graph);
}
}; };
/** /**
* Hybrid Gaussian Factor Graph * Hybrid Gaussian Factor Graph
* ----------------------- * -----------------------
* This is the linearized version of a hybrid factor graph. * This is the linearized version of a hybrid factor graph.
* Everything inside needs to be hybrid factor or hybrid conditional.
* *
* @ingroup hybrid * @ingroup hybrid
*/ */
@ -118,59 +131,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
: Base(graph) {} : Base(graph) {}
/// @}
/// @name Adding factors.
/// @{
using Base::add;
using Base::push_back;
using Base::reserve;
/// Add a Jacobian factor to the factor graph.
void add(JacobianFactor&& factor);
/// Add a Jacobian factor as a shared ptr.
void add(boost::shared_ptr<JacobianFactor>& factor);
/// Add a DecisionTreeFactor to the factor graph.
void add(DecisionTreeFactor&& factor);
/// Add a DecisionTreeFactor as a shared ptr.
void add(DecisionTreeFactor::shared_ptr 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);
}
}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -185,11 +145,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
using Base::empty;
using Base::size;
using Base::operator[];
using Base::resize;
/** /**
* @brief Compute error for each discrete assignment, * @brief Compute error for each discrete assignment,
* and return as a tree. * and return as a tree.
@ -228,14 +183,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
*/ */
double probPrime(const HybridValues& values) const; double probPrime(const HybridValues& values) const;
/**
* @brief Return a Colamd constrained ordering where the discrete keys are
* eliminated after the continuous keys.
*
* @return const Ordering
*/
const Ordering getHybridOrdering() const;
/** /**
* @brief Create a decision tree of factor graphs out of this hybrid factor * @brief Create a decision tree of factor graphs out of this hybrid factor
* graph. * graph.

2
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -43,7 +43,7 @@ Ordering HybridGaussianISAM::GetOrdering(
HybridGaussianFactorGraph& factors, HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors) { const HybridGaussianFactorGraph& newFactors) {
// Get all the discrete keys from the factors // Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeys(); const KeySet allDiscrete = factors.discreteKeySet();
// Create KeyVector with continuous keys followed by discrete keys. // Create KeyVector with continuous keys followed by discrete keys.
KeyVector newKeysDiscreteLast; KeyVector newKeysDiscreteLast;

12
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -61,9 +61,15 @@ struct HybridConstructorTraversalData {
parentData.junctionTreeNode->addChild(data.junctionTreeNode); parentData.junctionTreeNode->addChild(data.junctionTreeNode);
// Add all the discrete keys in the hybrid factors to the current data // Add all the discrete keys in the hybrid factors to the current data
for (HybridFactor::shared_ptr& f : node->factors) { for (const auto& f : node->factors) {
for (auto& k : f->discreteKeys()) { if (auto hf = boost::dynamic_pointer_cast<HybridFactor>(f)) {
data.discreteKeys.insert(k.first); for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first);
}
} else if (auto hf = boost::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first);
}
} }
} }

41
gtsam/hybrid/HybridNonlinearFactor.cpp
View File

@ -1,41 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridNonlinearFactor.cpp
* @date May 28, 2022
* @author Varun Agrawal
*/
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/* ************************************************************************* */
HybridNonlinearFactor::HybridNonlinearFactor(
const NonlinearFactor::shared_ptr &other)
: Base(other->keys()), inner_(other) {}
/* ************************************************************************* */
bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const {
return Base::equals(lf, tol);
}
/* ************************************************************************* */
void HybridNonlinearFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
inner_->print("\n", formatter);
};
} // namespace gtsam

73
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -1,73 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridNonlinearFactor.h
* @date May 28, 2022
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not
* have a diamond inheritance.
*/
class HybridNonlinearFactor : public HybridFactor {
private:
NonlinearFactor::shared_ptr inner_;
public:
using Base = HybridFactor;
using This = HybridNonlinearFactor;
using shared_ptr = boost::shared_ptr<This>;
// Explicit conversion from a shared ptr of NonlinearFactor
explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other);
/// @name Testable
/// @{
/// Check equality.
virtual bool equals(const HybridFactor &lf, double tol) const override;
/// GTSAM print utility.
void print(
const std::string &s = "HybridNonlinearFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/// Return pointer to the internal nonlinear factor.
NonlinearFactor::shared_ptr inner() const { return inner_; }
/// Error for HybridValues is not provided for nonlinear factor.
double error(const HybridValues &values) const override {
throw std::runtime_error(
"HybridNonlinearFactor::error(HybridValues) not implemented.");
}
/// Linearize to a HybridGaussianFactor at the linearization point `c`.
boost::shared_ptr<HybridGaussianFactor> linearize(const Values &c) const {
return boost::make_shared<HybridGaussianFactor>(inner_->linearize(c));
}
/// @}
};
} // namespace gtsam

72
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -16,21 +16,14 @@
* @date May 28, 2022 * @date May 28, 2022
*/ */
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h> #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
void HybridNonlinearFactorGraph::add(
boost::shared_ptr<NonlinearFactor> factor) {
FactorGraph::add(boost::make_shared<HybridNonlinearFactor>(factor));
}
/* ************************************************************************* */
void HybridNonlinearFactorGraph::add(boost::shared_ptr<DiscreteFactor> factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
}
/* ************************************************************************* */ /* ************************************************************************* */
void HybridNonlinearFactorGraph::print(const std::string& s, void HybridNonlinearFactorGraph::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
@ -50,47 +43,38 @@ void HybridNonlinearFactorGraph::print(const std::string& s,
/* ************************************************************************* */ /* ************************************************************************* */
HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
const Values& continuousValues) const { const Values& continuousValues) const {
using boost::dynamic_pointer_cast;
// create an empty linear FG // create an empty linear FG
auto linearFG = boost::make_shared<HybridGaussianFactorGraph>(); auto linearFG = boost::make_shared<HybridGaussianFactorGraph>();
linearFG->reserve(size()); linearFG->reserve(size());
// linearize all hybrid factors // linearize all hybrid factors
for (auto&& factor : factors_) { for (auto& f : factors_) {
// First check if it is a valid factor // First check if it is a valid factor
if (factor) { if (!f) {
// Check if the factor is a hybrid factor. // TODO(dellaert): why?
// It can be either a nonlinear MixtureFactor or a linear
// GaussianMixtureFactor.
if (factor->isHybrid()) {
// Check if it is a nonlinear mixture factor
if (auto nlmf = boost::dynamic_pointer_cast<MixtureFactor>(factor)) {
linearFG->push_back(nlmf->linearize(continuousValues));
} else {
linearFG->push_back(factor);
}
// Now check if the factor is a continuous only factor.
} else if (factor->isContinuous()) {
// In this case, we check if factor->inner() is nonlinear since
// HybridFactors wrap over continuous factors.
auto nlhf = boost::dynamic_pointer_cast<HybridNonlinearFactor>(factor);
if (auto nlf =
boost::dynamic_pointer_cast<NonlinearFactor>(nlhf->inner())) {
auto hgf = boost::make_shared<HybridGaussianFactor>(
nlf->linearize(continuousValues));
linearFG->push_back(hgf);
} else {
linearFG->push_back(factor);
}
// Finally if nothing else, we are discrete-only which doesn't need
// lineariztion.
} else {
linearFG->push_back(factor);
}
} else {
linearFG->push_back(GaussianFactor::shared_ptr()); linearFG->push_back(GaussianFactor::shared_ptr());
continue;
}
// Check if it is a nonlinear mixture factor
if (auto mf = dynamic_pointer_cast<MixtureFactor>(f)) {
const GaussianMixtureFactor::shared_ptr& gmf =
mf->linearize(continuousValues);
linearFG->push_back(gmf);
} else if (auto nlf = dynamic_pointer_cast<NonlinearFactor>(f)) {
const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues);
linearFG->push_back(gf);
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// If discrete-only: doesn't need linearization.
linearFG->push_back(f);
} else {
auto& fr = *f;
throw std::invalid_argument(
std::string("HybridNonlinearFactorGraph::linearize: factor type "
"not handled: ") +
demangle(typeid(fr).name()));
} }
} }
return linearFG; return linearFG;

88
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -18,17 +18,12 @@
#pragma once #pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/format.hpp>
namespace gtsam { namespace gtsam {
class HybridGaussianFactorGraph;
/** /**
* Nonlinear Hybrid Factor Graph * Nonlinear Hybrid Factor Graph
* ----------------------- * -----------------------
@ -37,21 +32,6 @@ namespace gtsam {
*/ */
class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
protected: protected:
/// Check if FACTOR type is derived from NonlinearFactor.
template <typename FACTOR>
using IsNonlinear = typename std::enable_if<
std::is_base_of<NonlinearFactor, FACTOR>::value>::type;
/// Check if T has a value_type derived from FactorType.
template <typename T>
using HasDerivedValueType = typename std::enable_if<
std::is_base_of<HybridFactor, typename T::value_type>::value>::type;
/// Check if T has a pointer type derived from FactorType.
template <typename T>
using HasDerivedElementType = typename std::enable_if<std::is_base_of<
HybridFactor, typename T::value_type::element_type>::value>::type;
public: public:
using Base = HybridFactorGraph; using Base = HybridFactorGraph;
using This = HybridNonlinearFactorGraph; ///< this class using This = HybridNonlinearFactorGraph; ///< this class
@ -76,70 +56,6 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
/// @} /// @}
// Allow use of selected FactorGraph methods:
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::resize;
/**
* Add a nonlinear factor *pointer* to the internal nonlinear factor graph
* @param nonlinearFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsNonlinear<FACTOR> push_nonlinear(
const boost::shared_ptr<FACTOR>& nonlinearFactor) {
Base::push_back(boost::make_shared<HybridNonlinearFactor>(nonlinearFactor));
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsNonlinear<FACTOR> emplace_nonlinear(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_nonlinear(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.
*
* @tparam FACTOR The factor type template
* @param sharedFactor The factor to add to this factor graph.
*/
template <typename FACTOR>
void push_back(const boost::shared_ptr<FACTOR>& sharedFactor) {
if (auto p = boost::dynamic_pointer_cast<NonlinearFactor>(sharedFactor)) {
push_nonlinear(p);
} else {
Base::push_back(sharedFactor);
}
}
/**
* Push back many factors as shared_ptr's in a container (factors are not
* copied)
*/
template <typename CONTAINER>
HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
Base::push_back(container.begin(), container.end());
}
/// Push back non-pointer objects in a container (factors are copied).
template <typename CONTAINER>
HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
Base::push_back(container.begin(), container.end());
}
/// Add a nonlinear factor as a shared ptr.
void add(boost::shared_ptr<NonlinearFactor> factor);
/// Add a discrete factor as a shared ptr.
void add(boost::shared_ptr<DiscreteFactor> factor);
/// Print the factor graph. /// Print the factor graph.
void print( void print(
const std::string& s = "HybridNonlinearFactorGraph", const std::string& s = "HybridNonlinearFactorGraph",

1
gtsam/hybrid/MixtureFactor.h
View File

@ -21,7 +21,6 @@
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>

15
gtsam/hybrid/hybrid.i
View File

@ -68,17 +68,6 @@ virtual class HybridConditional {
double error(const gtsam::HybridValues& values) const; double error(const gtsam::HybridValues& values) const;
}; };
#include <gtsam/hybrid/HybridDiscreteFactor.h>
virtual class HybridDiscreteFactor {
HybridDiscreteFactor(gtsam::DecisionTreeFactor dtf);
void print(string s = "HybridDiscreteFactor\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::HybridDiscreteFactor& other, double tol = 1e-9) const;
gtsam::Factor* inner();
double error(const gtsam::HybridValues &values) const;
};
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
class GaussianMixtureFactor : gtsam::HybridFactor { class GaussianMixtureFactor : gtsam::HybridFactor {
GaussianMixtureFactor( GaussianMixtureFactor(
@ -217,9 +206,7 @@ class HybridNonlinearFactorGraph {
HybridNonlinearFactorGraph(const gtsam::HybridNonlinearFactorGraph& graph); HybridNonlinearFactorGraph(const gtsam::HybridNonlinearFactorGraph& graph);
void push_back(gtsam::HybridFactor* factor); void push_back(gtsam::HybridFactor* factor);
void push_back(gtsam::NonlinearFactor* factor); void push_back(gtsam::NonlinearFactor* factor);
void push_back(gtsam::HybridDiscreteFactor* factor); void push_back(gtsam::DiscreteFactor* factor);
void add(gtsam::NonlinearFactor* factor);
void add(gtsam::DiscreteFactor* factor);
gtsam::HybridGaussianFactorGraph linearize(const gtsam::Values& continuousValues) const; gtsam::HybridGaussianFactorGraph linearize(const gtsam::Values& continuousValues) const;
bool empty() const; bool empty() const;

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

@ -136,6 +136,8 @@ struct Switching {
std::vector<double> measurements = {}, std::vector<double> measurements = {},
std::string discrete_transition_prob = "1/2 3/2") std::string discrete_transition_prob = "1/2 3/2")
: K(K) { : K(K) {
using noiseModel::Isotropic;
// Create DiscreteKeys for binary K modes. // Create DiscreteKeys for binary K modes.
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
modes.emplace_back(M(k), 2); modes.emplace_back(M(k), 2);
@ -150,9 +152,8 @@ struct Switching {
// Create hybrid factor graph. // Create hybrid factor graph.
// Add a prior on X(0). // Add a prior on X(0).
auto prior = boost::make_shared<PriorFactor<double>>( nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
X(0), measurements.at(0), noiseModel::Isotropic::Sigma(1, prior_sigma)); X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
nonlinearFactorGraph.push_nonlinear(prior);
// Add "motion models". // Add "motion models".
for (size_t k = 0; k < K - 1; k++) { for (size_t k = 0; k < K - 1; k++) {
@ -162,14 +163,14 @@ struct Switching {
for (auto &&f : motion_models) { for (auto &&f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f)); components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
} }
nonlinearFactorGraph.emplace_hybrid<MixtureFactor>( nonlinearFactorGraph.emplace_shared<MixtureFactor>(
keys, DiscreteKeys{modes[k]}, components); keys, DiscreteKeys{modes[k]}, components);
} }
// Add measurement factors // Add measurement factors
auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma); auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
for (size_t k = 1; k < K; k++) { for (size_t k = 1; k < K; k++) {
nonlinearFactorGraph.emplace_nonlinear<PriorFactor<double>>( nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
X(k), measurements.at(k), measurement_noise); X(k), measurements.at(k), measurement_noise);
} }
@ -205,13 +206,11 @@ struct Switching {
*/ */
void addModeChain(HybridNonlinearFactorGraph *fg, void addModeChain(HybridNonlinearFactorGraph *fg,
std::string discrete_transition_prob = "1/2 3/2") { std::string discrete_transition_prob = "1/2 3/2") {
auto prior = boost::make_shared<DiscreteDistribution>(modes[0], "1/1"); fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
fg->push_discrete(prior);
for (size_t k = 0; k < K - 2; k++) { for (size_t k = 0; k < K - 2; k++) {
auto parents = {modes[k]}; auto parents = {modes[k]};
auto conditional = boost::make_shared<DiscreteConditional>( fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
modes[k + 1], parents, discrete_transition_prob); discrete_transition_prob);
fg->push_discrete(conditional);
} }
} }
@ -223,13 +222,11 @@ struct Switching {
*/ */
void addModeChain(HybridGaussianFactorGraph *fg, void addModeChain(HybridGaussianFactorGraph *fg,
std::string discrete_transition_prob = "1/2 3/2") { std::string discrete_transition_prob = "1/2 3/2") {
auto prior = boost::make_shared<DiscreteDistribution>(modes[0], "1/1"); fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
fg->push_discrete(prior);
for (size_t k = 0; k < K - 2; k++) { for (size_t k = 0; k < K - 2; k++) {
auto parents = {modes[k]}; auto parents = {modes[k]};
auto conditional = boost::make_shared<DiscreteConditional>( fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
modes[k + 1], parents, discrete_transition_prob); discrete_transition_prob);
fg->push_discrete(conditional);
} }
} }
}; };

25
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -93,8 +93,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
// Create hybrid Bayes net. // Create hybrid Bayes net.
HybridBayesNet bayesNet; HybridBayesNet bayesNet;
bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( bayesNet.push_back(continuousConditional);
X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0));
bayesNet.emplace_back( bayesNet.emplace_back(
new GaussianMixture({X(1)}, {}, {Asia}, {conditional0, conditional1})); new GaussianMixture({X(1)}, {}, {Asia}, {conditional0, conditional1}));
bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1")); bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1"));
@ -185,9 +184,8 @@ TEST(HybridBayesNet, OptimizeAssignment) {
TEST(HybridBayesNet, Optimize) { TEST(HybridBayesNet, Optimize) {
Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1"); Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1");
Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential(hybridOrdering); s.linearizedFactorGraph.eliminateSequential();
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
@ -212,9 +210,8 @@ TEST(HybridBayesNet, Optimize) {
TEST(HybridBayesNet, Error) { TEST(HybridBayesNet, Error) {
Switching s(3); Switching s(3);
Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential(hybridOrdering); s.linearizedFactorGraph.eliminateSequential();
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
auto error_tree = hybridBayesNet->error(delta.continuous()); auto error_tree = hybridBayesNet->error(delta.continuous());
@ -266,9 +263,8 @@ TEST(HybridBayesNet, Error) {
TEST(HybridBayesNet, Prune) { TEST(HybridBayesNet, Prune) {
Switching s(4); Switching s(4);
Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential(hybridOrdering); s.linearizedFactorGraph.eliminateSequential();
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
@ -284,9 +280,8 @@ TEST(HybridBayesNet, Prune) {
TEST(HybridBayesNet, UpdateDiscreteConditionals) { TEST(HybridBayesNet, UpdateDiscreteConditionals) {
Switching s(4); Switching s(4);
Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential(hybridOrdering); s.linearizedFactorGraph.eliminateSequential();
size_t maxNrLeaves = 3; size_t maxNrLeaves = 3;
auto discreteConditionals = hybridBayesNet->discreteConditionals(); auto discreteConditionals = hybridBayesNet->discreteConditionals();
@ -337,13 +332,12 @@ TEST(HybridBayesNet, Sampling) {
auto one_motion = auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion}; std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_hybrid<MixtureFactor>( nfg.emplace_shared<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
DiscreteKey mode(M(0), 2); DiscreteKey mode(M(0), 2);
auto discrete_prior = boost::make_shared<DiscreteDistribution>(mode, "1/1"); nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");
nfg.push_discrete(discrete_prior);
Values initial; Values initial;
double z0 = 0.0, z1 = 1.0; double z0 = 0.0, z1 = 1.0;
@ -353,8 +347,7 @@ TEST(HybridBayesNet, Sampling) {
// Create the factor graph from the nonlinear factor graph. // Create the factor graph from the nonlinear factor graph.
HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial); HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial);
// Eliminate into BN // Eliminate into BN
Ordering ordering = fg->getHybridOrdering(); HybridBayesNet::shared_ptr bn = fg->eliminateSequential();
HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering);
// Set up sampling // Set up sampling
std::mt19937_64 gen(11); std::mt19937_64 gen(11);

21
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -37,9 +37,8 @@ using symbol_shorthand::X;
TEST(HybridBayesTree, OptimizeMultifrontal) { TEST(HybridBayesTree, OptimizeMultifrontal) {
Switching s(4); Switching s(4);
Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
HybridBayesTree::shared_ptr hybridBayesTree = HybridBayesTree::shared_ptr hybridBayesTree =
s.linearizedFactorGraph.eliminateMultifrontal(hybridOrdering); s.linearizedFactorGraph.eliminateMultifrontal();
HybridValues delta = hybridBayesTree->optimize(); HybridValues delta = hybridBayesTree->optimize();
VectorValues expectedValues; VectorValues expectedValues;
@ -151,9 +150,9 @@ TEST(HybridBayesTree, Optimize) {
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto&& f : *remainingFactorGraph) { for (auto&& f : *remainingFactorGraph) {
auto factor = dynamic_pointer_cast<HybridDiscreteFactor>(f); auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(f);
dfg.push_back( assert(discreteFactor);
boost::dynamic_pointer_cast<DecisionTreeFactor>(factor->inner())); dfg.push_back(discreteFactor);
} }
// Add the probabilities for each branch // Add the probabilities for each branch
@ -203,16 +202,8 @@ TEST(HybridBayesTree, Choose) {
GaussianBayesTree gbt = isam.choose(assignment); GaussianBayesTree gbt = isam.choose(assignment);
Ordering ordering; // Specify ordering so it matches that of HybridGaussianISAM.
ordering += X(0); Ordering ordering(KeyVector{X(0), X(1), X(2), X(3), M(0), M(1), M(2)});
ordering += X(1);
ordering += X(2);
ordering += X(3);
ordering += M(0);
ordering += M(1);
ordering += M(2);
// TODO(Varun) get segfault if ordering not provided
auto bayesTree = s.linearizedFactorGraph.eliminateMultifrontal(ordering); auto bayesTree = s.linearizedFactorGraph.eliminateMultifrontal(ordering);
auto expected_gbt = bayesTree->choose(assignment); auto expected_gbt = bayesTree->choose(assignment);

23
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -46,7 +46,7 @@ Ordering getOrdering(HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors) { const HybridGaussianFactorGraph& newFactors) {
factors += newFactors; factors += newFactors;
// Get all the discrete keys from the factors // Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeys(); KeySet allDiscrete = factors.discreteKeySet();
// Create KeyVector with continuous keys followed by discrete keys. // Create KeyVector with continuous keys followed by discrete keys.
KeyVector newKeysDiscreteLast; KeyVector newKeysDiscreteLast;
@ -90,7 +90,7 @@ TEST(HybridEstimation, Full) {
} }
HybridBayesNet::shared_ptr bayesNet = HybridBayesNet::shared_ptr bayesNet =
graph.eliminateSequential(hybridOrdering); graph.eliminateSequential();
EXPECT_LONGS_EQUAL(2 * K - 1, bayesNet->size()); EXPECT_LONGS_EQUAL(2 * K - 1, bayesNet->size());
@ -241,7 +241,7 @@ AlgebraicDecisionTree<Key> getProbPrimeTree(
const HybridGaussianFactorGraph& graph) { const HybridGaussianFactorGraph& graph) {
HybridBayesNet::shared_ptr bayesNet; HybridBayesNet::shared_ptr bayesNet;
HybridGaussianFactorGraph::shared_ptr remainingGraph; HybridGaussianFactorGraph::shared_ptr remainingGraph;
Ordering continuous(graph.continuousKeys()); Ordering continuous(graph.continuousKeySet());
std::tie(bayesNet, remainingGraph) = std::tie(bayesNet, remainingGraph) =
graph.eliminatePartialSequential(continuous); graph.eliminatePartialSequential(continuous);
@ -296,14 +296,14 @@ TEST(HybridEstimation, Probability) {
auto graph = switching.linearizedFactorGraph; auto graph = switching.linearizedFactorGraph;
// Continuous elimination // Continuous elimination
Ordering continuous_ordering(graph.continuousKeys()); Ordering continuous_ordering(graph.continuousKeySet());
HybridBayesNet::shared_ptr bayesNet; HybridBayesNet::shared_ptr bayesNet;
HybridGaussianFactorGraph::shared_ptr discreteGraph; HybridGaussianFactorGraph::shared_ptr discreteGraph;
std::tie(bayesNet, discreteGraph) = std::tie(bayesNet, discreteGraph) =
graph.eliminatePartialSequential(continuous_ordering); graph.eliminatePartialSequential(continuous_ordering);
// Discrete elimination // Discrete elimination
Ordering discrete_ordering(graph.discreteKeys()); Ordering discrete_ordering(graph.discreteKeySet());
auto discreteBayesNet = discreteGraph->eliminateSequential(discrete_ordering); auto discreteBayesNet = discreteGraph->eliminateSequential(discrete_ordering);
// Add the discrete conditionals to make it a full bayes net. // Add the discrete conditionals to make it a full bayes net.
@ -346,7 +346,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
AlgebraicDecisionTree<Key> expected_probPrimeTree = getProbPrimeTree(graph); AlgebraicDecisionTree<Key> expected_probPrimeTree = getProbPrimeTree(graph);
// Eliminate continuous // Eliminate continuous
Ordering continuous_ordering(graph.continuousKeys()); Ordering continuous_ordering(graph.continuousKeySet());
HybridBayesTree::shared_ptr bayesTree; HybridBayesTree::shared_ptr bayesTree;
HybridGaussianFactorGraph::shared_ptr discreteGraph; HybridGaussianFactorGraph::shared_ptr discreteGraph;
std::tie(bayesTree, discreteGraph) = std::tie(bayesTree, discreteGraph) =
@ -358,7 +358,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); auto last_conditional = (*bayesTree)[last_continuous_key]->conditional();
DiscreteKeys discrete_keys = last_conditional->discreteKeys(); DiscreteKeys discrete_keys = last_conditional->discreteKeys();
Ordering discrete(graph.discreteKeys()); Ordering discrete(graph.discreteKeySet());
auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete); auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete);
EXPECT_LONGS_EQUAL(1, discreteBayesTree->size()); EXPECT_LONGS_EQUAL(1, discreteBayesTree->size());
@ -407,8 +407,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
// Add "measurement" factors: // Add "measurement" factors:
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_nonlinear<PriorFactor<double>>(X(1), 1.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(1), 1.0, noise_model);
// Add mixture factor: // Add mixture factor:
DiscreteKey m(M(0), 2); DiscreteKey m(M(0), 2);
@ -416,7 +416,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion = const auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_hybrid<MixtureFactor>( nfg.emplace_shared<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{m}, KeyVector{X(0), X(1)}, DiscreteKeys{m},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion}); std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
@ -481,8 +481,7 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
const auto fg = createHybridGaussianFactorGraph(); const auto fg = createHybridGaussianFactorGraph();
// 2. Eliminate into BN // 2. Eliminate into BN
const Ordering ordering = fg->getHybridOrdering(); const HybridBayesNet::shared_ptr bn = fg->eliminateSequential();
const HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering);
// Set up sampling // Set up sampling
std::mt19937_64 rng(11); std::mt19937_64 rng(11);

57
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -26,9 +26,7 @@
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianISAM.h> #include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridValues.h> #include <gtsam/hybrid/HybridValues.h>
@ -65,7 +63,7 @@ TEST(HybridGaussianFactorGraph, Creation) {
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1))); hfg.emplace_shared<JacobianFactor>(X(0), I_3x3, Z_3x1);
// Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
// graph // graph
@ -86,7 +84,7 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) {
// Test elimination of a single variable. // Test elimination of a single variable.
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); hfg.emplace_shared<JacobianFactor>(0, I_3x3, Z_3x1);
auto result = hfg.eliminatePartialSequential(KeyVector{0}); auto result = hfg.eliminatePartialSequential(KeyVector{0});
@ -102,7 +100,7 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
// Add priors on x0 and c1 // Add priors on x0 and c1
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); hfg.add(DecisionTreeFactor(m, {2, 8}));
Ordering ordering; Ordering ordering;
ordering.push_back(X(0)); ordering.push_back(X(0));
@ -116,24 +114,23 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;
DiscreteKey m1(M(1), 2);
// Add prior on x0 // Add prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Add factor between x0 and x1 // Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Add a gaussian mixture factor ϕ(x1, c1) // Add a gaussian mixture factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactor::shared_ptr> dt( DecisionTree<Key, GaussianFactor::shared_ptr> dt(
M(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), M(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())); boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt)); hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt));
auto result = auto result = hfg.eliminateSequential();
hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {M(1)}));
auto dc = result->at(2)->asDiscrete(); auto dc = result->at(2)->asDiscrete();
CHECK(dc);
DiscreteValues dv; DiscreteValues dv;
dv[M(1)] = 0; dv[M(1)] = 0;
// Regression test // Regression test
@ -161,8 +158,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
// Joint discrete probability table for c1, c2 // Joint discrete probability table for c1, c2
hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
HybridBayesNet::shared_ptr result = hfg.eliminateSequential( HybridBayesNet::shared_ptr result = hfg.eliminateSequential();
Ordering::ColamdConstrainedLast(hfg, {M(1), M(2)}));
// There are 4 variables (2 continuous + 2 discrete) in the bayes net. // There are 4 variables (2 continuous + 2 discrete) in the bayes net.
EXPECT_LONGS_EQUAL(4, result->size()); EXPECT_LONGS_EQUAL(4, result->size());
@ -187,8 +183,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
// variable throws segfault // variable throws segfault
// hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); // hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
HybridBayesTree::shared_ptr result = HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal();
hfg.eliminateMultifrontal(hfg.getHybridOrdering());
// The bayes tree should have 3 cliques // The bayes tree should have 3 cliques
EXPECT_LONGS_EQUAL(3, result->size()); EXPECT_LONGS_EQUAL(3, result->size());
@ -215,10 +210,10 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
// Hybrid factor P(x1|c1) // Hybrid factor P(x1|c1)
hfg.add(GaussianMixtureFactor({X(1)}, {m}, dt)); hfg.add(GaussianMixtureFactor({X(1)}, {m}, dt));
// Prior factor on c1 // Prior factor on c1
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); hfg.add(DecisionTreeFactor(m, {2, 8}));
// Get a constrained ordering keeping c1 last // Get a constrained ordering keeping c1 last
auto ordering_full = hfg.getHybridOrdering(); auto ordering_full = HybridOrdering(hfg);
// Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1) // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full); HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
@ -250,8 +245,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(GaussianMixtureFactor({X(2)}, {{M(1), 2}}, dt1)); hfg.add(GaussianMixtureFactor({X(2)}, {{M(1), 2}}, dt1));
} }
hfg.add(HybridDiscreteFactor( hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")));
hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
@ -518,8 +512,7 @@ TEST(HybridGaussianFactorGraph, optimize) {
hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
auto result = auto result = hfg.eliminateSequential();
hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
HybridValues hv = result->optimize(); HybridValues hv = result->optimize();
@ -572,9 +565,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
HybridGaussianFactorGraph graph = s.linearizedFactorGraph; HybridGaussianFactorGraph graph = s.linearizedFactorGraph;
Ordering hybridOrdering = graph.getHybridOrdering(); HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
HybridBayesNet::shared_ptr hybridBayesNet =
graph.eliminateSequential(hybridOrdering);
const HybridValues delta = hybridBayesNet->optimize(); const HybridValues delta = hybridBayesNet->optimize();
const double error = graph.error(delta); const double error = graph.error(delta);
@ -593,9 +584,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) {
HybridGaussianFactorGraph graph = s.linearizedFactorGraph; HybridGaussianFactorGraph graph = s.linearizedFactorGraph;
Ordering hybridOrdering = graph.getHybridOrdering(); HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
HybridBayesNet::shared_ptr hybridBayesNet =
graph.eliminateSequential(hybridOrdering);
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
auto error_tree = graph.error(delta.continuous()); auto error_tree = graph.error(delta.continuous());
@ -684,10 +673,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
expectedBayesNet.emplace_back(new DiscreteConditional(mode, "74/26")); expectedBayesNet.emplace_back(new DiscreteConditional(mode, "74/26"));
// Test elimination // Test elimination
Ordering ordering; const auto posterior = fg.eliminateSequential();
ordering.push_back(X(0));
ordering.push_back(M(0));
const auto posterior = fg.eliminateSequential(ordering);
EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01)); EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
} }
@ -719,10 +705,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
expectedBayesNet.emplace_back(new DiscreteConditional(mode, "23/77")); expectedBayesNet.emplace_back(new DiscreteConditional(mode, "23/77"));
// Test elimination // Test elimination
Ordering ordering; const auto posterior = fg.eliminateSequential();
ordering.push_back(X(0));
ordering.push_back(M(0));
const auto posterior = fg.eliminateSequential(ordering);
EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01)); EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
} }
@ -741,11 +724,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny22) {
EXPECT_LONGS_EQUAL(5, fg.size()); EXPECT_LONGS_EQUAL(5, fg.size());
// Test elimination // Test elimination
Ordering ordering; const auto posterior = fg.eliminateSequential();
ordering.push_back(X(0));
ordering.push_back(M(0));
ordering.push_back(M(1));
const auto posterior = fg.eliminateSequential(ordering);
// Compute the log-ratio between the Bayes net and the factor graph. // Compute the log-ratio between the Bayes net and the factor graph.
auto compute_ratio = [&](HybridValues *sample) -> double { auto compute_ratio = [&](HybridValues *sample) -> double {

32
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -380,7 +380,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise); fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
@ -389,11 +389,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
// where X is the base link and W is the foot link. // where X is the base link and W is the foot link.
// Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
// Create initial estimate // Create initial estimate
@ -432,14 +432,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise); poseNoise);
initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
@ -472,14 +472,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
poseNoise); poseNoise);
initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
@ -515,14 +515,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=3 // PoseFactors-like at k=3
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
poseNoise); poseNoise);
initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(X(3), Pose2(3.0, 0.0, 0.0));

47
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -54,7 +54,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
HybridNonlinearFactorGraph fg; HybridNonlinearFactorGraph fg;
// Add a simple prior factor to the nonlinear factor graph // Add a simple prior factor to the nonlinear factor graph
fg.emplace_nonlinear<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1)); fg.emplace_shared<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1));
// Linearization point // Linearization point
Values linearizationPoint; Values linearizationPoint;
@ -311,8 +311,7 @@ TEST(HybridsGaussianElimination, Eliminate_x1) {
Ordering ordering; Ordering ordering;
ordering += X(1); ordering += X(1);
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> result = auto result = EliminateHybrid(factors, ordering);
EliminateHybrid(factors, ordering);
CHECK(result.first); CHECK(result.first);
EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
CHECK(result.second); CHECK(result.second);
@ -350,7 +349,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
ordering += X(1); ordering += X(1);
HybridConditional::shared_ptr hybridConditionalMixture; HybridConditional::shared_ptr hybridConditionalMixture;
HybridFactor::shared_ptr factorOnModes; boost::shared_ptr<Factor> factorOnModes;
std::tie(hybridConditionalMixture, factorOnModes) = std::tie(hybridConditionalMixture, factorOnModes) =
EliminateHybrid(factors, ordering); EliminateHybrid(factors, ordering);
@ -364,17 +363,11 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
// 1 parent, which is the mode // 1 parent, which is the mode
EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents()); EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents());
// This is now a HybridDiscreteFactor // This is now a discreteFactor
auto hybridDiscreteFactor = auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
dynamic_pointer_cast<HybridDiscreteFactor>(factorOnModes);
// Access the type-erased inner object and convert to DecisionTreeFactor
auto discreteFactor =
dynamic_pointer_cast<DecisionTreeFactor>(hybridDiscreteFactor->inner());
CHECK(discreteFactor); CHECK(discreteFactor);
EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
EXPECT(discreteFactor->root_->isLeaf() == false); EXPECT(discreteFactor->root_->isLeaf() == false);
// TODO(Varun) Test emplace_discrete
} }
/**************************************************************************** /****************************************************************************
@ -435,9 +428,10 @@ TEST(HybridFactorGraph, Full_Elimination) {
DiscreteFactorGraph discrete_fg; DiscreteFactorGraph discrete_fg;
// TODO(Varun) Make this a function of HybridGaussianFactorGraph? // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
for (HybridFactor::shared_ptr& factor : (*remainingFactorGraph_partial)) { for (auto& factor : (*remainingFactorGraph_partial)) {
auto df = dynamic_pointer_cast<HybridDiscreteFactor>(factor); auto df = dynamic_pointer_cast<DecisionTreeFactor>(factor);
discrete_fg.push_back(df->inner()); assert(df);
discrete_fg.push_back(df);
} }
ordering.clear(); ordering.clear();
@ -500,8 +494,7 @@ TEST(HybridFactorGraph, Printing) {
string expected_hybridFactorGraph = R"( string expected_hybridFactorGraph = R"(
size: 7 size: 7
factor 0: Continuous [x0] factor 0:
A[x0] = [ A[x0] = [
10 10
] ]
@ -553,26 +546,22 @@ factor 2: Hybrid [x1 x2; m1]{
No noise model No noise model
} }
factor 3: Continuous [x1] factor 3:
A[x1] = [ A[x1] = [
10 10
] ]
b = [ -10 ] b = [ -10 ]
No noise model No noise model
factor 4: Continuous [x2] factor 4:
A[x2] = [ A[x2] = [
10 10
] ]
b = [ -10 ] b = [ -10 ]
No noise model No noise model
factor 5: Discrete [m0] factor 5: P( m0 ):
P( m0 ):
Leaf 0.5 Leaf 0.5
factor 6: Discrete [m1 m0] factor 6: P( m1 | m0 ):
P( m1 | m0 ):
Choice(m1) Choice(m1)
0 Choice(m0) 0 Choice(m0)
0 0 Leaf 0.33333333 0 0 Leaf 0.33333333
@ -683,7 +672,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise); fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
using PlanarMotionModel = BetweenFactor<Pose2>; using PlanarMotionModel = BetweenFactor<Pose2>;
@ -696,7 +685,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry, moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model); noise_model);
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving}; std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
fg.emplace_hybrid<MixtureFactor>( fg.emplace_shared<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
@ -708,9 +697,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0; double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
// Add Bearing-Range factors // Add Bearing-Range factors
fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>( fg.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
X(0), L(0), bearing11, range11, measurementNoise); X(0), L(0), bearing11, range11, measurementNoise);
fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>( fg.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
X(1), L(1), bearing22, range22, measurementNoise); X(1), L(1), bearing22, range22, measurementNoise);
// Create (deliberately inaccurate) initial estimate // Create (deliberately inaccurate) initial estimate

32
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -409,7 +409,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise); fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
@ -418,11 +418,11 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// where X is the base link and W is the foot link. // where X is the base link and W is the foot link.
// Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
// Create initial estimate // Create initial estimate
@ -451,14 +451,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise); poseNoise);
initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
@ -491,14 +491,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
poseNoise); poseNoise);
initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
@ -534,14 +534,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=3 // PoseFactors-like at k=3
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
poseNoise); poseNoise);
initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(X(3), Pose2(3.0, 0.0, 0.0));

31
gtsam/hybrid/tests/testSerializationHybrid.cpp
View File

@ -23,8 +23,6 @@
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
@ -73,28 +71,6 @@ BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
/* ****************************************************************************/
// Test HybridGaussianFactor serialization.
TEST(HybridSerialization, HybridGaussianFactor) {
const HybridGaussianFactor factor(JacobianFactor(X(0), I_3x3, Z_3x1));
EXPECT(equalsObj<HybridGaussianFactor>(factor));
EXPECT(equalsXML<HybridGaussianFactor>(factor));
EXPECT(equalsBinary<HybridGaussianFactor>(factor));
}
/* ****************************************************************************/
// Test HybridDiscreteFactor serialization.
TEST(HybridSerialization, HybridDiscreteFactor) {
DiscreteKeys discreteKeys{{M(0), 2}};
const HybridDiscreteFactor factor(
DecisionTreeFactor(discreteKeys, std::vector<double>{0.4, 0.6}));
EXPECT(equalsObj<HybridDiscreteFactor>(factor));
EXPECT(equalsXML<HybridDiscreteFactor>(factor));
EXPECT(equalsBinary<HybridDiscreteFactor>(factor));
}
/* ****************************************************************************/ /* ****************************************************************************/
// Test GaussianMixtureFactor serialization. // Test GaussianMixtureFactor serialization.
TEST(HybridSerialization, GaussianMixtureFactor) { TEST(HybridSerialization, GaussianMixtureFactor) {
@ -150,8 +126,7 @@ TEST(HybridSerialization, GaussianMixture) {
// Test HybridBayesNet serialization. // Test HybridBayesNet serialization.
TEST(HybridSerialization, HybridBayesNet) { TEST(HybridSerialization, HybridBayesNet) {
Switching s(2); Switching s(2);
Ordering ordering = s.linearizedFactorGraph.getHybridOrdering(); HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential());
HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential(ordering));
EXPECT(equalsObj<HybridBayesNet>(hbn)); EXPECT(equalsObj<HybridBayesNet>(hbn));
EXPECT(equalsXML<HybridBayesNet>(hbn)); EXPECT(equalsXML<HybridBayesNet>(hbn));
@ -162,9 +137,7 @@ TEST(HybridSerialization, HybridBayesNet) {
// Test HybridBayesTree serialization. // Test HybridBayesTree serialization.
TEST(HybridSerialization, HybridBayesTree) { TEST(HybridSerialization, HybridBayesTree) {
Switching s(2); Switching s(2);
Ordering ordering = s.linearizedFactorGraph.getHybridOrdering(); HybridBayesTree hbt = *(s.linearizedFactorGraph.eliminateMultifrontal());
HybridBayesTree hbt =
*(s.linearizedFactorGraph.eliminateMultifrontal(ordering));
EXPECT(equalsObj<HybridBayesTree>(hbt)); EXPECT(equalsObj<HybridBayesTree>(hbt));
EXPECT(equalsXML<HybridBayesTree>(hbt)); EXPECT(equalsXML<HybridBayesTree>(hbt));

18
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -44,9 +44,16 @@ namespace gtsam {
if (orderingType == Ordering::METIS) { if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived()); Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex); return eliminateSequential(computedOrdering, function, variableIndex);
} else { } else if (orderingType == Ordering::COLAMD) {
Ordering computedOrdering = Ordering::Colamd(*variableIndex); Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateSequential(computedOrdering, function, variableIndex); return eliminateSequential(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::NATURAL) {
Ordering computedOrdering = Ordering::Natural(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex);
} else {
Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
asDerived(), variableIndex);
return eliminateSequential(computedOrdering, function, variableIndex);
} }
} }
} }
@ -100,9 +107,16 @@ namespace gtsam {
if (orderingType == Ordering::METIS) { if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived()); Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else { } else if (orderingType == Ordering::COLAMD) {
Ordering computedOrdering = Ordering::Colamd(*variableIndex); Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::NATURAL) {
Ordering computedOrdering = Ordering::Natural(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else {
Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
asDerived(), variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex);
} }
} }
} }

23
gtsam/inference/Factor.h
View File

@ -34,22 +34,17 @@ typedef FastVector<FactorIndex> FactorIndices;
typedef FastSet<FactorIndex> FactorIndexSet; typedef FastSet<FactorIndex> FactorIndexSet;
/** /**
* This is the base class for all factor types. It is templated on a KEY type, * This is the base class for all factor types. This class does not store any
* which will be the type used to label variables. Key types currently in use * data other than its keys. Derived classes store data such as matrices and
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and * probability tables.
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
* *
* Note that derived classes *must* redefine the ConditionalType and shared_ptr * Note that derived classes *must* redefine the `This` and `shared_ptr`
* typedefs to refer to the associated conditional and shared_ptr types of the * typedefs. See JacobianFactor, etc. for examples.
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
* *
* This class is \b not virtual for performance reasons - derived symbolic classes, * This class is \b not virtual for performance reasons - the derived class
* IndexFactor and IndexConditional, need to be created and destroyed quickly * SymbolicFactor needs to be created and destroyed quickly during symbolic
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. * elimination. GaussianFactor and NonlinearFactor are virtual.
*
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Factor class GTSAM_EXPORT Factor

6
gtsam/linear/GaussianFactorGraph.h
View File

@ -54,6 +54,12 @@ namespace gtsam {
static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminatePreferCholesky(factors, keys); } return EliminatePreferCholesky(factors, keys); }
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
return Ordering::Colamd(*variableIndex);
}
}; };
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/symbolic/SymbolicFactorGraph.h
View File

@ -46,6 +46,12 @@ namespace gtsam {
static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminateSymbolic(factors, keys); } return EliminateSymbolic(factors, keys); }
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
return Ordering::Colamd(*variableIndex);
}
}; };
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -25,7 +25,6 @@ from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
class TestHybridGaussianFactorGraph(GtsamTestCase): class TestHybridGaussianFactorGraph(GtsamTestCase):
"""Unit tests for HybridGaussianFactorGraph.""" """Unit tests for HybridGaussianFactorGraph."""
def test_create(self): def test_create(self):
"""Test construction of hybrid factor graph.""" """Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3) model = noiseModel.Unit.Create(3)
@ -42,9 +41,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
hfg.push_back(jf2) hfg.push_back(jf2)
hfg.push_back(gmf) hfg.push_back(gmf)
hbn = hfg.eliminateSequential( hbn = hfg.eliminateSequential()
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
hfg, [C(0)]))
self.assertEqual(hbn.size(), 2) self.assertEqual(hbn.size(), 2)
@ -74,15 +71,14 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
dtf = gtsam.DecisionTreeFactor([(C(0), 2)], "0 1") dtf = gtsam.DecisionTreeFactor([(C(0), 2)], "0 1")
hfg.push_back(dtf) hfg.push_back(dtf)
hbn = hfg.eliminateSequential( hbn = hfg.eliminateSequential()
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
hfg, [C(0)]))
hv = hbn.optimize() hv = hbn.optimize()
self.assertEqual(hv.atDiscrete(C(0)), 1) self.assertEqual(hv.atDiscrete(C(0)), 1)
@staticmethod @staticmethod
def tiny(num_measurements: int = 1, prior_mean: float = 5.0, def tiny(num_measurements: int = 1,
prior_mean: float = 5.0,
prior_sigma: float = 0.5) -> HybridBayesNet: prior_sigma: float = 0.5) -> HybridBayesNet:
""" """
Create a tiny two variable hybrid model which represents Create a tiny two variable hybrid model which represents
@ -129,20 +125,23 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
bayesNet2 = self.tiny(prior_sigma=5.0, num_measurements=1) bayesNet2 = self.tiny(prior_sigma=5.0, num_measurements=1)
# bn1: # 1/sqrt(2*pi*0.5^2) # bn1: # 1/sqrt(2*pi*0.5^2)
# bn2: # 1/sqrt(2*pi*5.0^2) # bn2: # 1/sqrt(2*pi*5.0^2)
expected_ratio = np.sqrt(2*np.pi*5.0**2)/np.sqrt(2*np.pi*0.5**2) expected_ratio = np.sqrt(2 * np.pi * 5.0**2) / np.sqrt(
2 * np.pi * 0.5**2)
mean0 = HybridValues() mean0 = HybridValues()
mean0.insert(X(0), [5.0]) mean0.insert(X(0), [5.0])
mean0.insert(Z(0), [5.0]) mean0.insert(Z(0), [5.0])
mean0.insert(M(0), 0) mean0.insert(M(0), 0)
self.assertAlmostEqual(bayesNet1.evaluate(mean0) / self.assertAlmostEqual(bayesNet1.evaluate(mean0) /
bayesNet2.evaluate(mean0), expected_ratio, bayesNet2.evaluate(mean0),
expected_ratio,
delta=1e-9) delta=1e-9)
mean1 = HybridValues() mean1 = HybridValues()
mean1.insert(X(0), [5.0]) mean1.insert(X(0), [5.0])
mean1.insert(Z(0), [5.0]) mean1.insert(Z(0), [5.0])
mean1.insert(M(0), 1) mean1.insert(M(0), 1)
self.assertAlmostEqual(bayesNet1.evaluate(mean1) / self.assertAlmostEqual(bayesNet1.evaluate(mean1) /
bayesNet2.evaluate(mean1), expected_ratio, bayesNet2.evaluate(mean1),
expected_ratio,
delta=1e-9) delta=1e-9)
@staticmethod @staticmethod
@ -171,11 +170,13 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
return fg return fg
@classmethod @classmethod
def estimate_marginals(cls, target, proposal_density: HybridBayesNet, def estimate_marginals(cls,
target,
proposal_density: HybridBayesNet,
N=10000): N=10000):
"""Do importance sampling to estimate discrete marginal P(mode).""" """Do importance sampling to estimate discrete marginal P(mode)."""
# Allocate space for marginals on mode. # Allocate space for marginals on mode.
marginals = np.zeros((2,)) marginals = np.zeros((2, ))
# Do importance sampling. # Do importance sampling.
for s in range(N): for s in range(N):
@ -210,14 +211,15 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
return bayesNet.evaluate(x) return bayesNet.evaluate(x)
# Create proposal density on (x0, mode), making sure it has same mean: # Create proposal density on (x0, mode), making sure it has same mean:
posterior_information = 1/(prior_sigma**2) + 1/(0.5**2) posterior_information = 1 / (prior_sigma**2) + 1 / (0.5**2)
posterior_sigma = posterior_information**(-0.5) posterior_sigma = posterior_information**(-0.5)
proposal_density = self.tiny( proposal_density = self.tiny(num_measurements=0,
num_measurements=0, prior_mean=5.0, prior_sigma=posterior_sigma) prior_mean=5.0,
prior_sigma=posterior_sigma)
# Estimate marginals using importance sampling. # Estimate marginals using importance sampling.
marginals = self.estimate_marginals( marginals = self.estimate_marginals(target=unnormalized_posterior,
target=unnormalized_posterior, proposal_density=proposal_density) proposal_density=proposal_density)
# print(f"True mode: {values.atDiscrete(M(0))}") # print(f"True mode: {values.atDiscrete(M(0))}")
# print(f"P(mode=0; Z) = {marginals[0]}") # print(f"P(mode=0; Z) = {marginals[0]}")
# print(f"P(mode=1; Z) = {marginals[1]}") # print(f"P(mode=1; Z) = {marginals[1]}")
@ -230,10 +232,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
self.assertEqual(fg.size(), 3) self.assertEqual(fg.size(), 3)
# Test elimination. # Test elimination.
ordering = gtsam.Ordering() posterior = fg.eliminateSequential()
ordering.push_back(X(0))
ordering.push_back(M(0))
posterior = fg.eliminateSequential(ordering)
def true_posterior(x): def true_posterior(x):
"""Posterior from elimination.""" """Posterior from elimination."""
@ -241,8 +240,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
return posterior.evaluate(x) return posterior.evaluate(x)
# Estimate marginals using importance sampling. # Estimate marginals using importance sampling.
marginals = self.estimate_marginals( marginals = self.estimate_marginals(target=true_posterior,
target=true_posterior, proposal_density=proposal_density) proposal_density=proposal_density)
# print(f"True mode: {values.atDiscrete(M(0))}") # print(f"True mode: {values.atDiscrete(M(0))}")
# print(f"P(mode=0; z0) = {marginals[0]}") # print(f"P(mode=0; z0) = {marginals[0]}")
# print(f"P(mode=1; z0) = {marginals[1]}") # print(f"P(mode=1; z0) = {marginals[1]}")
@ -253,8 +252,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
@staticmethod @staticmethod
def calculate_ratio(bayesNet: HybridBayesNet, def calculate_ratio(bayesNet: HybridBayesNet,
fg: HybridGaussianFactorGraph, fg: HybridGaussianFactorGraph, sample: HybridValues):
sample: HybridValues):
"""Calculate ratio between Bayes net and factor graph.""" """Calculate ratio between Bayes net and factor graph."""
return bayesNet.evaluate(sample) / fg.probPrime(sample) if \ return bayesNet.evaluate(sample) / fg.probPrime(sample) if \
fg.probPrime(sample) > 0 else 0 fg.probPrime(sample) > 0 else 0
@ -285,14 +283,15 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
return bayesNet.evaluate(x) return bayesNet.evaluate(x)
# Create proposal density on (x0, mode), making sure it has same mean: # Create proposal density on (x0, mode), making sure it has same mean:
posterior_information = 1/(prior_sigma**2) + 2.0/(3.0**2) posterior_information = 1 / (prior_sigma**2) + 2.0 / (3.0**2)
posterior_sigma = posterior_information**(-0.5) posterior_sigma = posterior_information**(-0.5)
proposal_density = self.tiny( proposal_density = self.tiny(num_measurements=0,
num_measurements=0, prior_mean=5.0, prior_sigma=posterior_sigma) prior_mean=5.0,
prior_sigma=posterior_sigma)
# Estimate marginals using importance sampling. # Estimate marginals using importance sampling.
marginals = self.estimate_marginals( marginals = self.estimate_marginals(target=unnormalized_posterior,
target=unnormalized_posterior, proposal_density=proposal_density) proposal_density=proposal_density)
# print(f"True mode: {values.atDiscrete(M(0))}") # print(f"True mode: {values.atDiscrete(M(0))}")
# print(f"P(mode=0; Z) = {marginals[0]}") # print(f"P(mode=0; Z) = {marginals[0]}")
# print(f"P(mode=1; Z) = {marginals[1]}") # print(f"P(mode=1; Z) = {marginals[1]}")
@ -319,10 +318,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
self.assertAlmostEqual(ratio, expected_ratio) self.assertAlmostEqual(ratio, expected_ratio)
# Test elimination. # Test elimination.
ordering = gtsam.Ordering() posterior = fg.eliminateSequential()
ordering.push_back(X(0))
ordering.push_back(M(0))
posterior = fg.eliminateSequential(ordering)
# Calculate ratio between Bayes net probability and the factor graph: # Calculate ratio between Bayes net probability and the factor graph:
expected_ratio = self.calculate_ratio(posterior, fg, values) expected_ratio = self.calculate_ratio(posterior, fg, values)

33
python/gtsam/tests/test_HybridNonlinearFactorGraph.py
View File

@ -14,39 +14,38 @@ from __future__ import print_function
import unittest import unittest
import gtsam
import numpy as np import numpy as np
from gtsam.symbol_shorthand import C, X from gtsam.symbol_shorthand import C, X
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam import BetweenFactorPoint3, noiseModel, PriorFactorPoint3, Point3
import gtsam
class TestHybridGaussianFactorGraph(GtsamTestCase): class TestHybridGaussianFactorGraph(GtsamTestCase):
"""Unit tests for HybridGaussianFactorGraph.""" """Unit tests for HybridGaussianFactorGraph."""
def test_nonlinear_hybrid(self): def test_nonlinear_hybrid(self):
nlfg = gtsam.HybridNonlinearFactorGraph() nlfg = gtsam.HybridNonlinearFactorGraph()
dk = gtsam.DiscreteKeys() dk = gtsam.DiscreteKeys()
dk.push_back((10, 2)) dk.push_back((10, 2))
nlfg.add(gtsam.BetweenFactorPoint3(1, 2, gtsam.Point3(1, 2, 3), gtsam.noiseModel.Diagonal.Variances([1, 1, 1]))) nlfg.push_back(BetweenFactorPoint3(1, 2, Point3(
nlfg.add( 1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1])))
gtsam.PriorFactorPoint3(2, gtsam.Point3(1, 2, 3), gtsam.noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) nlfg.push_back(
PriorFactorPoint3(2, Point3(1, 2, 3),
noiseModel.Diagonal.Variances([0.5, 0.5, 0.5])))
nlfg.push_back( nlfg.push_back(
gtsam.MixtureFactor([1], dk, [ gtsam.MixtureFactor([1], dk, [
gtsam.PriorFactorPoint3(1, gtsam.Point3(0, 0, 0), PriorFactorPoint3(1, Point3(0, 0, 0),
gtsam.noiseModel.Unit.Create(3)), noiseModel.Unit.Create(3)),
gtsam.PriorFactorPoint3(1, gtsam.Point3(1, 2, 1), PriorFactorPoint3(1, Point3(1, 2, 1),
gtsam.noiseModel.Unit.Create(3)) noiseModel.Unit.Create(3))
])) ]))
nlfg.add(gtsam.DecisionTreeFactor((10, 2), "1 3")) nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3"))
values = gtsam.Values() values = gtsam.Values()
values.insert_point3(1, gtsam.Point3(0, 0, 0)) values.insert_point3(1, Point3(0, 0, 0))
values.insert_point3(2, gtsam.Point3(2, 3, 1)) values.insert_point3(2, Point3(2, 3, 1))
hfg = nlfg.linearize(values) hfg = nlfg.linearize(values)
o = gtsam.Ordering() hbn = hfg.eliminateSequential()
o.push_back(1)
o.push_back(2)
o.push_back(10)
hbn = hfg.eliminateSequential(o)
hbv = hbn.optimize() hbv = hbn.optimize()
self.assertEqual(hbv.atDiscrete(10), 0) self.assertEqual(hbv.atDiscrete(10), 0)