Now we have real multifrontal!
Fan Jiang
parent
36ee4ce7cb
commit
7ad2031b2f
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file CLGaussianConditional.cpp
|
||||
* @file GaussianMixture.cpp
|
||||
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
|
||||
* @author Fan Jiang
|
||||
* @date Mar 12, 2022
|
||||
|
|
@ -18,25 +18,25 @@
|
|||
|
||||
#include <gtsam/base/utilities.h>
|
||||
#include <gtsam/discrete/DecisionTree-inl.h>
|
||||
#include <gtsam/hybrid/CLGaussianConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/inference/Conditional-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
CLGaussianConditional::CLGaussianConditional(
|
||||
GaussianMixture::GaussianMixture(
|
||||
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const CLGaussianConditional::Conditionals &conditionals)
|
||||
const GaussianMixture::Conditionals &conditionals)
|
||||
: BaseFactor(CollectKeys(continuousFrontals, continuousParents),
|
||||
discreteParents),
|
||||
BaseConditional(continuousFrontals.size()),
|
||||
conditionals_(conditionals) {}
|
||||
|
||||
bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const {
|
||||
bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
void CLGaussianConditional::print(const std::string &s,
|
||||
void GaussianMixture::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << s << ": ";
|
||||
if (isContinuous_) std::cout << "Cont. ";
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file CLGaussianConditional.h
|
||||
* @file GaussianMixture.h
|
||||
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
|
||||
* @author Fan Jiang
|
||||
* @date Mar 12, 2022
|
||||
|
|
@ -24,21 +24,21 @@
|
|||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
||||
namespace gtsam {
|
||||
class CLGaussianConditional
|
||||
class GaussianMixture
|
||||
: public HybridFactor,
|
||||
public Conditional<HybridFactor, CLGaussianConditional> {
|
||||
public Conditional<HybridFactor, GaussianMixture> {
|
||||
public:
|
||||
using This = CLGaussianConditional;
|
||||
using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
|
||||
using This = GaussianMixture;
|
||||
using shared_ptr = boost::shared_ptr<GaussianMixture>;
|
||||
using BaseFactor = HybridFactor;
|
||||
using BaseConditional = Conditional<HybridFactor, CLGaussianConditional>;
|
||||
using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
|
||||
|
||||
using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
|
||||
|
||||
Conditionals conditionals_;
|
||||
|
||||
public:
|
||||
CLGaussianConditional(const KeyVector &continuousFrontals,
|
||||
GaussianMixture(const KeyVector &continuousFrontals,
|
||||
const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const Conditionals &conditionals);
|
||||
|
|
@ -46,7 +46,7 @@ class CLGaussianConditional
|
|||
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
|
||||
|
||||
void print(
|
||||
const std::string &s = "CLGaussianConditional\n",
|
||||
const std::string &s = "GaussianMixture\n",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
};
|
||||
} // namespace gtsam
|
||||
|
|
@ -19,9 +19,31 @@
|
|||
#include <gtsam/inference/Conditional-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
|
||||
const DiscreteKeys &discreteFrontals,
|
||||
const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents)
|
||||
: HybridConditional(
|
||||
CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
|
||||
{continuousParents.begin(), continuousParents.end()}),
|
||||
CollectDiscreteKeys(
|
||||
{discreteFrontals.begin(), discreteFrontals.end()},
|
||||
{discreteParents.begin(), discreteParents.end()}),
|
||||
continuousFrontals.size() + discreteFrontals.size()) {}
|
||||
|
||||
HybridConditional::HybridConditional(
|
||||
boost::shared_ptr<GaussianConditional> continuousConditional)
|
||||
: BaseFactor(continuousConditional->keys()),
|
||||
BaseConditional(continuousConditional->nrFrontals()) {}
|
||||
|
||||
void HybridConditional::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << s << "P(";
|
||||
std::cout << s;
|
||||
if (isContinuous_) std::cout << "Cont. ";
|
||||
if (isDiscrete_) std::cout << "Disc. ";
|
||||
if (isHybrid_) std::cout << "Hybr. ";
|
||||
std::cout << "P(";
|
||||
int index = 0;
|
||||
const size_t N = keys().size();
|
||||
const size_t contN = N - discreteKeys_.size();
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@
|
|||
#include <boost/shared_ptr.hpp>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "gtsam/inference/Key.h"
|
||||
#include "gtsam/linear/GaussianConditional.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -33,8 +35,8 @@ namespace gtsam {
|
|||
*
|
||||
* As a type-erased variant of:
|
||||
* - DiscreteConditional
|
||||
* - CLGaussianConditional
|
||||
* - GaussianConditional
|
||||
* - GaussianMixture
|
||||
*/
|
||||
class GTSAM_EXPORT HybridConditional
|
||||
: public HybridFactor,
|
||||
|
|
@ -62,6 +64,13 @@ class GTSAM_EXPORT HybridConditional
|
|||
const DiscreteKeys& discreteKeys, size_t nFrontals)
|
||||
: BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
|
||||
|
||||
HybridConditional(const KeyVector& continuousFrontals,
|
||||
const DiscreteKeys& discreteFrontals,
|
||||
const KeyVector& continuousParents,
|
||||
const DiscreteKeys& discreteParents);
|
||||
|
||||
HybridConditional(boost::shared_ptr<GaussianConditional> continuousConditional);
|
||||
|
||||
/**
|
||||
* @brief Combine two conditionals, yielding a new conditional with the union
|
||||
* of the frontal keys, ordered by gtsam::Key.
|
||||
|
|
|
|||
|
|
@ -53,7 +53,9 @@ HybridFactor::HybridFactor(const KeyVector &keys)
|
|||
HybridFactor::HybridFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys)
|
||||
: Base(CollectKeys(continuousKeys, discreteKeys)),
|
||||
isHybrid_(true),
|
||||
isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
|
||||
isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
|
||||
isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
|
||||
discreteKeys_(discreteKeys) {}
|
||||
|
||||
HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
|
||||
|
|
@ -61,6 +63,16 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
|
|||
isDiscrete_(true),
|
||||
discreteKeys_(discreteKeys) {}
|
||||
|
||||
void HybridFactor::print(
|
||||
const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << s;
|
||||
if (isContinuous_) std::cout << "Cont. ";
|
||||
if (isDiscrete_) std::cout << "Disc. ";
|
||||
if (isHybrid_) std::cout << "Hybr. ";
|
||||
this->printKeys("", formatter);
|
||||
}
|
||||
|
||||
HybridFactor::~HybridFactor() = default;
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -82,13 +82,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
|
|||
/// print
|
||||
void print(
|
||||
const std::string &s = "HybridFactor\n",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s;
|
||||
if (isContinuous_) std::cout << "Cont. ";
|
||||
if (isDiscrete_) std::cout << "Disc. ";
|
||||
if (isHybrid_) std::cout << "Hybr. ";
|
||||
this->printKeys("", formatter);
|
||||
}
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
|
|||
|
|
@ -26,8 +26,11 @@
|
|||
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
#include "gtsam/inference/Key.h"
|
||||
#include "gtsam/linear/GaussianFactorGraph.h"
|
||||
#include "gtsam/linear/HessianFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -60,6 +63,28 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
|
|||
// In the case of multifrontal, we will need to use a constrained ordering
|
||||
// so that the discrete parts will be guaranteed to be eliminated last!
|
||||
|
||||
// Because of all these reasons, we need to think very carefully about how to
|
||||
// implement the hybrid factors so that we do not get poor performance.
|
||||
//
|
||||
// The first thing is how to represent the GaussianMixture. A very possible
|
||||
// scenario is that the incoming factors will have different levels of discrete
|
||||
// keys. For example, imagine we are going to eliminate the fragment:
|
||||
// $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will need
|
||||
// to know how to retrieve the corresponding continuous densities for the assi-
|
||||
// -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). And we
|
||||
// also need to consider when there is pruning. Two mixture factors could have
|
||||
// different pruning patterns-one could have (c1=0,c2=1) pruned, and another
|
||||
// could have (c2=0,c3=1) pruned, and this creates a big problem in how to
|
||||
// identify the intersection of non-pruned branches.
|
||||
|
||||
// One possible approach is first building the collection of all discrete keys.
|
||||
// After that we enumerate the space of all key combinations *lazily* so that
|
||||
// the exploration branch terminates whenever an assignment yields NULL in any
|
||||
// of the hybrid factors.
|
||||
|
||||
// When the number of assignments is large we may encounter stack overflows.
|
||||
// However this is also the case with iSAM2, so no pressure :)
|
||||
|
||||
// PREPROCESS: Identify the nature of the current elimination
|
||||
std::unordered_map<Key, DiscreteKey> discreteCardinalities;
|
||||
std::set<DiscreteKey> discreteSeparator;
|
||||
|
|
@ -110,31 +135,62 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
|
|||
}
|
||||
}
|
||||
|
||||
std::cout << RED_BOLD << "Keys: " << RESET;
|
||||
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;
|
||||
} else {
|
||||
std::cout << " " << DefaultKeyFormatter(f) << ",";
|
||||
{
|
||||
std::cout << RED_BOLD << "Keys: " << RESET;
|
||||
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;
|
||||
} else {
|
||||
std::cout << " " << DefaultKeyFormatter(f) << ",";
|
||||
}
|
||||
}
|
||||
|
||||
if (separatorKeys.size() > 0) {
|
||||
std::cout << " | ";
|
||||
}
|
||||
|
||||
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;
|
||||
} else {
|
||||
std::cout << DefaultKeyFormatter(f) << ",";
|
||||
}
|
||||
}
|
||||
std::cout << "\n" << RESET;
|
||||
}
|
||||
|
||||
if (separatorKeys.size() > 0) {
|
||||
std::cout << " | ";
|
||||
// NOTE: We should really defer the product here because of pruning
|
||||
|
||||
// Case 1: we are only dealing with continuous
|
||||
if (discreteCardinalities.empty()) {
|
||||
GaussianFactorGraph gfg;
|
||||
for (auto &fp : factors) {
|
||||
gfg.push_back(boost::static_pointer_cast<HybridGaussianFactor>(fp)->inner);
|
||||
}
|
||||
|
||||
auto result = EliminatePreferCholesky(gfg, frontalKeys);
|
||||
return std::make_pair(
|
||||
boost::make_shared<HybridConditional>(result.first),
|
||||
boost::make_shared<HybridGaussianFactor>(result.second));
|
||||
}
|
||||
|
||||
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;
|
||||
} else {
|
||||
std::cout << DefaultKeyFormatter(f) << ",";
|
||||
// Case 2: we are only dealing with discrete
|
||||
if (discreteCardinalities.empty()) {
|
||||
GaussianFactorGraph gfg;
|
||||
for (auto &fp : factors) {
|
||||
gfg.push_back(boost::static_pointer_cast<HybridGaussianFactor>(fp)->inner);
|
||||
}
|
||||
|
||||
auto result = EliminatePreferCholesky(gfg, frontalKeys);
|
||||
return std::make_pair(
|
||||
boost::make_shared<HybridConditional>(result.first),
|
||||
boost::make_shared<HybridGaussianFactor>(result.second));
|
||||
}
|
||||
std::cout << "\n" << RESET;
|
||||
|
||||
// PRODUCT: multiply all factors
|
||||
gttic(product);
|
||||
|
||||
|
|
|
|||
|
|
@ -19,15 +19,163 @@
|
|||
#include <gtsam/hybrid/HybridJunctionTree.h>
|
||||
#include <gtsam/inference/JunctionTree-inst.h>
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include "gtsam/hybrid/HybridFactorGraph.h"
|
||||
#include "gtsam/inference/Key.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Instantiate base classes
|
||||
template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>;
|
||||
template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
|
||||
|
||||
struct HybridConstructorTraversalData {
|
||||
typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::Node Node;
|
||||
typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::sharedNode
|
||||
sharedNode;
|
||||
|
||||
HybridConstructorTraversalData* const parentData;
|
||||
sharedNode myJTNode;
|
||||
FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
|
||||
FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
|
||||
KeySet discreteKeys;
|
||||
|
||||
// Small inner class to store symbolic factors
|
||||
class SymbolicFactors : public FactorGraph<Factor> {};
|
||||
|
||||
HybridConstructorTraversalData(HybridConstructorTraversalData* _parentData)
|
||||
: parentData(_parentData) {}
|
||||
|
||||
// Pre-order visitor function
|
||||
static HybridConstructorTraversalData ConstructorTraversalVisitorPre(
|
||||
const boost::shared_ptr<HybridEliminationTree::Node>& node,
|
||||
HybridConstructorTraversalData& parentData) {
|
||||
// On the pre-order pass, before children have been visited, we just set up
|
||||
// a traversal data structure with its own JT node, and create a child
|
||||
// pointer in its parent.
|
||||
HybridConstructorTraversalData myData =
|
||||
HybridConstructorTraversalData(&parentData);
|
||||
myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
|
||||
parentData.myJTNode->addChild(myData.myJTNode);
|
||||
|
||||
std::cout << "Getting discrete info: ";
|
||||
for (HybridFactor::shared_ptr& f : node->factors) {
|
||||
for (auto& k : f->discreteKeys_) {
|
||||
std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n";
|
||||
myData.discreteKeys.insert(k.first);
|
||||
}
|
||||
}
|
||||
|
||||
return myData;
|
||||
}
|
||||
|
||||
// Post-order visitor function
|
||||
static void ConstructorTraversalVisitorPostAlg2(
|
||||
const boost::shared_ptr<HybridEliminationTree::Node>& ETreeNode,
|
||||
const HybridConstructorTraversalData& myData) {
|
||||
// In this post-order visitor, we combine the symbolic elimination results
|
||||
// from the elimination tree children and symbolically eliminate the current
|
||||
// elimination tree node. We then check whether each of our elimination
|
||||
// tree child nodes should be merged with us. The check for this is that
|
||||
// our number of symbolic elimination parents is exactly 1 less than
|
||||
// our child's symbolic elimination parents - this condition indicates that
|
||||
// eliminating the current node did not introduce any parents beyond those
|
||||
// already in the child->
|
||||
|
||||
// Do symbolic elimination for this node
|
||||
SymbolicFactors symbolicFactors;
|
||||
symbolicFactors.reserve(ETreeNode->factors.size() +
|
||||
myData.childSymbolicFactors.size());
|
||||
// Add ETree node factors
|
||||
symbolicFactors += ETreeNode->factors;
|
||||
// Add symbolic factors passed up from children
|
||||
symbolicFactors += myData.childSymbolicFactors;
|
||||
|
||||
Ordering keyAsOrdering;
|
||||
keyAsOrdering.push_back(ETreeNode->key);
|
||||
SymbolicConditional::shared_ptr myConditional;
|
||||
SymbolicFactor::shared_ptr mySeparatorFactor;
|
||||
boost::tie(myConditional, mySeparatorFactor) =
|
||||
internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
|
||||
|
||||
std::cout << "Symbolic: ";
|
||||
myConditional->print();
|
||||
|
||||
// Store symbolic elimination results in the parent
|
||||
myData.parentData->childSymbolicConditionals.push_back(myConditional);
|
||||
myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
|
||||
myData.parentData->discreteKeys.merge(myData.discreteKeys);
|
||||
|
||||
sharedNode node = myData.myJTNode;
|
||||
const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
|
||||
myData.childSymbolicConditionals;
|
||||
node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size());
|
||||
|
||||
// Merge our children if they are in our clique - if our conditional has
|
||||
// exactly one fewer parent than our child's conditional.
|
||||
const size_t myNrParents = myConditional->nrParents();
|
||||
const size_t nrChildren = node->nrChildren();
|
||||
assert(childConditionals.size() == nrChildren);
|
||||
|
||||
// decide which children to merge, as index into children
|
||||
std::vector<size_t> nrFrontals = node->nrFrontalsOfChildren();
|
||||
std::vector<bool> merge(nrChildren, false);
|
||||
size_t myNrFrontals = 1;
|
||||
for (size_t i = 0; i < nrChildren; i++) {
|
||||
// Check if we should merge the i^th child
|
||||
if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
|
||||
const bool myType =
|
||||
myData.discreteKeys.exists(myConditional->frontals()[0]);
|
||||
const bool theirType =
|
||||
myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
|
||||
std::cout << "Type: "
|
||||
<< DefaultKeyFormatter(myConditional->frontals()[0]) << " vs "
|
||||
<< DefaultKeyFormatter(childConditionals[i]->frontals()[0])
|
||||
<< "\n";
|
||||
if (myType == theirType) {
|
||||
// Increment number of frontal variables
|
||||
myNrFrontals += nrFrontals[i];
|
||||
std::cout << "Merging ";
|
||||
childConditionals[i]->print();
|
||||
merge[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// now really merge
|
||||
node->mergeChildren(merge);
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridJunctionTree::HybridJunctionTree(
|
||||
const HybridEliminationTree& eliminationTree)
|
||||
: Base(eliminationTree) {}
|
||||
const HybridEliminationTree& eliminationTree) {
|
||||
gttic(JunctionTree_FromEliminationTree);
|
||||
// Here we rely on the BayesNet having been produced by this elimination tree,
|
||||
// such that the conditionals are arranged in DFS post-order. We traverse the
|
||||
// elimination tree, and inspect the symbolic conditional corresponding to
|
||||
// each node. The elimination tree node is added to the same clique with its
|
||||
// parent if it has exactly one more Bayes net conditional parent than
|
||||
// does its elimination tree parent.
|
||||
|
||||
// Traverse the elimination tree, doing symbolic elimination and merging nodes
|
||||
// as we go. Gather the created junction tree roots in a dummy Node.
|
||||
typedef typename HybridEliminationTree::Node ETreeNode;
|
||||
typedef HybridConstructorTraversalData Data;
|
||||
Data rootData(0);
|
||||
rootData.myJTNode =
|
||||
boost::make_shared<typename Base::Node>(); // Make a dummy node to gather
|
||||
// the junction tree roots
|
||||
treeTraversal::DepthFirstForest(eliminationTree, rootData,
|
||||
Data::ConstructorTraversalVisitorPre,
|
||||
Data::ConstructorTraversalVisitorPostAlg2);
|
||||
|
||||
// Assign roots from the dummy node
|
||||
this->addChildrenAsRoots(rootData.myJTNode);
|
||||
|
||||
// Transfer remaining factors from elimination tree
|
||||
Base::remainingFactors_ = eliminationTree.remainingFactors();
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
#include <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/hybrid/CGMixtureFactor.h>
|
||||
#include <gtsam/hybrid/CLGaussianConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
|
|
@ -31,6 +31,7 @@
|
|||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
#include <boost/assign/std/map.hpp>
|
||||
#include "gtsam/base/Testable.h"
|
||||
|
||||
using namespace boost::assign;
|
||||
|
||||
|
|
@ -48,9 +49,9 @@ TEST(HybridFactorGraph, creation) {
|
|||
|
||||
hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
|
||||
|
||||
CLGaussianConditional clgc(
|
||||
GaussianMixture clgc(
|
||||
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
|
||||
CLGaussianConditional::Conditionals(
|
||||
GaussianMixture::Conditionals(
|
||||
C(0),
|
||||
boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
|
||||
I_3x3),
|
||||
|
|
@ -69,7 +70,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) {
|
|||
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
||||
}
|
||||
|
||||
TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
|
||||
TEST(HybridFactorGraph, eliminateMultifrontal) {
|
||||
HybridFactorGraph hfg;
|
||||
|
||||
DiscreteKey x(X(1), 2);
|
||||
|
|
@ -132,13 +133,77 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
|
|||
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
|
||||
// 2 3 4")));
|
||||
|
||||
auto result =
|
||||
hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
|
||||
auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});
|
||||
|
||||
GTSAM_PRINT(*result);
|
||||
HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
|
||||
|
||||
// We immediately need to escape the CLG domain if we do this!!!
|
||||
GTSAM_PRINT(*result->marginalFactor(X(1)));
|
||||
GTSAM_PRINT(*hbt);
|
||||
/*
|
||||
Explanation: the Junction tree will need to reeliminate to get to the marginal
|
||||
on X(1), which is not possible because it involves eliminating discrete before
|
||||
continuous. The solution to this, however, is in Murphy02. TLDR is that this
|
||||
is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
|
||||
should do this.
|
||||
*/
|
||||
}
|
||||
|
||||
/**
|
||||
* This test is about how to assemble the Bayes Tree roots after we do partial elimination
|
||||
*/
|
||||
TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||
std::cout << ">>>>>>>>>>>>>>\n";
|
||||
|
||||
HybridFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
|
||||
|
||||
{
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
|
||||
C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
|
||||
boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(CGMixtureFactor({X(0)}, {{C(0), 2}}, dt));
|
||||
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
|
||||
C(1), boost::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
|
||||
boost::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(CGMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
|
||||
}
|
||||
|
||||
// hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
|
||||
hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
|
||||
|
||||
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));
|
||||
|
||||
{
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
|
||||
C(3), boost::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1),
|
||||
boost::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(CGMixtureFactor({X(3)}, {{C(3), 2}}, dt));
|
||||
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
|
||||
C(2), boost::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1),
|
||||
boost::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1));
|
||||
}
|
||||
|
||||
auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
|
||||
|
||||
GTSAM_PRINT(ordering_full);
|
||||
|
||||
HybridBayesTree::shared_ptr hbt;
|
||||
HybridFactorGraph::shared_ptr remaining;
|
||||
std::tie(hbt, remaining) =
|
||||
hfg.eliminatePartialMultifrontal(Ordering(ordering_full.begin(), ordering_full.end()));
|
||||
|
||||
GTSAM_PRINT(*hbt);
|
||||
|
||||
GTSAM_PRINT(*remaining);
|
||||
/*
|
||||
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
|
||||
|
|
|
|||
|
|
@ -70,7 +70,7 @@ namespace gtsam {
|
|||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
protected:
|
||||
|
||||
// Private default constructor (used in static construction methods)
|
||||
JunctionTree() {}
|
||||
|
|
|
|||
Loading…
Reference in New Issue