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

Address comments

release/4.3a0
Fan Jiang 2022-03-15 12:50:31 -04:00
parent fe5dde7e27
commit f237438bf3
18 changed files with 346 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/hybrid/CGMixtureFactor.cpp
View File

@ -1,6 +1,22 @@
//
// Created by Fan Jiang on 3/11/22.
//
/* ----------------------------------------------------------------------------
* 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 CGMixtureFactor.cpp
* @brief A set of Gaussian factors indexed by a set of discrete keys.
* @author Fan Jiang
* @author Varun Agrawal
* @author Frank Dellaert
* @date Mar 12, 2022
*/
#include <gtsam/hybrid/CGMixtureFactor.h>

16
gtsam/hybrid/CGMixtureFactor.h
View File

@ -18,15 +18,15 @@
* @date Mar 12, 2022
*/
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/linear/GaussianFactor.h>
namespace gtsam {
class CGMixtureFactor : public HybridFactor {
public:
public:
using Base = HybridFactor;
using This = CGMixtureFactor;
using shared_ptr = boost::shared_ptr<This>;
@ -38,13 +38,13 @@ public:
CGMixtureFactor() = default;
CGMixtureFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const Factors &factors);
const DiscreteKeys &discreteKeys, const Factors &factors);
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
void print(const std::string &s = "HybridFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
void print(
const std::string &s = "HybridFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
};
}
} // namespace gtsam

40
gtsam/hybrid/CLGaussianConditional.cpp
View File

@ -16,30 +16,28 @@
* @date Mar 12, 2022
*/
#include <gtsam/hybrid/CLGaussianConditional.h>
#include <gtsam/base/utilities.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/hybrid/CLGaussianConditional.h>
#include <gtsam/inference/Conditional-inst.h>
namespace gtsam {
CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals,
const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const CLGaussianConditional::Conditionals &conditionals)
: BaseFactor(
CollectKeys(continuousFrontals, continuousParents), discreteParents),
BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {
}
CLGaussianConditional::CLGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const CLGaussianConditional::Conditionals &conditionals)
: BaseFactor(CollectKeys(continuousFrontals, continuousParents),
discreteParents),
BaseConditional(continuousFrontals.size()),
conditionals_(conditionals) {}
bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const {
return false;
}
void CLGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const {
void CLGaussianConditional::print(const std::string &s,
const KeyFormatter &formatter) const {
std::cout << s << ": ";
if (isContinuous_) std::cout << "Cont. ";
if (isDiscrete_) std::cout << "Disc. ";
@ -51,14 +49,14 @@ void CLGaussianConditional::print(const std::string &s, const KeyFormatter &form
}
std::cout << "\n";
conditionals_.print(
"",
[&](Key k) {
return formatter(k);
}, [&](const GaussianConditional::shared_ptr &gf) -> std::string {
"", [&](Key k) { return formatter(k); },
[&](const GaussianConditional::shared_ptr &gf) -> std::string {
RedirectCout rd;
if (!gf->empty()) gf->print("", formatter);
else return {"nullptr"};
if (!gf->empty())
gf->print("", formatter);
else
return {"nullptr"};
return rd.str();
});
}
}
} // namespace gtsam

18
gtsam/hybrid/CLGaussianConditional.h
View File

@ -16,15 +16,16 @@
* @date Mar 12, 2022
*/
#include <gtsam/inference/Conditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/linear/GaussianConditional.h>
namespace gtsam {
class CLGaussianConditional : public HybridFactor, public Conditional<HybridFactor, CLGaussianConditional> {
public:
class CLGaussianConditional
: public HybridFactor,
public Conditional<HybridFactor, CLGaussianConditional> {
public:
using This = CLGaussianConditional;
using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
using BaseFactor = HybridFactor;
@ -34,8 +35,7 @@ public:
Conditionals conditionals_;
public:
public:
CLGaussianConditional(const KeyVector &continuousFrontals,
const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
@ -47,4 +47,4 @@ public:
const std::string &s = "CLGaussianConditional\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
};
}
} // namespace gtsam

15
gtsam/hybrid/HybridBayesTree.h
View File

@ -11,8 +11,7 @@
/**
* @file HybridBayesTree.h
* @brief Hybrid Bayes Tree, the result of eliminating a
* HybridJunctionTree
* @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
* @date Mar 11, 2022
* @author Fan Jiang
*/
@ -22,8 +21,8 @@
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Conditional.h>
#include <string>
@ -39,22 +38,18 @@ class GTSAM_EXPORT HybridBayesTreeClique
: public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
public:
typedef HybridBayesTreeClique This;
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>
Base;
typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
HybridBayesTreeClique() {}
virtual ~HybridBayesTreeClique() {}
HybridBayesTreeClique(
const boost::shared_ptr<HybridConditional>& conditional)
HybridBayesTreeClique(const boost::shared_ptr<HybridConditional>& conditional)
: Base(conditional) {}
};
/* ************************************************************************* */
/** A Bayes tree representing a Hybrid density */
class GTSAM_EXPORT HybridBayesTree
: public BayesTree<HybridBayesTreeClique> {
class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
private:
typedef BayesTree<HybridBayesTreeClique> Base;

33
gtsam/hybrid/HybridConditional.cpp
View File

@ -1,6 +1,19 @@
//
// Created by Fan Jiang on 3/11/22.
//
/* ----------------------------------------------------------------------------
* 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 HybridConditional.cpp
* @date Mar 11, 2022
* @author Fan Jiang
*/
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/inference/Conditional-inst.h>
@ -14,7 +27,10 @@ void HybridConditional::print(const std::string &s,
const size_t contN = N - discreteKeys_.size();
while (index < N) {
if (index > 0) {
if (index == nrFrontals_) std::cout << " | "; else std::cout << ", ";
if (index == nrFrontals_)
std::cout << " | ";
else
std::cout << ", ";
}
if (index < contN) {
std::cout << formatter(keys()[index]);
@ -28,13 +44,12 @@ void HybridConditional::print(const std::string &s,
if (inner) inner->print("", formatter);
}
bool HybridConditional::equals(const HybridFactor &other,
double tol) const {
bool HybridConditional::equals(const HybridFactor &other, double tol) const {
return false;
}
HybridConditional HybridConditional::operator*(const HybridConditional &other) const {
HybridConditional HybridConditional::operator*(
const HybridConditional &other) const {
return {};
}
}
} // namespace gtsam

36
gtsam/hybrid/HybridConditional.h
View File

@ -17,11 +17,10 @@
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/inference/Conditional.h>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <string>
@ -38,33 +37,30 @@ namespace gtsam {
* - GaussianConditional
*/
class GTSAM_EXPORT HybridConditional
: public HybridFactor,
public Conditional<HybridFactor, HybridConditional> {
public:
: public HybridFactor,
public Conditional<HybridFactor, HybridConditional> {
public:
// typedefs needed to play nice with gtsam
typedef HybridConditional This; ///< Typedef to this class
typedef HybridConditional This; ///< Typedef to this class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
typedef HybridFactor BaseFactor; ///< Typedef to our factor base class
typedef Conditional<BaseFactor, This>
BaseConditional; ///< Typedef to our conditional base class
private:
private:
// Type-erased pointer to the inner type
std::unique_ptr<Factor> inner;
public:
/// @name Standard Constructors
/// @{
public:
/// @name Standard Constructors
/// @{
/// Default constructor needed for serialization.
HybridConditional() = default;
HybridConditional(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
size_t nFrontals)
: BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {
}
HybridConditional(const KeyVector& continuousKeys,
const DiscreteKeys& discreteKeys, size_t nFrontals)
: BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
/**
* @brief Combine two conditionals, yielding a new conditional with the union
@ -83,9 +79,9 @@ public:
*/
HybridConditional operator*(const HybridConditional& other) const;
/// @}
/// @name Testable
/// @{
/// @}
/// @name Testable
/// @{
/// GTSAM-style print
void print(
@ -95,7 +91,7 @@ public:
/// GTSAM-style equals
bool equals(const HybridFactor& other, double tol = 1e-9) const override;
/// @}
/// @}
};
// DiscreteConditional

29
gtsam/hybrid/HybridDiscreteFactor.cpp
View File

@ -1,6 +1,20 @@
//
// Created by Fan Jiang on 3/11/22.
//
/* ----------------------------------------------------------------------------
* 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>
@ -15,17 +29,16 @@ HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
: Base(dtf.discreteKeys()),
inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
}
inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {}
bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
return false;
}
void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const {
void HybridDiscreteFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
inner->print("inner: ", formatter);
};
}
} // namespace gtsam

10
gtsam/hybrid/HybridDiscreteFactor.h
View File

@ -15,8 +15,8 @@
* @author Fan Jiang
*/
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
namespace gtsam {
@ -36,8 +36,10 @@ class HybridDiscreteFactor : public HybridFactor {
HybridDiscreteFactor(DecisionTreeFactor &&dtf);
public:
virtual bool equals(const HybridFactor& lf, double tol) const override;
virtual bool equals(const HybridFactor &lf, double tol) const override;
void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override;
void print(
const std::string &s = "HybridFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
};
}
} // namespace gtsam

45
gtsam/hybrid/HybridEliminationTree.h
View File

@ -23,40 +23,39 @@
namespace gtsam {
class GTSAM_EXPORT HybridEliminationTree :
public EliminationTree<HybridBayesNet, HybridFactorGraph>
{
class GTSAM_EXPORT HybridEliminationTree
: public EliminationTree<HybridBayesNet, HybridFactorGraph> {
public:
typedef EliminationTree<HybridBayesNet, HybridFactorGraph> Base; ///< Base class
typedef HybridEliminationTree This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef EliminationTree<HybridBayesNet, HybridFactorGraph>
Base; ///< Base class
typedef HybridEliminationTree This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
/**
* Build the elimination tree of a factor graph using pre-computed column structure.
* @param factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead.
* @return The elimination tree
*/
* Build the elimination tree of a factor graph using pre-computed column
* structure.
* @param factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is
* not precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead.
* @return The elimination tree
*/
HybridEliminationTree(const HybridFactorGraph& factorGraph,
const VariableIndex& structure, const Ordering& order);
const VariableIndex& structure, const Ordering& order);
/** Build the elimination tree of a factor graph. Note that this has to compute the column
* structure as a VariableIndex, so if you already have this precomputed, use the other
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
/** Build the elimination tree of a factor graph. Note that this has to
* compute the column structure as a VariableIndex, so if you already have
* this precomputed, use the other constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
HybridEliminationTree(const HybridFactorGraph& factorGraph,
const Ordering& order);
const Ordering& order);
/** Test whether the tree is equal to another */
bool equals(const This& other, double tol = 1e-9) const;
private:
friend class ::EliminationTreeTester;
};
}
} // namespace gtsam

32
gtsam/hybrid/HybridFactor.cpp
View File

@ -19,11 +19,12 @@
namespace gtsam {
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) {
KeyVector CollectKeys(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys) {
KeyVector allKeys;
std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys));
std::transform(discreteKeys.begin(),
discreteKeys.end(),
std::copy(continuousKeys.begin(), continuousKeys.end(),
std::back_inserter(allKeys));
std::transform(discreteKeys.begin(), discreteKeys.end(),
std::back_inserter(allKeys),
[](const DiscreteKey &k) { return k.first; });
return allKeys;
@ -36,7 +37,8 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
return allKeys;
}
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) {
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
const DiscreteKeys &key2) {
DiscreteKeys allKeys;
std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys));
std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys));
@ -45,16 +47,20 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &k
HybridFactor::HybridFactor() = default;
HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
HybridFactor::HybridFactor(const KeyVector &keys)
: Base(keys), isContinuous_(true) {}
HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
: Base(
CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {}
HybridFactor::HybridFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys)
: Base(CollectKeys(continuousKeys, discreteKeys)),
isHybrid_(true),
discreteKeys_(discreteKeys) {}
HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)),
isDiscrete_(true),
discreteKeys_(discreteKeys) {}
HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
: Base(CollectKeys({}, discreteKeys)),
isDiscrete_(true),
discreteKeys_(discreteKeys) {}
HybridFactor::~HybridFactor() = default;
}
} // namespace gtsam

61
gtsam/hybrid/HybridFactor.h
View File

@ -17,29 +17,30 @@
#pragma once
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/nonlinear/Values.h>
#include <string>
namespace gtsam {
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
KeyVector CollectKeys(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys);
KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2);
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
const DiscreteKeys &key2);
/**
* Base class for hybrid probabilistic factors
*/
class GTSAM_EXPORT HybridFactor : public Factor {
public:
public:
// typedefs needed to play nice with gtsam
typedef HybridFactor This; ///< This class
typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
typedef Factor Base; ///< Our base class
typedef HybridFactor This; ///< This class
typedef boost::shared_ptr<HybridFactor>
shared_ptr; ///< shared_ptr to this class
typedef Factor Base; ///< Our base class
bool isDiscrete_ = false;
bool isContinuous_ = false;
@ -47,31 +48,33 @@ public:
DiscreteKeys discreteKeys_;
public:
/// @name Standard Constructors
/// @{
public:
/// @name Standard Constructors
/// @{
/** Default constructor creates empty factor */
HybridFactor();
/** Construct from container of keys. This constructor is used internally from derived factor
* constructors, either from a container of keys or from a boost::assign::list_of. */
// template<typename CONTAINER>
// HybridFactor(const CONTAINER &keys) : Base(keys) {}
/** Construct from container of keys. This constructor is used internally
* from derived factor
* constructors, either from a container of keys or from a
* boost::assign::list_of. */
// template<typename CONTAINER>
// HybridFactor(const CONTAINER &keys) : Base(keys) {}
explicit HybridFactor(const KeyVector &keys);
HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
HybridFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys);
explicit HybridFactor(const DiscreteKeys &discreteKeys);
/// Virtual destructor
virtual ~HybridFactor();
/// @}
/// @name Testable
/// @{
/// @}
/// @name Testable
/// @{
/// equals
virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0;
@ -87,16 +90,16 @@ public:
this->printKeys("", formatter);
}
/// @}
/// @name Standard Interface
/// @{
/// @}
/// @name Standard Interface
/// @{
/// @}
/// @}
};
// HybridFactor
// traits
template<>
template <>
struct traits<HybridFactor> : public Testable<HybridFactor> {};
}// namespace gtsam
} // namespace gtsam

72
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -1,25 +1,37 @@
//
// Created by Fan Jiang on 3/11/22.
//
/* ----------------------------------------------------------------------------
#include "gtsam/inference/Key.h"
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridFactor.h>
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HybridFactorGraph.cpp
* @brief Hybrid factor graph that uses type erasure
* @author Fan Jiang
* @date Mar 11, 2022
*/
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <iostream>
#include <unordered_map>
#include "gtsam/inference/Key.h"
namespace gtsam {
template
class EliminateableFactorGraph<HybridFactorGraph>;
template class EliminateableFactorGraph<HybridFactorGraph>;
static std::string BLACK_BOLD = "\033[1;30m";
static std::string RED_BOLD = "\033[1;31m";
@ -29,8 +41,7 @@ static std::string RESET = "\033[0m";
/* ************************************************************************ */
std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> //
EliminateHybrid(const HybridFactorGraph &factors,
const Ordering &frontalKeys) {
EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
// NOTE(fan): Because we are in the Conditional Gaussian regime there are only
// a few cases:
// continuous variable, we make a GM if there are hybrid factors;
@ -38,9 +49,10 @@ EliminateHybrid(const HybridFactorGraph &factors,
// discrete variable, no continuous factor is allowed (escapes CG regime), so
// we panic, if discrete only we do the discrete elimination.
// However it is not that simple. During elimination it is possible that the multifrontal needs
// to eliminate an ordering that contains both Gaussian and hybrid variables, for example x1, c1.
// In this scenario, we will have a density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
// However it is not that simple. During elimination it is possible that the
// multifrontal needs to eliminate an ordering that contains both Gaussian and
// hybrid variables, for example x1, c1. In this scenario, we will have a
// density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
// The issue here is that, how can we know which variable is discrete if we
// unify Values? Obviously we can tell using the factors, but is that fast?
@ -102,7 +114,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
for (auto &f : frontalKeys) {
if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
auto &key = discreteCardinalities.at(f);
std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
std::cout << boost::format(" (%1%,%2%),") %
DefaultKeyFormatter(key.first) % key.second;
} else {
std::cout << " " << DefaultKeyFormatter(f) << ",";
}
@ -115,7 +128,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
for (auto &f : separatorKeys) {
if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
auto &key = discreteCardinalities.at(f);
std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
std::cout << boost::format(" (%1%,%2%),") %
DefaultKeyFormatter(key.first) % key.second;
} else {
std::cout << DefaultKeyFormatter(f) << ",";
}
@ -124,9 +138,10 @@ EliminateHybrid(const HybridFactorGraph &factors,
// PRODUCT: multiply all factors
gttic(product);
HybridConditional sum(KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()),
separatorKeys.size());
HybridConditional sum(
KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()),
separatorKeys.size());
// HybridDiscreteFactor product(DiscreteConditional());
// for (auto&& factor : factors) product = (*factor) * product;
@ -139,10 +154,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
// Ordering keys for the conditional so that frontalKeys are really in front
Ordering orderedKeys;
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
frontalKeys.end());
orderedKeys.insert(orderedKeys.end(), sum.keys().begin(),
sum.keys().end());
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
orderedKeys.insert(orderedKeys.end(), sum.keys().begin(), sum.keys().end());
// now divide product/sum to get conditional
gttic(divide);
@ -163,12 +176,11 @@ EliminateHybrid(const HybridFactorGraph &factors,
std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
// return std::make_pair(conditional, sum);
return std::make_pair(
conditional,
boost::make_shared<HybridConditional>(std::move(sum)));
return std::make_pair(conditional,
boost::make_shared<HybridConditional>(std::move(sum)));
}
void HybridFactorGraph::add(JacobianFactor &&factor) {
FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
}
}
} // namespace gtsam

54
gtsam/hybrid/HybridFactorGraph.h
View File

@ -36,43 +36,50 @@ class HybridJunctionTree;
class JacobianFactor;
/** Main elimination function for HybridFactorGraph */
GTSAM_EXPORT std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
GTSAM_EXPORT
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
/* ************************************************************************* */
template<> struct EliminationTraits<HybridFactorGraph>
{
typedef HybridFactor FactorType; ///< Type of factors in factor graph
typedef HybridFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph)
typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination
typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination
typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree
typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree
typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree
template <>
struct EliminationTraits<HybridFactorGraph> {
typedef HybridFactor FactorType; ///< Type of factors in factor graph
typedef HybridFactorGraph
FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph)
typedef HybridConditional
ConditionalType; ///< Type of conditionals from elimination
typedef HybridBayesNet
BayesNetType; ///< Type of Bayes net from sequential elimination
typedef HybridEliminationTree
EliminationTreeType; ///< Type of elimination tree
typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree
typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree
/// 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) {
return EliminateHybrid(factors, keys); }
return EliminateHybrid(factors, keys);
}
};
class HybridFactorGraph : public FactorGraph<HybridFactor>, public EliminateableFactorGraph<HybridFactorGraph> {
class HybridFactorGraph : public FactorGraph<HybridFactor>,
public EliminateableFactorGraph<HybridFactorGraph> {
public:
using Base = FactorGraph<HybridFactor>;
using This = HybridFactorGraph; ///< this class
using This = HybridFactorGraph; ///< this class
using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination
EliminateableFactorGraph<This>; ///< for elimination
using shared_ptr = boost::shared_ptr<This>; ///< shared_ptr to This
using Values = gtsam::Values; ///< backwards compatibility
using Indices = KeyVector; ///> map from keys to values
using Indices = KeyVector; ///> map from keys to values
public:
HybridFactorGraph() = default;
// /** Construct from container of factors (shared_ptr or plain objects) */
// template <class CONTAINER>
// explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
// /** Construct from container of factors (shared_ptr or plain objects) */
// template <class CONTAINER>
// explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
/** Implicit copy/downcast constructor to override explicit template container
* constructor. In BayesTree this is used for:
@ -84,8 +91,7 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>, public Eliminateable
using FactorGraph::add;
/// Add a factor directly using a shared_ptr.
void add(JacobianFactor &&factor);
void add(JacobianFactor&& factor);
};
}
} // namespace gtsam

35
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -1,6 +1,19 @@
//
// Created by Fan Jiang on 3/11/22.
//
/* ----------------------------------------------------------------------------
* 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>
@ -8,20 +21,22 @@
namespace gtsam {
HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) {
HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other)
: Base(other->keys()) {
inner = other;
}
HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner(boost::make_shared<JacobianFactor>(std::move(jf))) {
HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
: Base(jf.keys()),
inner(boost::make_shared<JacobianFactor>(std::move(jf))) {}
}
bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
return false;
}
void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const {
void HybridGaussianFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
inner->print("inner: ", formatter);
};
}
} // namespace gtsam

10
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -15,9 +15,9 @@
* @author Fan Jiang
*/
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
namespace gtsam {
@ -36,8 +36,10 @@ class HybridGaussianFactor : public HybridFactor {
explicit HybridGaussianFactor(JacobianFactor &&jf);
public:
virtual bool equals(const HybridFactor& lf, double tol) const override;
virtual bool equals(const HybridFactor &lf, double tol) const override;
void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override;
void print(
const std::string &s = "HybridFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
};
}
} // namespace gtsam

10
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -15,9 +15,9 @@
* @author Fan Jiang
*/
#include <gtsam/inference/JunctionTree-inst.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridJunctionTree.h>
#include <gtsam/inference/JunctionTree-inst.h>
namespace gtsam {
@ -27,7 +27,7 @@ template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
/* ************************************************************************* */
HybridJunctionTree::HybridJunctionTree(
const HybridEliminationTree& eliminationTree) :
Base(eliminationTree) {}
const HybridEliminationTree& eliminationTree)
: Base(eliminationTree) {}
}
} // namespace gtsam

87
gtsam/hybrid/tests/testHybridConditional.cpp
View File

@ -15,32 +15,30 @@
* @author Fan Jiang
*/
#include "Test.h"
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/CLGaussianConditional.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/hybrid/CGMixtureFactor.h>
#include <gtsam/hybrid/CLGaussianConditional.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using gtsam::symbol_shorthand::X;
using gtsam::symbol_shorthand::C;
using gtsam::symbol_shorthand::X;
/* ************************************************************************* */
TEST(HybridFactorGraph, creation) {
@ -51,17 +49,13 @@ TEST(HybridFactorGraph, creation) {
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
CLGaussianConditional clgc(
{X(0)}, {X(1)},
DiscreteKeys(DiscreteKey{C(0), 2}),
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
CLGaussianConditional::Conditionals(
C(0),
boost::make_shared<GaussianConditional>(
X(0), Z_3x1, I_3x3, X(1), I_3x3),
boost::make_shared<GaussianConditional>(
X(0), Vector3::Ones(),
I_3x3, X(1),
I_3x3))
);
boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
I_3x3),
boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
X(1), I_3x3)));
GTSAM_PRINT(clgc);
}
@ -90,7 +84,6 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
}
TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
std::cout << ">>>>>>>>>>>>>>\n";
HybridFactorGraph hfg;
@ -100,29 +93,27 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
boost::make_shared<JacobianFactor>(X(1),
I_3x3,
Z_3x1),
boost::make_shared<JacobianFactor>(X(1),
I_3x3,
Vector3::Ones()));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1 2 3 4")));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2}, {C(1), 2}}, "1 2 2 1")));
hfg.add(HybridDiscreteFactor(
DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
// 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
// {C(1), 2}}, "1 2 2 1")));
auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
auto result = hfg.eliminateMultifrontal(
Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
GTSAM_PRINT(*result);
GTSAM_PRINT(*result->marginalFactor(C(1)));
}
TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
std::cout << ">>>>>>>>>>>>>>\n";
HybridFactorGraph hfg;
@ -132,29 +123,28 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
boost::make_shared<JacobianFactor>(X(1),
I_3x3,
Z_3x1),
boost::make_shared<JacobianFactor>(X(1),
I_3x3,
Vector3::Ones()));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
hfg.add(CGMixtureFactor({X(1)}, {c}, dt));
hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
// 2 3 4")));
auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
auto result =
hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
GTSAM_PRINT(*result);
// We immediately need to escape the CLG domain if we do this!!!
GTSAM_PRINT(*result->marginalFactor(X(1)));
/*
Explanation: the Junction tree will need to reeliminate to get to the marginal on X(1), which
is not possible because it involves eliminating discrete before continuous. The solution to this,
however, is in Murphy02. TLDR is that this is 1. expensive and 2. inexact. neverless it is doable.
And I believe that we should do this.
Explanation: the Junction tree will need to reeliminate to get to the marginal
on X(1), which is not possible because it involves eliminating discrete before
continuous. The solution to this, however, is in Murphy02. TLDR is that this
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
should do this.
*/
}
@ -164,4 +154,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */