Squashed commit
Revised asProductFactor methods collectProductFactor() method Move test better print Formatting Efficiency Fix several bugs Fix print methods Fix print methods More tests, BT tests in different file More product tests Disable printing tests Minimize diff Fix rebase issuerelease/4.3a0
Frank Dellaert
parent
55ca557b1e
commit
92540298e1
|
|
@ -203,9 +203,6 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf,
|
|||
void HybridGaussianConditional::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << (s.empty() ? "" : s + "\n");
|
||||
if (isContinuous()) std::cout << "Continuous ";
|
||||
if (isDiscrete()) std::cout << "Discrete ";
|
||||
if (isHybrid()) std::cout << "Hybrid ";
|
||||
BaseConditional::print("", formatter);
|
||||
std::cout << " Discrete Keys = ";
|
||||
for (auto &dk : discreteKeys()) {
|
||||
|
|
|
|||
|
|
@ -32,8 +32,8 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianFactor::FactorValuePairs HybridGaussianFactor::augment(
|
||||
const FactorValuePairs &factors) {
|
||||
HybridGaussianFactor::FactorValuePairs
|
||||
HybridGaussianFactor::augment(const FactorValuePairs &factors) {
|
||||
// Find the minimum value so we can "proselytize" to positive values.
|
||||
// Done because we can't have sqrt of negative numbers.
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr> gaussianFactors;
|
||||
|
|
@ -48,12 +48,14 @@ HybridGaussianFactor::FactorValuePairs HybridGaussianFactor::augment(
|
|||
auto [gf, value] = gfv;
|
||||
|
||||
auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
if (!jf) return {gf, 0.0}; // should this be zero or infinite?
|
||||
if (!jf)
|
||||
return {gf, 0.0}; // should this be zero or infinite?
|
||||
|
||||
double normalized_value = value - min_value;
|
||||
|
||||
// If the value is 0, do nothing
|
||||
if (normalized_value == 0.0) return {gf, 0.0};
|
||||
if (normalized_value == 0.0)
|
||||
return {gf, 0.0};
|
||||
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.push_back(jf);
|
||||
|
|
@ -71,9 +73,9 @@ HybridGaussianFactor::FactorValuePairs HybridGaussianFactor::augment(
|
|||
|
||||
/* *******************************************************************************/
|
||||
struct HybridGaussianFactor::ConstructorHelper {
|
||||
KeyVector continuousKeys; // Continuous keys extracted from factors
|
||||
DiscreteKeys discreteKeys; // Discrete keys provided to the constructors
|
||||
FactorValuePairs pairs; // The decision tree with factors and scalars
|
||||
KeyVector continuousKeys; // Continuous keys extracted from factors
|
||||
DiscreteKeys discreteKeys; // Discrete keys provided to the constructors
|
||||
FactorValuePairs pairs; // The decision tree with factors and scalars
|
||||
|
||||
ConstructorHelper(const DiscreteKey &discreteKey,
|
||||
const std::vector<GaussianFactor::shared_ptr> &factors)
|
||||
|
|
@ -88,7 +90,9 @@ struct HybridGaussianFactor::ConstructorHelper {
|
|||
// Build the FactorValuePairs DecisionTree
|
||||
pairs = FactorValuePairs(
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors),
|
||||
[](const auto &f) { return std::pair{f, 0.0}; });
|
||||
[](const auto &f) {
|
||||
return std::pair{f, 0.0};
|
||||
});
|
||||
}
|
||||
|
||||
ConstructorHelper(const DiscreteKey &discreteKey,
|
||||
|
|
@ -147,15 +151,17 @@ HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
|
|||
/* *******************************************************************************/
|
||||
bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
|
||||
const This *e = dynamic_cast<const This *>(&lf);
|
||||
if (e == nullptr) return false;
|
||||
if (e == nullptr)
|
||||
return false;
|
||||
|
||||
// This will return false if either factors_ is empty or e->factors_ is
|
||||
// empty, but not if both are empty or both are not empty:
|
||||
if (factors_.empty() ^ e->factors_.empty()) return false;
|
||||
if (factors_.empty() ^ e->factors_.empty())
|
||||
return false;
|
||||
|
||||
// Check the base and the factors:
|
||||
auto compareFunc = [tol](const auto &pair1, const auto &pair2) {
|
||||
auto f1 = pair1.first, f2 = pair2.first;
|
||||
auto f1 = pair1.first, f2 = pair2.first;
|
||||
bool match = (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
|
||||
return match && gtsam::equal(pair1.second, pair2.second, tol);
|
||||
};
|
||||
|
|
@ -166,7 +172,6 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
|
|||
void HybridGaussianFactor::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << (s.empty() ? "" : s + "\n");
|
||||
std::cout << "HybridGaussianFactor" << std::endl;
|
||||
HybridFactor::print("", formatter);
|
||||
std::cout << "{\n";
|
||||
if (factors_.empty()) {
|
||||
|
|
@ -179,19 +184,19 @@ void HybridGaussianFactor::print(const std::string &s,
|
|||
std::cout << ":\n";
|
||||
if (pair.first) {
|
||||
pair.first->print("", formatter);
|
||||
std::cout << "scalar: " << pair.second << "\n";
|
||||
return rd.str();
|
||||
} else {
|
||||
return "nullptr";
|
||||
}
|
||||
std::cout << "scalar: " << pair.second << "\n";
|
||||
});
|
||||
}
|
||||
std::cout << "}" << std::endl;
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()(
|
||||
const DiscreteValues &assignment) const {
|
||||
HybridGaussianFactor::sharedFactor
|
||||
HybridGaussianFactor::operator()(const DiscreteValues &assignment) const {
|
||||
return factors_(assignment).first;
|
||||
}
|
||||
|
||||
|
|
@ -203,8 +208,9 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
|
|||
|
||||
/* *******************************************************************************/
|
||||
/// Helper method to compute the error of a component.
|
||||
static double PotentiallyPrunedComponentError(
|
||||
const GaussianFactor::shared_ptr &gf, const VectorValues &values) {
|
||||
static double
|
||||
PotentiallyPrunedComponentError(const GaussianFactor::shared_ptr &gf,
|
||||
const VectorValues &values) {
|
||||
// Check if valid pointer
|
||||
if (gf) {
|
||||
return gf->error(values);
|
||||
|
|
@ -216,8 +222,8 @@ static double PotentiallyPrunedComponentError(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
|
||||
const VectorValues &continuousValues) const {
|
||||
AlgebraicDecisionTree<Key>
|
||||
HybridGaussianFactor::errorTree(const VectorValues &continuousValues) const {
|
||||
// functor to convert from sharedFactor to double error value.
|
||||
auto errorFunc = [&continuousValues](const auto &pair) {
|
||||
return PotentiallyPrunedComponentError(pair.first, continuousValues);
|
||||
|
|
@ -233,4 +239,4 @@ double HybridGaussianFactor::error(const HybridValues &values) const {
|
|||
return PotentiallyPrunedComponentError(pair.first, values.continuous());
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@ using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
|
|||
* @ingroup hybrid
|
||||
*/
|
||||
class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
||||
public:
|
||||
public:
|
||||
using Base = HybridFactor;
|
||||
using This = HybridGaussianFactor;
|
||||
using shared_ptr = std::shared_ptr<This>;
|
||||
|
|
@ -68,11 +68,11 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
|||
/// typedef for Decision Tree of Gaussian factors and arbitrary value.
|
||||
using FactorValuePairs = DecisionTree<Key, GaussianFactorValuePair>;
|
||||
|
||||
private:
|
||||
private:
|
||||
/// Decision tree of Gaussian factors indexed by discrete keys.
|
||||
FactorValuePairs factors_;
|
||||
|
||||
public:
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -120,8 +120,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
|||
|
||||
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
|
||||
|
||||
void print(const std::string &s = "", const KeyFormatter &formatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
void
|
||||
print(const std::string &s = "",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// @}
|
||||
/// @name Standard API
|
||||
|
|
@ -137,8 +138,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
|||
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
|
||||
* as the factors involved, and leaf values as the error.
|
||||
*/
|
||||
AlgebraicDecisionTree<Key> errorTree(
|
||||
const VectorValues &continuousValues) const override;
|
||||
AlgebraicDecisionTree<Key>
|
||||
errorTree(const VectorValues &continuousValues) const override;
|
||||
|
||||
/**
|
||||
* @brief Compute the log-likelihood, including the log-normalizing constant.
|
||||
|
|
@ -148,13 +149,14 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
|||
|
||||
/// Getter for GaussianFactor decision tree
|
||||
const FactorValuePairs &factors() const { return factors_; }
|
||||
/**
|
||||
/**
|
||||
* @brief Helper function to return factors and functional to create a
|
||||
* DecisionTree of Gaussian Factor Graphs.
|
||||
*
|
||||
* @return HybridGaussianProductFactor
|
||||
*/
|
||||
virtual HybridGaussianProductFactor asProductFactor() const;
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
@ -189,4 +191,4 @@ private:
|
|||
template <>
|
||||
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
* @date Mar 11, 2022
|
||||
*/
|
||||
|
||||
#include "gtsam/discrete/DiscreteValues.h"
|
||||
#include <gtsam/base/utilities.h>
|
||||
#include <gtsam/discrete/Assignment.h>
|
||||
#include <gtsam/discrete/DiscreteEliminationTree.h>
|
||||
|
|
@ -78,10 +79,14 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
|
|||
const DiscreteValues &assignment,
|
||||
const KeyFormatter &keyFormatter) {
|
||||
if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
|
||||
hgf->operator()(assignment)
|
||||
->print("HybridGaussianFactor, component:", keyFormatter);
|
||||
if (assignment.empty())
|
||||
hgf->print("HybridGaussianFactor:", keyFormatter);
|
||||
else
|
||||
hgf->operator()(assignment)
|
||||
->print("HybridGaussianFactor, component:", keyFormatter);
|
||||
} else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
|
||||
factor->print("GaussianFactor:\n", keyFormatter);
|
||||
|
||||
} else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
|
||||
factor->print("DiscreteFactor:\n", keyFormatter);
|
||||
} else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(factor)) {
|
||||
|
|
@ -90,15 +95,38 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
|
|||
} else if (hc->isDiscrete()) {
|
||||
factor->print("DiscreteConditional:\n", keyFormatter);
|
||||
} else {
|
||||
hc->asHybrid()
|
||||
->choose(assignment)
|
||||
->print("HybridConditional, component:\n", keyFormatter);
|
||||
if (assignment.empty())
|
||||
hc->print("HybridConditional:", keyFormatter);
|
||||
else
|
||||
hc->asHybrid()
|
||||
->choose(assignment)
|
||||
->print("HybridConditional, component:\n", keyFormatter);
|
||||
}
|
||||
} else {
|
||||
factor->print("Unknown factor type\n", keyFormatter);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridGaussianFactorGraph::print(const std::string &s,
|
||||
const KeyFormatter &keyFormatter) const {
|
||||
std::cout << (s.empty() ? "" : s + " ") << std::endl;
|
||||
std::cout << "size: " << size() << std::endl;
|
||||
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
auto &&factor = factors_[i];
|
||||
if (factor == nullptr) {
|
||||
std::cout << "Factor " << i << ": nullptr\n";
|
||||
continue;
|
||||
}
|
||||
// Print the factor
|
||||
std::cout << "Factor " << i << "\n";
|
||||
printFactor(factor, {}, keyFormatter);
|
||||
std::cout << "\n";
|
||||
}
|
||||
std::cout.flush();
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridGaussianFactorGraph::printErrors(
|
||||
const HybridValues &values, const std::string &str,
|
||||
|
|
@ -106,7 +134,7 @@ void HybridGaussianFactorGraph::printErrors(
|
|||
const std::function<bool(const Factor * /*factor*/,
|
||||
double /*whitenedError*/, size_t /*index*/)>
|
||||
&printCondition) const {
|
||||
std::cout << str << "size: " << size() << std::endl << std::endl;
|
||||
std::cout << str << " size: " << size() << std::endl << std::endl;
|
||||
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
auto &&factor = factors_[i];
|
||||
|
|
@ -267,7 +295,7 @@ using Result = std::pair<std::shared_ptr<GaussianConditional>,
|
|||
*/
|
||||
static std::shared_ptr<Factor> createDiscreteFactor(
|
||||
const DecisionTree<Key, Result> &eliminationResults,
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
auto negLogProbability = [&](const Result &pair) -> double {
|
||||
const auto &[conditional, factor] = pair;
|
||||
if (conditional && factor) {
|
||||
|
|
@ -298,7 +326,7 @@ static std::shared_ptr<Factor> createDiscreteFactor(
|
|||
// for conditional constants.
|
||||
static std::shared_ptr<Factor> createHybridGaussianFactor(
|
||||
const DecisionTree<Key, Result> &eliminationResults,
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
// Correct for the normalization constant used up by the conditional
|
||||
auto correct = [&](const Result &pair) -> GaussianFactorValuePair {
|
||||
const auto &[conditional, factor] = pair;
|
||||
|
|
@ -459,6 +487,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
|
|||
} else if (hybrid_factor->isHybrid()) {
|
||||
only_continuous = false;
|
||||
only_discrete = false;
|
||||
break;
|
||||
}
|
||||
} else if (auto cont_factor =
|
||||
std::dynamic_pointer_cast<GaussianFactor>(factor)) {
|
||||
|
|
@ -495,10 +524,11 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
|
|||
} else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
|
||||
// If discrete, just add its errorTree as well
|
||||
result = result + df->errorTree();
|
||||
} else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
|
||||
// For a continuous only factor, just add its error
|
||||
result = result + gf->error(continuousValues);
|
||||
} else {
|
||||
// Everything else is a continuous only factor
|
||||
HybridValues hv(continuousValues, DiscreteValues());
|
||||
result = result + factor->error(hv); // NOTE: yes, you can add constants
|
||||
throwRuntimeError("HybridGaussianFactorGraph::errorTree", factor);
|
||||
}
|
||||
}
|
||||
return result;
|
||||
|
|
@ -533,8 +563,13 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose(
|
|||
gfg.push_back(gf);
|
||||
} else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
gfg.push_back((*hgf)(assignment));
|
||||
} else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
|
||||
} else if (auto hgc = std::dynamic_pointer_cast<HybridGaussianConditional>(f)) {
|
||||
gfg.push_back((*hgc)(assignment));
|
||||
} else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(f)) {
|
||||
if (auto gc = hc->asGaussian())
|
||||
gfg.push_back(gc);
|
||||
else if (auto hgc = hc->asHybrid())
|
||||
gfg.push_back((*hgc)(assignment));
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -145,10 +145,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
|
|||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
// TODO(dellaert): customize print and equals.
|
||||
// void print(
|
||||
// const std::string& s = "HybridGaussianFactorGraph",
|
||||
// const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
|
||||
void
|
||||
print(const std::string &s = "HybridGaussianFactorGraph",
|
||||
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/**
|
||||
* @brief Print the errors of each factor in the hybrid factor graph.
|
||||
|
|
|
|||
|
|
@ -196,8 +196,8 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
|
|||
}
|
||||
};
|
||||
|
||||
HybridGaussianFactor::FactorValuePairs linearized_factors(factors_,
|
||||
linearizeDT);
|
||||
DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
|
||||
linearized_factors(factors_, linearizeDT);
|
||||
|
||||
return std::make_shared<HybridGaussianFactor>(discreteKeys_,
|
||||
linearized_factors);
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/inference/DotWriter.h>
|
||||
|
||||
#include "Switching.h"
|
||||
|
||||
|
|
@ -28,9 +29,319 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using noiseModel::Isotropic;
|
||||
using symbol_shorthand::D;
|
||||
using symbol_shorthand::M;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::Y;
|
||||
|
||||
static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2), m3(M(3), 2);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
|
||||
// Test multifrontal elimination
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
// Add priors on x0 and c1
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
|
||||
Ordering ordering;
|
||||
ordering.push_back(X(0));
|
||||
auto result = hfg.eliminatePartialMultifrontal(ordering);
|
||||
|
||||
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
||||
EXPECT_LONGS_EQUAL(result.second->size(), 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace two {
|
||||
std::vector<GaussianFactor::shared_ptr> components(Key key) {
|
||||
return {std::make_shared<JacobianFactor>(key, I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(key, I_3x3, Vector3::Ones())};
|
||||
}
|
||||
} // namespace two
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
|
||||
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
// TODO(Varun) Adding extra discrete variable not connected to continuous
|
||||
// variable throws segfault
|
||||
// hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4"));
|
||||
|
||||
HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal();
|
||||
|
||||
// The bayes tree should have 3 cliques
|
||||
EXPECT_LONGS_EQUAL(3, result->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
// Prior on x0
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
// Factor between x0-x1
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
// Hybrid factor P(x1|c1)
|
||||
hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
|
||||
// Prior factor on c1
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
|
||||
// Get a constrained ordering keeping c1 last
|
||||
auto ordering_full = HybridOrdering(hfg);
|
||||
|
||||
// Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
|
||||
HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
|
||||
|
||||
EXPECT_LONGS_EQUAL(3, hbt->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check assembling the Bayes Tree roots after we do partial elimination
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
|
||||
|
||||
hfg.add(HybridGaussianFactor(m0, two::components(X(0))));
|
||||
hfg.add(HybridGaussianFactor(m1, two::components(X(2))));
|
||||
|
||||
hfg.add(DecisionTreeFactor({m1, m2}, "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));
|
||||
|
||||
hfg.add(HybridGaussianFactor(m3, two::components(X(3))));
|
||||
hfg.add(HybridGaussianFactor(m2, two::components(X(5))));
|
||||
|
||||
auto ordering_full =
|
||||
Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)});
|
||||
|
||||
const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// 9 cliques in the bayes tree and 0 remaining variables to eliminate.
|
||||
EXPECT_LONGS_EQUAL(9, hbt->size());
|
||||
EXPECT_LONGS_EQUAL(0, remaining->size());
|
||||
|
||||
/*
|
||||
(Fan) Explanation: the Junction tree will need to re-eliminate to get to the
|
||||
marginal on X(1), which is not possible because it involves eliminating
|
||||
discrete before continuous. The solution to this, however, is in Murphy02.
|
||||
TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
|
||||
And I believe that we should do this.
|
||||
*/
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg,
|
||||
const HybridBayesTree::shared_ptr &hbt,
|
||||
const Ordering &ordering) {
|
||||
DotWriter dw;
|
||||
dw.positionHints['c'] = 2;
|
||||
dw.positionHints['x'] = 1;
|
||||
std::cout << hfg->dot(DefaultKeyFormatter, dw);
|
||||
std::cout << "\n";
|
||||
hbt->dot(std::cout);
|
||||
|
||||
std::cout << "\n";
|
||||
std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(HybridGaussianFactorGraph, Switching) {
|
||||
auto N = 12;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
// X(5) will be the center, X(1-4), X(6-9)
|
||||
// X(3), X(7)
|
||||
// X(2), X(8)
|
||||
// X(1), X(4), X(6), X(9)
|
||||
// M(5) will be the center, M(1-4), M(6-8)
|
||||
// M(3), M(7)
|
||||
// M(1), M(4), M(2), M(6), M(8)
|
||||
// auto ordering_full =
|
||||
// Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
|
||||
// X(5),
|
||||
// M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
|
||||
KeyVector ordering;
|
||||
|
||||
{
|
||||
std::vector<int> naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
std::vector<Key> ordX;
|
||||
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
|
||||
[](int x) { return X(x); });
|
||||
|
||||
auto [ndX, lvls] = makeBinaryOrdering(ordX);
|
||||
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
|
||||
// TODO(dellaert): this has no effect!
|
||||
for (auto &l : lvls) {
|
||||
l = -l;
|
||||
}
|
||||
}
|
||||
{
|
||||
std::vector<int> naturalC(N - 1);
|
||||
std::iota(naturalC.begin(), naturalC.end(), 1);
|
||||
std::vector<Key> ordC;
|
||||
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
|
||||
[](int x) { return M(x); });
|
||||
|
||||
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
|
||||
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
|
||||
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
|
||||
}
|
||||
auto ordering_full = Ordering(ordering);
|
||||
|
||||
const auto [hbt, remaining] =
|
||||
hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// 12 cliques in the bayes tree and 0 remaining variables to eliminate.
|
||||
EXPECT_LONGS_EQUAL(12, hbt->size());
|
||||
EXPECT_LONGS_EQUAL(0, remaining->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(HybridGaussianFactorGraph, SwitchingISAM) {
|
||||
auto N = 11;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
// X(5) will be the center, X(1-4), X(6-9)
|
||||
// X(3), X(7)
|
||||
// X(2), X(8)
|
||||
// X(1), X(4), X(6), X(9)
|
||||
// M(5) will be the center, M(1-4), M(6-8)
|
||||
// M(3), M(7)
|
||||
// M(1), M(4), M(2), M(6), M(8)
|
||||
// auto ordering_full =
|
||||
// Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
|
||||
// X(5),
|
||||
// M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
|
||||
KeyVector ordering;
|
||||
|
||||
{
|
||||
std::vector<int> naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
std::vector<Key> ordX;
|
||||
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
|
||||
[](int x) { return X(x); });
|
||||
|
||||
auto [ndX, lvls] = makeBinaryOrdering(ordX);
|
||||
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
|
||||
// TODO(dellaert): this has no effect!
|
||||
for (auto &l : lvls) {
|
||||
l = -l;
|
||||
}
|
||||
}
|
||||
{
|
||||
std::vector<int> naturalC(N - 1);
|
||||
std::iota(naturalC.begin(), naturalC.end(), 1);
|
||||
std::vector<Key> ordC;
|
||||
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
|
||||
[](int x) { return M(x); });
|
||||
|
||||
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
|
||||
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
|
||||
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
|
||||
}
|
||||
auto ordering_full = Ordering(ordering);
|
||||
|
||||
const auto [hbt, remaining] =
|
||||
hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
auto new_fg = makeSwitchingChain(12);
|
||||
auto isam = HybridGaussianISAM(*hbt);
|
||||
|
||||
// Run an ISAM update.
|
||||
HybridGaussianFactorGraph factorGraph;
|
||||
factorGraph.push_back(new_fg->at(new_fg->size() - 2));
|
||||
factorGraph.push_back(new_fg->at(new_fg->size() - 1));
|
||||
isam.update(factorGraph);
|
||||
|
||||
// ISAM should have 12 factors after the last update
|
||||
EXPECT_LONGS_EQUAL(12, isam.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
|
||||
const int N = 7;
|
||||
auto hfg = makeSwitchingChain(N, X);
|
||||
hfg->push_back(*makeSwitchingChain(N, Y, D));
|
||||
|
||||
for (int t = 1; t <= N; t++) {
|
||||
hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
KeyVector ordering;
|
||||
|
||||
KeyVector naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
KeyVector ordX;
|
||||
for (size_t i = 1; i <= N; i++) {
|
||||
ordX.emplace_back(X(i));
|
||||
ordX.emplace_back(Y(i));
|
||||
}
|
||||
|
||||
for (size_t i = 1; i <= N - 1; i++) {
|
||||
ordX.emplace_back(M(i));
|
||||
}
|
||||
for (size_t i = 1; i <= N - 1; i++) {
|
||||
ordX.emplace_back(D(i));
|
||||
}
|
||||
|
||||
{
|
||||
DotWriter dw;
|
||||
dw.positionHints['x'] = 1;
|
||||
dw.positionHints['c'] = 0;
|
||||
dw.positionHints['d'] = 3;
|
||||
dw.positionHints['y'] = 2;
|
||||
// std::cout << hfg->dot(DefaultKeyFormatter, dw);
|
||||
// std::cout << "\n";
|
||||
}
|
||||
|
||||
{
|
||||
DotWriter dw;
|
||||
dw.positionHints['y'] = 9;
|
||||
// dw.positionHints['c'] = 0;
|
||||
// dw.positionHints['d'] = 3;
|
||||
dw.positionHints['x'] = 1;
|
||||
// std::cout << "\n";
|
||||
// std::cout << hfg->eliminateSequential(Ordering(ordX))
|
||||
// ->dot(DefaultKeyFormatter, dw);
|
||||
// hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout);
|
||||
}
|
||||
|
||||
Ordering ordering_partial;
|
||||
for (size_t i = 1; i <= N; i++) {
|
||||
ordering_partial.emplace_back(X(i));
|
||||
ordering_partial.emplace_back(Y(i));
|
||||
}
|
||||
const auto [hbn, remaining] =
|
||||
hfg->eliminatePartialSequential(ordering_partial);
|
||||
|
||||
EXPECT_LONGS_EQUAL(14, hbn->size());
|
||||
EXPECT_LONGS_EQUAL(11, remaining->size());
|
||||
|
||||
{
|
||||
DotWriter dw;
|
||||
dw.positionHints['x'] = 1;
|
||||
dw.positionHints['c'] = 0;
|
||||
dw.positionHints['d'] = 3;
|
||||
dw.positionHints['y'] = 2;
|
||||
// std::cout << remaining->dot(DefaultKeyFormatter, dw);
|
||||
// std::cout << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
// Test multifrontal optimize
|
||||
|
|
@ -97,7 +408,8 @@ TEST(HybridBayesTree, OptimizeAssignment) {
|
|||
|
||||
// Create ordering.
|
||||
Ordering ordering;
|
||||
for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
|
||||
for (size_t k = 0; k < s.K; k++)
|
||||
ordering.push_back(X(k));
|
||||
|
||||
const auto [hybridBayesNet, remainingFactorGraph] =
|
||||
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
|
||||
|
|
@ -139,20 +451,21 @@ TEST(HybridBayesTree, Optimize) {
|
|||
|
||||
// Create ordering.
|
||||
Ordering ordering;
|
||||
for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
|
||||
for (size_t k = 0; k < s.K; k++)
|
||||
ordering.push_back(X(k));
|
||||
|
||||
const auto [hybridBayesNet, remainingFactorGraph] =
|
||||
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
|
||||
|
||||
DiscreteFactorGraph dfg;
|
||||
for (auto&& f : *remainingFactorGraph) {
|
||||
for (auto &&f : *remainingFactorGraph) {
|
||||
auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(f);
|
||||
assert(discreteFactor);
|
||||
dfg.push_back(discreteFactor);
|
||||
}
|
||||
|
||||
// Add the probabilities for each branch
|
||||
DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}};
|
||||
DiscreteKeys discrete_keys = {m0, m1, m2};
|
||||
vector<double> probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656,
|
||||
0.037152205, 0.12248971, 0.07349729, 0.08};
|
||||
dfg.emplace_shared<DecisionTreeFactor>(discrete_keys, probs);
|
||||
|
|
|
|||
|
|
@ -104,7 +104,7 @@ TEST(HybridGaussianFactor, Keys) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactor, Printing) {
|
||||
TEST_DISABLED(HybridGaussianFactor, Printing) {
|
||||
using namespace test_constructor;
|
||||
HybridGaussianFactor hybridFactor(m1, {f10, f11});
|
||||
|
||||
|
|
@ -123,6 +123,7 @@ Hybrid [x1 x2; 1]{
|
|||
]
|
||||
b = [ 0 0 ]
|
||||
No noise model
|
||||
scalar: 0
|
||||
|
||||
1 Leaf :
|
||||
A[x1] = [
|
||||
|
|
@ -135,6 +136,7 @@ Hybrid [x1 x2; 1]{
|
|||
]
|
||||
b = [ 0 0 ]
|
||||
No noise model
|
||||
scalar: 0
|
||||
|
||||
}
|
||||
)";
|
||||
|
|
|
|||
|
|
@ -13,39 +13,35 @@
|
|||
* @file testHybridGaussianFactorGraph.cpp
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/DotWriter.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <iterator>
|
||||
#include <memory>
|
||||
#include <numeric>
|
||||
#include <vector>
|
||||
|
||||
#include "Switching.h"
|
||||
|
|
@ -54,17 +50,15 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
using gtsam::symbol_shorthand::D;
|
||||
using gtsam::symbol_shorthand::M;
|
||||
using gtsam::symbol_shorthand::N;
|
||||
using gtsam::symbol_shorthand::X;
|
||||
using gtsam::symbol_shorthand::Y;
|
||||
using gtsam::symbol_shorthand::Z;
|
||||
|
||||
// Set up sampling
|
||||
std::mt19937_64 kRng(42);
|
||||
|
||||
static const DiscreteKey m1(M(1), 2);
|
||||
static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, Creation) {
|
||||
|
|
@ -77,7 +71,7 @@ TEST(HybridGaussianFactorGraph, Creation) {
|
|||
// Define a hybrid gaussian conditional P(x0|x1, c0)
|
||||
// and add it to the factor graph.
|
||||
HybridGaussianConditional gm(
|
||||
{M(0), 2},
|
||||
m0,
|
||||
{std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
|
||||
std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
|
||||
I_3x3)});
|
||||
|
|
@ -98,22 +92,6 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) {
|
|||
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
|
||||
// Test multifrontal elimination
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
// Add priors on x0 and c1
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
|
||||
Ordering ordering;
|
||||
ordering.push_back(X(0));
|
||||
auto result = hfg.eliminatePartialMultifrontal(ordering);
|
||||
|
||||
EXPECT_LONGS_EQUAL(result.first->size(), 1);
|
||||
EXPECT_LONGS_EQUAL(result.second->size(), 1);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
namespace two {
|
||||
|
|
@ -121,7 +99,7 @@ std::vector<GaussianFactor::shared_ptr> components(Key key) {
|
|||
return {std::make_shared<JacobianFactor>(key, I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(key, I_3x3, Vector3::Ones())};
|
||||
}
|
||||
} // namespace two
|
||||
} // namespace two
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
|
||||
|
|
@ -179,7 +157,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
|
|||
// Discrete probability table for c1
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
// Joint discrete probability table for c1, c2
|
||||
hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
|
||||
hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4"));
|
||||
|
||||
HybridBayesNet::shared_ptr result = hfg.eliminateSequential();
|
||||
|
||||
|
|
@ -187,296 +165,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
|
|||
EXPECT_LONGS_EQUAL(4, result->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(1))));
|
||||
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
// TODO(Varun) Adding extra discrete variable not connected to continuous
|
||||
// variable throws segfault
|
||||
// hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
|
||||
|
||||
HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal();
|
||||
|
||||
// The bayes tree should have 3 cliques
|
||||
EXPECT_LONGS_EQUAL(3, result->size());
|
||||
// GTSAM_PRINT(*result);
|
||||
// GTSAM_PRINT(*result->marginalFactor(M(2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
// Prior on x0
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
// Factor between x0-x1
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
|
||||
// Hybrid factor P(x1|c1)
|
||||
hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
|
||||
// Prior factor on c1
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
|
||||
// Get a constrained ordering keeping c1 last
|
||||
auto ordering_full = HybridOrdering(hfg);
|
||||
|
||||
// Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
|
||||
HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
|
||||
|
||||
EXPECT_LONGS_EQUAL(3, hbt->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/*
|
||||
* This test is about how to assemble the Bayes Tree roots after we do partial
|
||||
* elimination
|
||||
*/
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
|
||||
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
|
||||
|
||||
hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0))));
|
||||
hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2))));
|
||||
|
||||
hfg.add(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(4), I_3x3, X(5), -I_3x3, Z_3x1));
|
||||
|
||||
hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3))));
|
||||
hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5))));
|
||||
|
||||
auto ordering_full =
|
||||
Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)});
|
||||
|
||||
const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// 9 cliques in the bayes tree and 0 remaining variables to eliminate.
|
||||
EXPECT_LONGS_EQUAL(9, hbt->size());
|
||||
EXPECT_LONGS_EQUAL(0, remaining->size());
|
||||
|
||||
/*
|
||||
(Fan) Explanation: the Junction tree will need to re-eliminate to get to the
|
||||
marginal on X(1), which is not possible because it involves eliminating
|
||||
discrete before continuous. The solution to this, however, is in Murphy02.
|
||||
TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
|
||||
And I believe that we should do this.
|
||||
*/
|
||||
}
|
||||
|
||||
void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg,
|
||||
const HybridBayesTree::shared_ptr &hbt,
|
||||
const Ordering &ordering) {
|
||||
DotWriter dw;
|
||||
dw.positionHints['c'] = 2;
|
||||
dw.positionHints['x'] = 1;
|
||||
std::cout << hfg->dot(DefaultKeyFormatter, dw);
|
||||
std::cout << "\n";
|
||||
hbt->dot(std::cout);
|
||||
|
||||
std::cout << "\n";
|
||||
std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(HybridGaussianFactorGraph, Switching) {
|
||||
auto N = 12;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
// X(5) will be the center, X(1-4), X(6-9)
|
||||
// X(3), X(7)
|
||||
// X(2), X(8)
|
||||
// X(1), X(4), X(6), X(9)
|
||||
// M(5) will be the center, M(1-4), M(6-8)
|
||||
// M(3), M(7)
|
||||
// M(1), M(4), M(2), M(6), M(8)
|
||||
// auto ordering_full =
|
||||
// Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
|
||||
// X(5),
|
||||
// M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
|
||||
KeyVector ordering;
|
||||
|
||||
{
|
||||
std::vector<int> naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
std::vector<Key> ordX;
|
||||
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
|
||||
[](int x) { return X(x); });
|
||||
|
||||
auto [ndX, lvls] = makeBinaryOrdering(ordX);
|
||||
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
|
||||
// TODO(dellaert): this has no effect!
|
||||
for (auto &l : lvls) {
|
||||
l = -l;
|
||||
}
|
||||
}
|
||||
{
|
||||
std::vector<int> naturalC(N - 1);
|
||||
std::iota(naturalC.begin(), naturalC.end(), 1);
|
||||
std::vector<Key> ordC;
|
||||
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
|
||||
[](int x) { return M(x); });
|
||||
|
||||
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
|
||||
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
|
||||
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
|
||||
}
|
||||
auto ordering_full = Ordering(ordering);
|
||||
|
||||
// GTSAM_PRINT(*hfg);
|
||||
// GTSAM_PRINT(ordering_full);
|
||||
|
||||
const auto [hbt, remaining] =
|
||||
hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
// 12 cliques in the bayes tree and 0 remaining variables to eliminate.
|
||||
EXPECT_LONGS_EQUAL(12, hbt->size());
|
||||
EXPECT_LONGS_EQUAL(0, remaining->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO(fan): make a graph like Varun's paper one
|
||||
TEST(HybridGaussianFactorGraph, SwitchingISAM) {
|
||||
auto N = 11;
|
||||
auto hfg = makeSwitchingChain(N);
|
||||
|
||||
// X(5) will be the center, X(1-4), X(6-9)
|
||||
// X(3), X(7)
|
||||
// X(2), X(8)
|
||||
// X(1), X(4), X(6), X(9)
|
||||
// M(5) will be the center, M(1-4), M(6-8)
|
||||
// M(3), M(7)
|
||||
// M(1), M(4), M(2), M(6), M(8)
|
||||
// auto ordering_full =
|
||||
// Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
|
||||
// X(5),
|
||||
// M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
|
||||
KeyVector ordering;
|
||||
|
||||
{
|
||||
std::vector<int> naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
std::vector<Key> ordX;
|
||||
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
|
||||
[](int x) { return X(x); });
|
||||
|
||||
auto [ndX, lvls] = makeBinaryOrdering(ordX);
|
||||
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
|
||||
// TODO(dellaert): this has no effect!
|
||||
for (auto &l : lvls) {
|
||||
l = -l;
|
||||
}
|
||||
}
|
||||
{
|
||||
std::vector<int> naturalC(N - 1);
|
||||
std::iota(naturalC.begin(), naturalC.end(), 1);
|
||||
std::vector<Key> ordC;
|
||||
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
|
||||
[](int x) { return M(x); });
|
||||
|
||||
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
|
||||
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
|
||||
std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
|
||||
}
|
||||
auto ordering_full = Ordering(ordering);
|
||||
|
||||
const auto [hbt, remaining] =
|
||||
hfg->eliminatePartialMultifrontal(ordering_full);
|
||||
|
||||
auto new_fg = makeSwitchingChain(12);
|
||||
auto isam = HybridGaussianISAM(*hbt);
|
||||
|
||||
// Run an ISAM update.
|
||||
HybridGaussianFactorGraph factorGraph;
|
||||
factorGraph.push_back(new_fg->at(new_fg->size() - 2));
|
||||
factorGraph.push_back(new_fg->at(new_fg->size() - 1));
|
||||
isam.update(factorGraph);
|
||||
|
||||
// ISAM should have 12 factors after the last update
|
||||
EXPECT_LONGS_EQUAL(12, isam.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
|
||||
const int N = 7;
|
||||
auto hfg = makeSwitchingChain(N, X);
|
||||
hfg->push_back(*makeSwitchingChain(N, Y, D));
|
||||
|
||||
for (int t = 1; t <= N; t++) {
|
||||
hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
KeyVector ordering;
|
||||
|
||||
KeyVector naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
KeyVector ordX;
|
||||
for (size_t i = 1; i <= N; i++) {
|
||||
ordX.emplace_back(X(i));
|
||||
ordX.emplace_back(Y(i));
|
||||
}
|
||||
|
||||
for (size_t i = 1; i <= N - 1; i++) {
|
||||
ordX.emplace_back(M(i));
|
||||
}
|
||||
for (size_t i = 1; i <= N - 1; i++) {
|
||||
ordX.emplace_back(D(i));
|
||||
}
|
||||
|
||||
{
|
||||
DotWriter dw;
|
||||
dw.positionHints['x'] = 1;
|
||||
dw.positionHints['c'] = 0;
|
||||
dw.positionHints['d'] = 3;
|
||||
dw.positionHints['y'] = 2;
|
||||
// std::cout << hfg->dot(DefaultKeyFormatter, dw);
|
||||
// std::cout << "\n";
|
||||
}
|
||||
|
||||
{
|
||||
DotWriter dw;
|
||||
dw.positionHints['y'] = 9;
|
||||
// dw.positionHints['c'] = 0;
|
||||
// dw.positionHints['d'] = 3;
|
||||
dw.positionHints['x'] = 1;
|
||||
// std::cout << "\n";
|
||||
// std::cout << hfg->eliminateSequential(Ordering(ordX))
|
||||
// ->dot(DefaultKeyFormatter, dw);
|
||||
// hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout);
|
||||
}
|
||||
|
||||
Ordering ordering_partial;
|
||||
for (size_t i = 1; i <= N; i++) {
|
||||
ordering_partial.emplace_back(X(i));
|
||||
ordering_partial.emplace_back(Y(i));
|
||||
}
|
||||
const auto [hbn, remaining] =
|
||||
hfg->eliminatePartialSequential(ordering_partial);
|
||||
|
||||
EXPECT_LONGS_EQUAL(14, hbn->size());
|
||||
EXPECT_LONGS_EQUAL(11, remaining->size());
|
||||
|
||||
{
|
||||
DotWriter dw;
|
||||
dw.positionHints['x'] = 1;
|
||||
dw.positionHints['c'] = 0;
|
||||
dw.positionHints['d'] = 3;
|
||||
dw.positionHints['y'] = 2;
|
||||
// std::cout << remaining->dot(DefaultKeyFormatter, dw);
|
||||
// std::cout << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
****************************************************************************/
|
||||
// Select a particular continuous factor graph given a discrete assignment
|
||||
TEST(HybridGaussianFactorGraph, DiscreteSelection) {
|
||||
Switching s(3);
|
||||
|
|
@ -547,23 +237,43 @@ TEST(HybridGaussianFactorGraph, optimize) {
|
|||
// Test adding of gaussian conditional and re-elimination.
|
||||
TEST(HybridGaussianFactorGraph, Conditionals) {
|
||||
Switching switching(4);
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X1)
|
||||
HybridGaussianFactorGraph hfg;
|
||||
hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X0)
|
||||
Ordering ordering;
|
||||
ordering.push_back(X(0));
|
||||
HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering);
|
||||
|
||||
hfg.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1)
|
||||
hfg.push_back(*bayes_net);
|
||||
hfg.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2)
|
||||
hfg.push_back(switching.linearizedFactorGraph.at(5)); // P(M1)
|
||||
ordering.push_back(X(1));
|
||||
ordering.push_back(X(2));
|
||||
ordering.push_back(M(0));
|
||||
ordering.push_back(M(1));
|
||||
HybridGaussianFactorGraph hfg2;
|
||||
hfg2.push_back(*bayes_net); // P(X0)
|
||||
hfg2.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0)
|
||||
hfg2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1)
|
||||
hfg2.push_back(switching.linearizedFactorGraph.at(5)); // P(M1)
|
||||
ordering += X(1), X(2), M(0), M(1);
|
||||
|
||||
bayes_net = hfg.eliminateSequential(ordering);
|
||||
// Created product of first two factors and check eliminate:
|
||||
HybridGaussianFactorGraph fragment;
|
||||
fragment.push_back(hfg2[0]);
|
||||
fragment.push_back(hfg2[1]);
|
||||
|
||||
// Check that product
|
||||
HybridGaussianProductFactor product = fragment.collectProductFactor();
|
||||
auto leaf = fragment(DiscreteValues{{M(0), 0}});
|
||||
EXPECT_LONGS_EQUAL(2, leaf.size());
|
||||
|
||||
// Check product and that pruneEmpty does not touch it
|
||||
auto pruned = product.removeEmpty();
|
||||
LONGS_EQUAL(2, pruned.nrLeaves());
|
||||
|
||||
// Test eliminate
|
||||
auto [hybridConditional, factor] = fragment.eliminate({X(0)});
|
||||
EXPECT(hybridConditional->isHybrid());
|
||||
EXPECT(hybridConditional->keys() == KeyVector({X(0), X(1), M(0)}));
|
||||
|
||||
EXPECT(dynamic_pointer_cast<HybridGaussianFactor>(factor));
|
||||
EXPECT(factor->keys() == KeyVector({X(1), M(0)}));
|
||||
|
||||
bayes_net = hfg2.eliminateSequential(ordering);
|
||||
|
||||
HybridValues result = bayes_net->optimize();
|
||||
|
||||
|
|
@ -647,7 +357,7 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
|
|||
HybridValues delta = hybridBayesNet->optimize();
|
||||
auto error_tree = graph.errorTree(delta.continuous());
|
||||
|
||||
std::vector<DiscreteKey> discrete_keys = {{M(0), 2}, {M(1), 2}};
|
||||
std::vector<DiscreteKey> discrete_keys = {m0, m1};
|
||||
std::vector<double> leaves = {2.7916153, 1.5888555, 1.7233422, 1.6191947};
|
||||
AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
|
||||
|
||||
|
|
@ -712,11 +422,12 @@ TEST(HybridGaussianFactorGraph, collectProductFactor) {
|
|||
|
||||
/* ****************************************************************************/
|
||||
// Check that the factor graph unnormalized probability is proportional to the
|
||||
// Bayes net probability for the given measurements.
|
||||
bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
|
||||
const HybridGaussianFactorGraph &fg, size_t num_samples = 100) {
|
||||
// Bayes net probability for the given measurements.
|
||||
bool
|
||||
ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
|
||||
const HybridGaussianFactorGraph &fg, size_t num_samples = 100) {
|
||||
auto compute_ratio = [&](HybridValues *sample) -> double {
|
||||
sample->update(measurements); // update sample with given measurements:
|
||||
sample->update(measurements); // update sample with given measurements:
|
||||
return bn.evaluate(*sample) / fg.probPrime(*sample);
|
||||
};
|
||||
|
||||
|
|
@ -726,7 +437,8 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
|
|||
// Test ratios for a number of independent samples:
|
||||
for (size_t i = 0; i < num_samples; i++) {
|
||||
HybridValues sample = bn.sample(&kRng);
|
||||
if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6) return false;
|
||||
if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
|
@ -737,7 +449,7 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
|
|||
bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
|
||||
const HybridBayesNet &posterior, size_t num_samples = 100) {
|
||||
auto compute_ratio = [&](HybridValues *sample) -> double {
|
||||
sample->update(measurements); // update sample with given measurements:
|
||||
sample->update(measurements); // update sample with given measurements:
|
||||
return bn.evaluate(*sample) / posterior.evaluate(*sample);
|
||||
};
|
||||
|
||||
|
|
@ -749,7 +461,8 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
|
|||
HybridValues sample = bn.sample(&kRng);
|
||||
// GTSAM_PRINT(sample);
|
||||
// std::cout << "ratio: " << compute_ratio(&sample) << std::endl;
|
||||
if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6) return false;
|
||||
if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using symbol_shorthand::M;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::Z;
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace examples {
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@
|
|||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include "Switching.h"
|
||||
#include "Test.h"
|
||||
|
||||
// Include for test suite
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -498,7 +499,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
|
|||
/****************************************************************************
|
||||
* Test printing
|
||||
*/
|
||||
TEST(HybridNonlinearFactorGraph, Printing) {
|
||||
TEST_DISABLED(HybridNonlinearFactorGraph, Printing) {
|
||||
Switching self(3);
|
||||
|
||||
auto linearizedFactorGraph = self.linearizedFactorGraph;
|
||||
|
|
@ -514,78 +515,98 @@ TEST(HybridNonlinearFactorGraph, Printing) {
|
|||
#ifdef GTSAM_DT_MERGING
|
||||
string expected_hybridFactorGraph = R"(
|
||||
size: 7
|
||||
factor 0:
|
||||
Factor 0
|
||||
GaussianFactor:
|
||||
|
||||
A[x0] = [
|
||||
10
|
||||
10
|
||||
]
|
||||
b = [ -10 ]
|
||||
No noise model
|
||||
factor 1:
|
||||
HybridGaussianFactor
|
||||
|
||||
Factor 1
|
||||
HybridGaussianFactor:
|
||||
Hybrid [x0 x1; m0]{
|
||||
Choice(m0)
|
||||
0 Leaf :
|
||||
A[x0] = [
|
||||
-1
|
||||
-1
|
||||
]
|
||||
A[x1] = [
|
||||
1
|
||||
1
|
||||
]
|
||||
b = [ -1 ]
|
||||
No noise model
|
||||
scalar: 0
|
||||
|
||||
1 Leaf :
|
||||
A[x0] = [
|
||||
-1
|
||||
-1
|
||||
]
|
||||
A[x1] = [
|
||||
1
|
||||
1
|
||||
]
|
||||
b = [ -0 ]
|
||||
No noise model
|
||||
scalar: 0
|
||||
|
||||
}
|
||||
factor 2:
|
||||
HybridGaussianFactor
|
||||
|
||||
Factor 2
|
||||
HybridGaussianFactor:
|
||||
Hybrid [x1 x2; m1]{
|
||||
Choice(m1)
|
||||
0 Leaf :
|
||||
A[x1] = [
|
||||
-1
|
||||
-1
|
||||
]
|
||||
A[x2] = [
|
||||
1
|
||||
1
|
||||
]
|
||||
b = [ -1 ]
|
||||
No noise model
|
||||
scalar: 0
|
||||
|
||||
1 Leaf :
|
||||
A[x1] = [
|
||||
-1
|
||||
-1
|
||||
]
|
||||
A[x2] = [
|
||||
1
|
||||
1
|
||||
]
|
||||
b = [ -0 ]
|
||||
No noise model
|
||||
scalar: 0
|
||||
|
||||
}
|
||||
factor 3:
|
||||
|
||||
Factor 3
|
||||
GaussianFactor:
|
||||
|
||||
A[x1] = [
|
||||
10
|
||||
10
|
||||
]
|
||||
b = [ -10 ]
|
||||
No noise model
|
||||
factor 4:
|
||||
|
||||
Factor 4
|
||||
GaussianFactor:
|
||||
|
||||
A[x2] = [
|
||||
10
|
||||
10
|
||||
]
|
||||
b = [ -10 ]
|
||||
No noise model
|
||||
factor 5: P( m0 ):
|
||||
|
||||
Factor 5
|
||||
DiscreteFactor:
|
||||
P( m0 ):
|
||||
Leaf 0.5
|
||||
|
||||
factor 6: P( m1 | m0 ):
|
||||
|
||||
Factor 6
|
||||
DiscreteFactor:
|
||||
P( m1 | m0 ):
|
||||
Choice(m1)
|
||||
0 Choice(m0)
|
||||
0 0 Leaf 0.33333333
|
||||
|
|
@ -594,6 +615,7 @@ factor 6: P( m1 | m0 ):
|
|||
1 0 Leaf 0.66666667
|
||||
1 1 Leaf 0.4
|
||||
|
||||
|
||||
)";
|
||||
#else
|
||||
string expected_hybridFactorGraph = R"(
|
||||
|
|
@ -686,7 +708,7 @@ factor 6: P( m1 | m0 ):
|
|||
// Expected output for hybridBayesNet.
|
||||
string expected_hybridBayesNet = R"(
|
||||
size: 3
|
||||
conditional 0: Hybrid P( x0 | x1 m0)
|
||||
conditional 0: P( x0 | x1 m0)
|
||||
Discrete Keys = (m0, 2),
|
||||
logNormalizationConstant: 1.38862
|
||||
|
||||
|
|
@ -705,7 +727,7 @@ conditional 0: Hybrid P( x0 | x1 m0)
|
|||
logNormalizationConstant: 1.38862
|
||||
No noise model
|
||||
|
||||
conditional 1: Hybrid P( x1 | x2 m0 m1)
|
||||
conditional 1: P( x1 | x2 m0 m1)
|
||||
Discrete Keys = (m0, 2), (m1, 2),
|
||||
logNormalizationConstant: 1.3935
|
||||
|
||||
|
|
@ -740,7 +762,7 @@ conditional 1: Hybrid P( x1 | x2 m0 m1)
|
|||
logNormalizationConstant: 1.3935
|
||||
No noise model
|
||||
|
||||
conditional 2: Hybrid P( x2 | m0 m1)
|
||||
conditional 2: P( x2 | m0 m1)
|
||||
Discrete Keys = (m0, 2), (m1, 2),
|
||||
logNormalizationConstant: 1.38857
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue