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

Minimize formatting changes

release/4.3a0
Frank Dellaert 2024-10-08 17:57:53 +09:00
parent 88b8dc9afc
commit 9f7ccbb33c
17 changed files with 509 additions and 385 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -40,7 +40,8 @@ namespace gtsam {
* - nrFrontals: Optional size_t for number of frontal variables * - nrFrontals: Optional size_t for number of frontal variables
* - pairs: FactorValuePairs for storing conditionals with their negLogConstant * - pairs: FactorValuePairs for storing conditionals with their negLogConstant
* - conditionals: Conditionals for storing conditionals. TODO(frank): kill! * - conditionals: Conditionals for storing conditionals. TODO(frank): kill!
* - minNegLogConstant: minimum negLogConstant, computed here, subtracted in constructor * - minNegLogConstant: minimum negLogConstant, computed here, subtracted in
* constructor
*/ */
struct HybridGaussianConditional::Helper { struct HybridGaussianConditional::Helper {
std::optional<size_t> nrFrontals; std::optional<size_t> nrFrontals;
@ -53,7 +54,7 @@ struct HybridGaussianConditional::Helper {
/// Construct from a vector of mean and sigma pairs, plus extra args. /// Construct from a vector of mean and sigma pairs, plus extra args.
template <typename... Args> template <typename... Args>
explicit Helper(const DiscreteKey& mode, const P& p, Args&&... args) { explicit Helper(const DiscreteKey &mode, const P &p, Args &&...args) {
nrFrontals = 1; nrFrontals = 1;
minNegLogConstant = std::numeric_limits<double>::infinity(); minNegLogConstant = std::numeric_limits<double>::infinity();
@ -61,8 +62,9 @@ struct HybridGaussianConditional::Helper {
std::vector<GC::shared_ptr> gcs; std::vector<GC::shared_ptr> gcs;
fvs.reserve(p.size()); fvs.reserve(p.size());
gcs.reserve(p.size()); gcs.reserve(p.size());
for (auto&& [mean, sigma] : p) { for (auto &&[mean, sigma] : p) {
auto gaussianConditional = GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma); auto gaussianConditional =
GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
double value = gaussianConditional->negLogConstant(); double value = gaussianConditional->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value); minNegLogConstant = std::min(minNegLogConstant, value);
fvs.emplace_back(gaussianConditional, value); fvs.emplace_back(gaussianConditional, value);
@ -74,8 +76,9 @@ struct HybridGaussianConditional::Helper {
} }
/// Construct from tree of GaussianConditionals. /// Construct from tree of GaussianConditionals.
explicit Helper(const Conditionals& conditionals) explicit Helper(const Conditionals &conditionals)
: conditionals(conditionals), minNegLogConstant(std::numeric_limits<double>::infinity()) { : conditionals(conditionals),
minNegLogConstant(std::numeric_limits<double>::infinity()) {
auto func = [this](const GC::shared_ptr& gc) -> GaussianFactorValuePair { auto func = [this](const GC::shared_ptr& gc) -> GaussianFactorValuePair {
if (!gc) return {nullptr, std::numeric_limits<double>::infinity()}; if (!gc) return {nullptr, std::numeric_limits<double>::infinity()};
if (!nrFrontals) nrFrontals = gc->nrFrontals(); if (!nrFrontals) nrFrontals = gc->nrFrontals();
@ -93,67 +96,63 @@ struct HybridGaussianConditional::Helper {
}; };
/* *******************************************************************************/ /* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(const DiscreteKeys& discreteParents, HybridGaussianConditional::HybridGaussianConditional(
const Helper& helper) const DiscreteKeys& discreteParents, const Helper& helper)
: BaseFactor( : BaseFactor(discreteParents,
discreteParents, FactorValuePairs(helper.pairs,
FactorValuePairs( [&](const GaussianFactorValuePair&
helper.pairs, pair) { // subtract minNegLogConstant
[&](const GaussianFactorValuePair& pair) { // subtract minNegLogConstant return GaussianFactorValuePair{
return GaussianFactorValuePair{pair.first, pair.second - helper.minNegLogConstant}; pair.first,
pair.second - helper.minNegLogConstant};
})), })),
BaseConditional(*helper.nrFrontals), BaseConditional(*helper.nrFrontals),
conditionals_(helper.conditionals), conditionals_(helper.conditionals),
negLogConstant_(helper.minNegLogConstant) {} negLogConstant_(helper.minNegLogConstant) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent, const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr>& conditionals) const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(DiscreteKeys{discreteParent}, : HybridGaussianConditional(DiscreteKeys{discreteParent},
Conditionals({discreteParent}, conditionals)) {} Conditionals({discreteParent}, conditionals)) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent, const DiscreteKey &discreteParent, Key key, //
Key key, // const std::vector<std::pair<Vector, double>> &parameters)
const std::vector<std::pair<Vector, double>>& parameters)
: HybridGaussianConditional(DiscreteKeys{discreteParent}, : HybridGaussianConditional(DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key)) {} Helper(discreteParent, parameters, key)) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent, const DiscreteKey &discreteParent, Key key, //
Key key, // const Matrix &A, Key parent,
const Matrix& A, const std::vector<std::pair<Vector, double>> &parameters)
Key parent, : HybridGaussianConditional(
const std::vector<std::pair<Vector, double>>& parameters) DiscreteKeys{discreteParent},
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key, A, parent)) {} Helper(discreteParent, parameters, key, A, parent)) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent, const DiscreteKey &discreteParent, Key key, //
Key key, // const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
const Matrix& A1, const std::vector<std::pair<Vector, double>> &parameters)
Key parent1, : HybridGaussianConditional(
const Matrix& A2, DiscreteKeys{discreteParent},
Key parent2, Helper(discreteParent, parameters, key, A1, parent1, A2, parent2)) {}
const std::vector<std::pair<Vector, double>>& parameters)
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key, A1, parent1, A2, parent2)) {
}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys& discreteParents, const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals& conditionals) const HybridGaussianConditional::Conditionals &conditionals)
: HybridGaussianConditional(discreteParents, Helper(conditionals)) {} : HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
/* *******************************************************************************/ /* *******************************************************************************/
const HybridGaussianConditional::Conditionals& HybridGaussianConditional::conditionals() const { const HybridGaussianConditional::Conditionals &
HybridGaussianConditional::conditionals() const {
return conditionals_; return conditionals_;
} }
/* *******************************************************************************/ /* *******************************************************************************/
size_t HybridGaussianConditional::nrComponents() const { size_t HybridGaussianConditional::nrComponents() const {
size_t total = 0; size_t total = 0;
conditionals_.visit([&total](const GaussianFactor::shared_ptr& node) { conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) {
if (node) total += 1; if (node) total += 1;
}); });
return total; return total;
@ -161,19 +160,21 @@ size_t HybridGaussianConditional::nrComponents() const {
/* *******************************************************************************/ /* *******************************************************************************/
GaussianConditional::shared_ptr HybridGaussianConditional::choose( GaussianConditional::shared_ptr HybridGaussianConditional::choose(
const DiscreteValues& discreteValues) const { const DiscreteValues &discreteValues) const {
auto& ptr = conditionals_(discreteValues); auto &ptr = conditionals_(discreteValues);
if (!ptr) return nullptr; if (!ptr) return nullptr;
auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr); auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
if (conditional) if (conditional)
return conditional; return conditional;
else else
throw std::logic_error("A HybridGaussianConditional unexpectedly contained a non-conditional"); throw std::logic_error(
"A HybridGaussianConditional unexpectedly contained a non-conditional");
} }
/* *******************************************************************************/ /* *******************************************************************************/
bool HybridGaussianConditional::equals(const HybridFactor& lf, double tol) const { bool HybridGaussianConditional::equals(const HybridFactor &lf,
const This* e = dynamic_cast<const This*>(&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 conditionals_ is empty or e->conditionals_ // This will return false if either conditionals_ is empty or e->conditionals_
@ -182,26 +183,27 @@ bool HybridGaussianConditional::equals(const HybridFactor& lf, double tol) const
// Check the base and the factors: // Check the base and the factors:
return BaseFactor::equals(*e, tol) && return BaseFactor::equals(*e, tol) &&
conditionals_.equals(e->conditionals_, [tol](const auto& f1, const auto& f2) { conditionals_.equals(
e->conditionals_, [tol](const auto &f1, const auto &f2) {
return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol)); return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
}); });
} }
/* *******************************************************************************/ /* *******************************************************************************/
void HybridGaussianConditional::print(const std::string& s, const KeyFormatter& formatter) const { void HybridGaussianConditional::print(const std::string &s,
const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n"); std::cout << (s.empty() ? "" : s + "\n");
BaseConditional::print("", formatter); BaseConditional::print("", formatter);
std::cout << " Discrete Keys = "; std::cout << " Discrete Keys = ";
for (auto& dk : discreteKeys()) { for (auto &dk : discreteKeys()) {
std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
} }
std::cout << std::endl std::cout << std::endl
<< " logNormalizationConstant: " << -negLogConstant() << std::endl << " logNormalizationConstant: " << -negLogConstant() << std::endl
<< std::endl; << std::endl;
conditionals_.print( conditionals_.print(
"", "", [&](Key k) { return formatter(k); },
[&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string {
[&](const GaussianConditional::shared_ptr& gf) -> std::string {
RedirectCout rd; RedirectCout rd;
if (gf && !gf->empty()) { if (gf && !gf->empty()) {
gf->print("", formatter); gf->print("", formatter);
@ -218,19 +220,20 @@ KeyVector HybridGaussianConditional::continuousParents() const {
const auto range = parents(); const auto range = parents();
KeyVector continuousParentKeys(range.begin(), range.end()); KeyVector continuousParentKeys(range.begin(), range.end());
// Loop over all discrete keys: // Loop over all discrete keys:
for (const auto& discreteKey : discreteKeys()) { for (const auto &discreteKey : discreteKeys()) {
const Key key = discreteKey.first; const Key key = discreteKey.first;
// remove that key from continuousParentKeys: // remove that key from continuousParentKeys:
continuousParentKeys.erase( continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
std::remove(continuousParentKeys.begin(), continuousParentKeys.end(), key), continuousParentKeys.end(), key),
continuousParentKeys.end()); continuousParentKeys.end());
} }
return continuousParentKeys; return continuousParentKeys;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool HybridGaussianConditional::allFrontalsGiven(const VectorValues& given) const { bool HybridGaussianConditional::allFrontalsGiven(
for (auto&& kv : given) { const VectorValues &given) const {
for (auto &&kv : given) {
if (given.find(kv.first) == given.end()) { if (given.find(kv.first) == given.end()) {
return false; return false;
} }
@ -240,7 +243,7 @@ bool HybridGaussianConditional::allFrontalsGiven(const VectorValues& given) cons
/* ************************************************************************* */ /* ************************************************************************* */
std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood( std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const VectorValues& given) const { const VectorValues &given) const {
if (!allFrontalsGiven(given)) { if (!allFrontalsGiven(given)) {
throw std::runtime_error( throw std::runtime_error(
"HybridGaussianConditional::likelihood: given values are missing some " "HybridGaussianConditional::likelihood: given values are missing some "
@ -251,29 +254,32 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const KeyVector continuousParentKeys = continuousParents(); const KeyVector continuousParentKeys = continuousParents();
const HybridGaussianFactor::FactorValuePairs likelihoods( const HybridGaussianFactor::FactorValuePairs likelihoods(
conditionals_, conditionals_,
[&](const GaussianConditional::shared_ptr& conditional) -> GaussianFactorValuePair { [&](const GaussianConditional::shared_ptr &conditional)
-> GaussianFactorValuePair {
const auto likelihood_m = conditional->likelihood(given); const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm = conditional->negLogConstant() - negLogConstant_; const double Cgm_Kgcm = conditional->negLogConstant() - negLogConstant_;
return {likelihood_m, Cgm_Kgcm}; return {likelihood_m, Cgm_Kgcm};
}); });
return std::make_shared<HybridGaussianFactor>(discreteParentKeys, likelihoods); return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
likelihoods);
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys& discreteKeys) { std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
std::set<DiscreteKey> s(discreteKeys.begin(), discreteKeys.end()); std::set<DiscreteKey> s(discreteKeys.begin(), discreteKeys.end());
return s; return s;
} }
/* *******************************************************************************/ /* *******************************************************************************/
HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
const DecisionTreeFactor& discreteProbs) const { const DecisionTreeFactor &discreteProbs) const {
// Find keys in discreteProbs.keys() but not in this->keys(): // Find keys in discreteProbs.keys() but not in this->keys():
std::set<Key> mine(this->keys().begin(), this->keys().end()); std::set<Key> mine(this->keys().begin(), this->keys().end());
std::set<Key> theirs(discreteProbs.keys().begin(), discreteProbs.keys().end()); std::set<Key> theirs(discreteProbs.keys().begin(),
discreteProbs.keys().end());
std::vector<Key> diff; std::vector<Key> diff;
std::set_difference( std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
theirs.begin(), theirs.end(), mine.begin(), mine.end(), std::back_inserter(diff)); std::back_inserter(diff));
// Find maximum probability value for every combination of our keys. // Find maximum probability value for every combination of our keys.
Ordering keys(diff); Ordering keys(diff);
@ -281,24 +287,26 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
// Check the max value for every combination of our keys. // Check the max value for every combination of our keys.
// If the max value is 0.0, we can prune the corresponding conditional. // If the max value is 0.0, we can prune the corresponding conditional.
auto pruner = auto pruner = [&](const Assignment<Key> &choices,
[&](const Assignment<Key>& choices, const GaussianConditional::shared_ptr &conditional)
const GaussianConditional::shared_ptr& conditional) -> GaussianConditional::shared_ptr { -> GaussianConditional::shared_ptr {
return (max->evaluate(choices) == 0.0) ? nullptr : conditional; return (max->evaluate(choices) == 0.0) ? nullptr : conditional;
}; };
auto pruned_conditionals = conditionals_.apply(pruner); auto pruned_conditionals = conditionals_.apply(pruner);
return std::make_shared<HybridGaussianConditional>(discreteKeys(), pruned_conditionals); return std::make_shared<HybridGaussianConditional>(discreteKeys(),
pruned_conditionals);
} }
/* *******************************************************************************/ /* *******************************************************************************/
double HybridGaussianConditional::logProbability(const HybridValues& values) const { double HybridGaussianConditional::logProbability(
const HybridValues &values) const {
auto conditional = conditionals_(values.discrete()); auto conditional = conditionals_(values.discrete());
return conditional->logProbability(values.continuous()); return conditional->logProbability(values.continuous());
} }
/* *******************************************************************************/ /* *******************************************************************************/
double HybridGaussianConditional::evaluate(const HybridValues& values) const { double HybridGaussianConditional::evaluate(const HybridValues &values) const {
auto conditional = conditionals_(values.discrete()); auto conditional = conditionals_(values.discrete());
return conditional->evaluate(values.continuous()); return conditional->evaluate(values.continuous());
} }

56
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -50,12 +50,15 @@ struct HybridGaussianFactor::ConstructorHelper {
} }
// Build the FactorValuePairs DecisionTree // Build the FactorValuePairs DecisionTree
pairs = FactorValuePairs( pairs = FactorValuePairs(
DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors), [](const auto& f) { DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors),
return std::pair{f, f ? 0.0 : std::numeric_limits<double>::infinity()}; [](const auto& f) {
return std::pair{f,
f ? 0.0 : std::numeric_limits<double>::infinity()};
}); });
} }
/// Constructor for a single discrete key and a vector of GaussianFactorValuePairs /// Constructor for a single discrete key and a vector of
/// GaussianFactorValuePairs
ConstructorHelper(const DiscreteKey& discreteKey, ConstructorHelper(const DiscreteKey& discreteKey,
const std::vector<GaussianFactorValuePair>& factorPairs) const std::vector<GaussianFactorValuePair>& factorPairs)
: discreteKeys({discreteKey}) { : discreteKeys({discreteKey}) {
@ -71,8 +74,10 @@ struct HybridGaussianFactor::ConstructorHelper {
pairs = FactorValuePairs(discreteKeys, factorPairs); pairs = FactorValuePairs(discreteKeys, factorPairs);
} }
/// Constructor for a vector of discrete keys and a vector of GaussianFactorValuePairs /// Constructor for a vector of discrete keys and a vector of
ConstructorHelper(const DiscreteKeys& discreteKeys, const FactorValuePairs& factorPairs) /// GaussianFactorValuePairs
ConstructorHelper(const DiscreteKeys& discreteKeys,
const FactorValuePairs& factorPairs)
: discreteKeys(discreteKeys) { : discreteKeys(discreteKeys) {
// Extract continuous keys from the first non-null factor // Extract continuous keys from the first non-null factor
// TODO: just stop after first non-null factor // TODO: just stop after first non-null factor
@ -88,24 +93,27 @@ struct HybridGaussianFactor::ConstructorHelper {
}; };
/* *******************************************************************************/ /* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper& helper) HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
: Base(helper.continuousKeys, helper.discreteKeys), factors_(helper.pairs) {} : Base(helper.continuousKeys, helper.discreteKeys),
factors_(helper.pairs) {}
HybridGaussianFactor::HybridGaussianFactor( HybridGaussianFactor::HybridGaussianFactor(
const DiscreteKey& discreteKey, const std::vector<GaussianFactor::shared_ptr>& factorPairs) const DiscreteKey &discreteKey,
const std::vector<GaussianFactor::shared_ptr> &factors)
: HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
HybridGaussianFactor::HybridGaussianFactor(
const DiscreteKey &discreteKey,
const std::vector<GaussianFactorValuePair> &factorPairs)
: HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
HybridGaussianFactor::HybridGaussianFactor(const DiscreteKey& discreteKey, HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactorValuePair>& factorPairs) const FactorValuePairs &factors)
: HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys& discreteKeys,
const FactorValuePairs& factorPairs)
: HybridGaussianFactor(ConstructorHelper(discreteKeys, factorPairs)) {}
/* *******************************************************************************/ /* *******************************************************************************/
bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
const This* e = dynamic_cast<const This*>(&lf); 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 // This will return false if either factors_ is empty or e->factors_ is
@ -122,7 +130,8 @@ bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
} }
/* *******************************************************************************/ /* *******************************************************************************/
void HybridGaussianFactor::print(const std::string& s, const KeyFormatter& formatter) const { void HybridGaussianFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n"); std::cout << (s.empty() ? "" : s + "\n");
HybridFactor::print("", formatter); HybridFactor::print("", formatter);
std::cout << "{\n"; std::cout << "{\n";
@ -148,7 +157,8 @@ void HybridGaussianFactor::print(const std::string& s, const KeyFormatter& forma
} }
/* *******************************************************************************/ /* *******************************************************************************/
GaussianFactorValuePair HybridGaussianFactor::operator()(const DiscreteValues& assignment) const { GaussianFactorValuePair HybridGaussianFactor::operator()(
const DiscreteValues &assignment) const {
return factors_(assignment); return factors_(assignment);
} }
@ -156,15 +166,17 @@ GaussianFactorValuePair HybridGaussianFactor::operator()(const DiscreteValues& a
HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const { HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
// Implemented by creating a new DecisionTree where: // Implemented by creating a new DecisionTree where:
// - The structure (keys and assignments) is preserved from factors_ // - The structure (keys and assignments) is preserved from factors_
// - Each leaf converted to a GaussianFactorGraph with just the factor and its scalar. // - Each leaf converted to a GaussianFactorGraph with just the factor and its
return {{factors_, [](const auto& pair) -> std::pair<GaussianFactorGraph, double> { // scalar.
return {{factors_,
[](const auto& pair) -> std::pair<GaussianFactorGraph, double> {
return {GaussianFactorGraph{pair.first}, pair.second}; return {GaussianFactorGraph{pair.first}, pair.second};
}}}; }}};
} }
/* *******************************************************************************/ /* *******************************************************************************/
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree( AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
const VectorValues& continuousValues) const { const VectorValues &continuousValues) const {
// functor to convert from sharedFactor to double error value. // functor to convert from sharedFactor to double error value.
auto errorFunc = [&continuousValues](const auto& pair) { auto errorFunc = [&continuousValues](const auto& pair) {
return pair.first ? pair.first->error(continuousValues) + pair.second return pair.first ? pair.first->error(continuousValues) + pair.second

145
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -55,21 +55,23 @@ template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
using std::dynamic_pointer_cast; using std::dynamic_pointer_cast;
using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>; using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
using Result = std::pair<std::shared_ptr<GaussianConditional>, GaussianFactor::shared_ptr>; using Result =
std::pair<std::shared_ptr<GaussianConditional>, GaussianFactor::shared_ptr>;
using ResultTree = DecisionTree<Key, std::pair<Result, double>>; using ResultTree = DecisionTree<Key, std::pair<Result, double>>;
static const VectorValues kEmpty; static const VectorValues kEmpty;
/* ************************************************************************ */ /* ************************************************************************ */
// Throw a runtime exception for method specified in string s, and factor f: // Throw a runtime exception for method specified in string s, and factor f:
static void throwRuntimeError(const std::string& s, const std::shared_ptr<Factor>& f) { static void throwRuntimeError(const std::string& s,
const std::shared_ptr<Factor>& f) {
auto& fr = *f; auto& fr = *f;
throw std::runtime_error(s + " not implemented for factor type " + demangle(typeid(fr).name()) + throw std::runtime_error(s + " not implemented for factor type " +
"."); demangle(typeid(fr).name()) + ".");
} }
/* ************************************************************************ */ /* ************************************************************************ */
const Ordering HybridOrdering(const HybridGaussianFactorGraph& graph) { const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) {
KeySet discrete_keys = graph.discreteKeySet(); KeySet discrete_keys = graph.discreteKeySet();
const VariableIndex index(graph); const VariableIndex index(graph);
return Ordering::ColamdConstrainedLast( return Ordering::ColamdConstrainedLast(
@ -77,20 +79,21 @@ const Ordering HybridOrdering(const HybridGaussianFactorGraph& graph) {
} }
/* ************************************************************************ */ /* ************************************************************************ */
static void printFactor(const std::shared_ptr<Factor>& factor, static void printFactor(const std::shared_ptr<Factor> &factor,
const DiscreteValues& assignment, const DiscreteValues &assignment,
const KeyFormatter& keyFormatter) { const KeyFormatter &keyFormatter) {
if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) { if (auto hgf = dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
if (assignment.empty()) if (assignment.empty())
hgf->print("HybridGaussianFactor:", keyFormatter); hgf->print("HybridGaussianFactor:", keyFormatter);
else else
hgf->operator()(assignment).first->print("HybridGaussianFactor, component:", keyFormatter); hgf->operator()(assignment)
} else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) { .first->print("HybridGaussianFactor, component:", keyFormatter);
} else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
factor->print("GaussianFactor:\n", keyFormatter); factor->print("GaussianFactor:\n", keyFormatter);
} else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) { } else if (auto df = dynamic_pointer_cast<DiscreteFactor>(factor)) {
factor->print("DiscreteFactor:\n", keyFormatter); factor->print("DiscreteFactor:\n", keyFormatter);
} else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(factor)) { } else if (auto hc = dynamic_pointer_cast<HybridConditional>(factor)) {
if (hc->isContinuous()) { if (hc->isContinuous()) {
factor->print("GaussianConditional:\n", keyFormatter); factor->print("GaussianConditional:\n", keyFormatter);
} else if (hc->isDiscrete()) { } else if (hc->isDiscrete()) {
@ -99,7 +102,9 @@ static void printFactor(const std::shared_ptr<Factor>& factor,
if (assignment.empty()) if (assignment.empty())
hc->print("HybridConditional:", keyFormatter); hc->print("HybridConditional:", keyFormatter);
else else
hc->asHybrid()->choose(assignment)->print("HybridConditional, component:\n", keyFormatter); hc->asHybrid()
->choose(assignment)
->print("HybridConditional, component:\n", keyFormatter);
} }
} else { } else {
factor->print("Unknown factor type\n", keyFormatter); factor->print("Unknown factor type\n", keyFormatter);
@ -128,15 +133,15 @@ void HybridGaussianFactorGraph::print(const std::string& s,
/* ************************************************************************ */ /* ************************************************************************ */
void HybridGaussianFactorGraph::printErrors( void HybridGaussianFactorGraph::printErrors(
const HybridValues& values, const HybridValues &values, const std::string &str,
const std::string& str, const KeyFormatter &keyFormatter,
const KeyFormatter& keyFormatter, const std::function<bool(const Factor * /*factor*/,
const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>& double /*whitenedError*/, size_t /*index*/)>
printCondition) const { &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++) { for (size_t i = 0; i < factors_.size(); i++) {
auto&& factor = factors_[i]; auto &&factor = factors_[i];
if (factor == nullptr) { if (factor == nullptr) {
std::cout << "Factor " << i << ": nullptr\n"; std::cout << "Factor " << i << ": nullptr\n";
continue; continue;
@ -154,7 +159,8 @@ void HybridGaussianFactorGraph::printErrors(
} }
/* ************************************************************************ */ /* ************************************************************************ */
HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor() const { HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor()
const {
HybridGaussianProductFactor result; HybridGaussianProductFactor result;
for (auto& f : factors_) { for (auto& f : factors_) {
@ -194,10 +200,11 @@ HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor() co
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> continuousElimination( static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
const HybridGaussianFactorGraph& factors, const Ordering& frontalKeys) { continuousElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
for (auto& f : factors) { for (auto &f : factors) {
if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) { if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
gfg.push_back(gf); gfg.push_back(gf);
} else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) { } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
@ -217,20 +224,20 @@ static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> continu
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> discreteElimination( static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
const HybridGaussianFactorGraph& factors, const Ordering& frontalKeys) { discreteElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto& f : factors) { for (auto &f : factors) {
if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) { if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
dfg.push_back(df); dfg.push_back(df);
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) { } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
// Case where we have a HybridGaussianFactor with no continuous keys. // Case where we have a HybridGaussianFactor with no continuous keys.
// In this case, compute discrete probabilities. // In this case, compute discrete probabilities.
// TODO(frank): What about the scalar!?
auto potential = [&](const auto& pair) -> double { auto potential = [&](const auto& pair) -> double {
auto [factor, scalar] = pair; auto [factor, scalar] = pair;
// If the factor is null, it has been pruned, hence return potential of zero // If factor is null, it has been pruned, hence return potential zero
if (!factor) return 0.0; if (!factor) return 0.0;
return exp(-scalar - factor->error(kEmpty)); return exp(-scalar - factor->error(kEmpty));
}; };
@ -262,9 +269,10 @@ static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> discret
* The residual error contains no keys, and only * The residual error contains no keys, and only
* depends on the discrete separator if present. * depends on the discrete separator if present.
*/ */
static std::shared_ptr<Factor> createDiscreteFactor(const ResultTree& eliminationResults, static std::shared_ptr<Factor> createDiscreteFactor(
const DiscreteKeys& discreteSeparator) { const ResultTree& eliminationResults,
auto potential = [&](const auto& pair) -> double { const DiscreteKeys &discreteSeparator) {
auto potential = [&](const auto &pair) -> double {
const auto& [conditional, factor] = pair.first; const auto& [conditional, factor] = pair.first;
const double scalar = pair.second; const double scalar = pair.second;
if (conditional && factor) { if (conditional && factor) {
@ -276,7 +284,8 @@ static std::shared_ptr<Factor> createDiscreteFactor(const ResultTree& eliminatio
const double error = scalar + factor->error(kEmpty) - negLogK; const double error = scalar + factor->error(kEmpty) - negLogK;
return exp(-error); return exp(-error);
} else if (!conditional && !factor) { } else if (!conditional && !factor) {
// If the factor is null, it has been pruned, hence return potential of zero // If the factor is null, it has been pruned, hence return potential of
// zero
return 0.0; return 0.0;
} else { } else {
throw std::runtime_error("createDiscreteFactor has mixed NULLs"); throw std::runtime_error("createDiscreteFactor has mixed NULLs");
@ -290,11 +299,12 @@ static std::shared_ptr<Factor> createDiscreteFactor(const ResultTree& eliminatio
/* *******************************************************************************/ /* *******************************************************************************/
// Create HybridGaussianFactor on the separator, taking care to correct // Create HybridGaussianFactor on the separator, taking care to correct
// for conditional constants. // for conditional constants.
static std::shared_ptr<Factor> createHybridGaussianFactor(const ResultTree& eliminationResults, static std::shared_ptr<Factor> createHybridGaussianFactor(
const DiscreteKeys& discreteSeparator) { const ResultTree &eliminationResults,
const DiscreteKeys &discreteSeparator) {
// Correct for the normalization constant used up by the conditional // Correct for the normalization constant used up by the conditional
auto correct = [&](const auto& pair) -> GaussianFactorValuePair { auto correct = [&](const auto &pair) -> GaussianFactorValuePair {
const auto& [conditional, factor] = pair.first; const auto &[conditional, factor] = pair.first;
const double scalar = pair.second; const double scalar = pair.second;
if (conditional && factor) { if (conditional && factor) {
const double negLogK = conditional->negLogConstant(); const double negLogK = conditional->negLogConstant();
@ -305,29 +315,32 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(const ResultTree& elim
throw std::runtime_error("createHybridGaussianFactors has mixed NULLs"); throw std::runtime_error("createHybridGaussianFactors has mixed NULLs");
} }
}; };
DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults, correct); DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
correct);
return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors); return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors);
} }
/* *******************************************************************************/ /* *******************************************************************************/
/// Get the discrete keys from the HybridGaussianFactorGraph as DiscreteKeys. /// Get the discrete keys from the HybridGaussianFactorGraph as DiscreteKeys.
static auto GetDiscreteKeys = [](const HybridGaussianFactorGraph& hfg) -> DiscreteKeys { static auto GetDiscreteKeys =
[](const HybridGaussianFactorGraph &hfg) -> DiscreteKeys {
const std::set<DiscreteKey> discreteKeySet = hfg.discreteKeys(); const std::set<DiscreteKey> discreteKeySet = hfg.discreteKeys();
return {discreteKeySet.begin(), discreteKeySet.end()}; return {discreteKeySet.begin(), discreteKeySet.end()};
}; };
/* *******************************************************************************/ /* *******************************************************************************/
std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
HybridGaussianFactorGraph::eliminate(const Ordering& keys) const { HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
// Since we eliminate all continuous variables first, // Since we eliminate all continuous variables first,
// the discrete separator will be *all* the discrete keys. // the discrete separator will be *all* the discrete keys.
DiscreteKeys discreteSeparator = GetDiscreteKeys(*this); DiscreteKeys discreteSeparator = GetDiscreteKeys(*this);
// Collect all the factors to create a set of Gaussian factor graphs in a // Collect all the factors to create a set of Gaussian factor graphs in a
// decision tree indexed by all discrete keys involved. Just like any hybrid factor, every // decision tree indexed by all discrete keys involved. Just like any hybrid
// assignment also has a scalar error, in this case the sum of all errors in the graph. This error // factor, every assignment also has a scalar error, in this case the sum of
// is assignment-specific and accounts for any difference in noise models used. // all errors in the graph. This error is assignment-specific and accounts for
// any difference in noise models used.
HybridGaussianProductFactor productFactor = collectProductFactor(); HybridGaussianProductFactor productFactor = collectProductFactor();
// Convert factor graphs with a nullptr to an empty factor graph. // Convert factor graphs with a nullptr to an empty factor graph.
@ -337,8 +350,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering& keys) const {
// This is the elimination method on the leaf nodes // This is the elimination method on the leaf nodes
bool someContinuousLeft = false; bool someContinuousLeft = false;
auto eliminate = auto eliminate = [&](const std::pair<GaussianFactorGraph, double>& pair)
[&](const std::pair<GaussianFactorGraph, double>& pair) -> std::pair<Result, double> { -> std::pair<Result, double> {
const auto& [graph, scalar] = pair; const auto& [graph, scalar] = pair;
if (graph.empty()) { if (graph.empty()) {
@ -346,7 +359,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering& keys) const {
} }
// Expensive elimination of product factor. // Expensive elimination of product factor.
auto result = EliminatePreferCholesky(graph, keys); /// <<<<<< MOST COMPUTE IS HERE auto result =
EliminatePreferCholesky(graph, keys); /// <<<<<< MOST COMPUTE IS HERE
// Record whether there any continuous variables left // Record whether there any continuous variables left
someContinuousLeft |= !result.second->empty(); someContinuousLeft |= !result.second->empty();
@ -361,15 +375,16 @@ HybridGaussianFactorGraph::eliminate(const Ordering& keys) const {
// If there are no more continuous parents we create a DiscreteFactor with the // If there are no more continuous parents we create a DiscreteFactor with the
// error for each discrete choice. Otherwise, create a HybridGaussianFactor // error for each discrete choice. Otherwise, create a HybridGaussianFactor
// on the separator, taking care to correct for conditional constants. // on the separator, taking care to correct for conditional constants.
auto newFactor = someContinuousLeft auto newFactor =
someContinuousLeft
? createHybridGaussianFactor(eliminationResults, discreteSeparator) ? createHybridGaussianFactor(eliminationResults, discreteSeparator)
: createDiscreteFactor(eliminationResults, discreteSeparator); : createDiscreteFactor(eliminationResults, discreteSeparator);
// Create the HybridGaussianConditional from the conditionals // Create the HybridGaussianConditional from the conditionals
HybridGaussianConditional::Conditionals conditionals( HybridGaussianConditional::Conditionals conditionals(
eliminationResults, [](const auto& pair) { return pair.first.first; }); eliminationResults, [](const auto& pair) { return pair.first.first; });
auto hybridGaussian = auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
std::make_shared<HybridGaussianConditional>(discreteSeparator, conditionals); discreteSeparator, conditionals);
return {std::make_shared<HybridConditional>(hybridGaussian), newFactor}; return {std::make_shared<HybridConditional>(hybridGaussian), newFactor};
} }
@ -389,7 +404,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering& keys) const {
* be INCORRECT and there will be NO error raised. * be INCORRECT and there will be NO error raised.
*/ */
std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> // std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> //
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys) { EliminateHybrid(const HybridGaussianFactorGraph &factors,
const Ordering &keys) {
// NOTE: Because we are in the Conditional Gaussian regime there are only // NOTE: Because we are in the Conditional Gaussian regime there are only
// a few cases: // a few cases:
// 1. continuous variable, make a hybrid Gaussian conditional if there are // 1. continuous variable, make a hybrid Gaussian conditional if there are
@ -440,7 +456,7 @@ EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys)
// 3. if not, we do hybrid elimination: // 3. if not, we do hybrid elimination:
bool only_discrete = true, only_continuous = true; bool only_discrete = true, only_continuous = true;
for (auto&& factor : factors) { for (auto &&factor : factors) {
if (auto hybrid_factor = std::dynamic_pointer_cast<HybridFactor>(factor)) { if (auto hybrid_factor = std::dynamic_pointer_cast<HybridFactor>(factor)) {
if (hybrid_factor->isDiscrete()) { if (hybrid_factor->isDiscrete()) {
only_continuous = false; only_continuous = false;
@ -451,9 +467,11 @@ EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys)
only_discrete = false; only_discrete = false;
break; break;
} }
} else if (auto cont_factor = std::dynamic_pointer_cast<GaussianFactor>(factor)) { } else if (auto cont_factor =
std::dynamic_pointer_cast<GaussianFactor>(factor)) {
only_discrete = false; only_discrete = false;
} else if (auto discrete_factor = std::dynamic_pointer_cast<DiscreteFactor>(factor)) { } else if (auto discrete_factor =
std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
only_continuous = false; only_continuous = false;
} }
} }
@ -474,17 +492,17 @@ EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys)
/* ************************************************************************ */ /* ************************************************************************ */
AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree( AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
const VectorValues& continuousValues) const { const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> result(0.0); AlgebraicDecisionTree<Key> result(0.0);
// Iterate over each factor. // Iterate over each factor.
for (auto& factor : factors_) { for (auto &factor : factors_) {
if (auto hf = std::dynamic_pointer_cast<HybridFactor>(factor)) { if (auto hf = std::dynamic_pointer_cast<HybridFactor>(factor)) {
// Add errorTree for hybrid factors, includes HybridGaussianConditionals! // Add errorTree for hybrid factors, includes HybridGaussianConditionals!
result = result + hf->errorTree(continuousValues); result = result + hf->errorTree(continuousValues);
} else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) { } else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
// If discrete, just add its errorTree as well // If discrete, just add its errorTree as well
result = result + df->errorTree(); result = result + df->errorTree();
} else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) { } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
// For a continuous only factor, just add its error // For a continuous only factor, just add its error
result = result + gf->error(continuousValues); result = result + gf->error(continuousValues);
} else { } else {
@ -495,7 +513,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
} }
/* ************************************************************************ */ /* ************************************************************************ */
double HybridGaussianFactorGraph::probPrime(const HybridValues& values) const { double HybridGaussianFactorGraph::probPrime(const HybridValues &values) const {
double error = this->error(values); double error = this->error(values);
// NOTE: The 0.5 term is handled by each factor // NOTE: The 0.5 term is handled by each factor
return std::exp(-error); return std::exp(-error);
@ -503,7 +521,7 @@ double HybridGaussianFactorGraph::probPrime(const HybridValues& values) const {
/* ************************************************************************ */ /* ************************************************************************ */
AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::discretePosterior( AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::discretePosterior(
const VectorValues& continuousValues) const { const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> errors = this->errorTree(continuousValues); AlgebraicDecisionTree<Key> errors = this->errorTree(continuousValues);
AlgebraicDecisionTree<Key> p = errors.apply([](double error) { AlgebraicDecisionTree<Key> p = errors.apply([](double error) {
// NOTE: The 0.5 term is handled by each factor // NOTE: The 0.5 term is handled by each factor
@ -513,18 +531,19 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::discretePosterior(
} }
/* ************************************************************************ */ /* ************************************************************************ */
GaussianFactorGraph HybridGaussianFactorGraph::choose(const DiscreteValues& assignment) const { GaussianFactorGraph HybridGaussianFactorGraph::choose(
const DiscreteValues &assignment) const {
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
for (auto&& f : *this) { for (auto &&f : *this) {
if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(f)) { if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(f)) {
gfg.push_back(gf); gfg.push_back(gf);
} else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) { } else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) {
gfg.push_back(gf); gfg.push_back(gf);
} else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) { } else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
gfg.push_back((*hgf)(assignment).first); gfg.push_back((*hgf)(assignment).first);
} else if (auto hgc = std::dynamic_pointer_cast<HybridGaussianConditional>(f)) { } else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
gfg.push_back((*hgc)(assignment)); gfg.push_back((*hgc)(assignment));
} else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(f)) { } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
if (auto gc = hc->asGaussian()) if (auto gc = hc->asGaussian())
gfg.push_back(gc); gfg.push_back(gc);
else if (auto hgc = hc->asHybrid()) else if (auto hgc = hc->asHybrid())

9
gtsam/hybrid/HybridGaussianProductFactor.cpp
View File

@ -59,7 +59,8 @@ HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
return *this; return *this;
} }
void HybridGaussianProductFactor::print(const std::string& s, const KeyFormatter& formatter) const { void HybridGaussianProductFactor::print(const std::string& s,
const KeyFormatter& formatter) const {
KeySet keys; KeySet keys;
auto printer = [&](const Y& y) { auto printer = [&](const Y& y) {
if (keys.empty()) keys = y.first.keys(); if (keys.empty()) keys = y.first.keys();
@ -77,8 +78,10 @@ void HybridGaussianProductFactor::print(const std::string& s, const KeyFormatter
HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const { HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const {
auto emptyGaussian = [](const Y& y) { auto emptyGaussian = [](const Y& y) {
bool hasNull = std::any_of( bool hasNull =
y.first.begin(), y.first.end(), [](const GaussianFactor::shared_ptr& ptr) { return !ptr; }); std::any_of(y.first.begin(),
y.first.end(),
[](const GaussianFactor::shared_ptr& ptr) { return !ptr; });
return hasNull ? Y{GaussianFactorGraph(), 0.0} : y; return hasNull ? Y{GaussianFactorGraph(), 0.0} : y;
}; };
return {Base(*this, emptyGaussian)}; return {Base(*this, emptyGaussian)};

30
gtsam/hybrid/HybridGaussianProductFactor.h
View File

@ -60,13 +60,16 @@ class HybridGaussianProductFactor
///@{ ///@{
/// Add GaussianFactor into HybridGaussianProductFactor /// Add GaussianFactor into HybridGaussianProductFactor
HybridGaussianProductFactor operator+(const GaussianFactor::shared_ptr& factor) const; HybridGaussianProductFactor operator+(
const GaussianFactor::shared_ptr& factor) const;
/// Add HybridGaussianFactor into HybridGaussianProductFactor /// Add HybridGaussianFactor into HybridGaussianProductFactor
HybridGaussianProductFactor operator+(const HybridGaussianFactor& factor) const; HybridGaussianProductFactor operator+(
const HybridGaussianFactor& factor) const;
/// Add-assign operator for GaussianFactor /// Add-assign operator for GaussianFactor
HybridGaussianProductFactor& operator+=(const GaussianFactor::shared_ptr& factor); HybridGaussianProductFactor& operator+=(
const GaussianFactor::shared_ptr& factor);
/// Add-assign operator for HybridGaussianFactor /// Add-assign operator for HybridGaussianFactor
HybridGaussianProductFactor& operator+=(const HybridGaussianFactor& factor); HybridGaussianProductFactor& operator+=(const HybridGaussianFactor& factor);
@ -81,7 +84,8 @@ class HybridGaussianProductFactor
* @param s Optional string to prepend * @param s Optional string to prepend
* @param formatter Optional key formatter * @param formatter Optional key formatter
*/ */
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** /**
* @brief Check if this HybridGaussianProductFactor is equal to another * @brief Check if this HybridGaussianProductFactor is equal to another
@ -89,9 +93,11 @@ class HybridGaussianProductFactor
* @param tol Tolerance for floating point comparisons * @param tol Tolerance for floating point comparisons
* @return true if equal, false otherwise * @return true if equal, false otherwise
*/ */
bool equals(const HybridGaussianProductFactor& other, double tol = 1e-9) const { bool equals(const HybridGaussianProductFactor& other,
double tol = 1e-9) const {
return Base::equals(other, [tol](const Y& a, const Y& b) { return Base::equals(other, [tol](const Y& a, const Y& b) {
return a.first.equals(b.first, tol) && std::abs(a.second - b.second) < tol; return a.first.equals(b.first, tol) &&
std::abs(a.second - b.second) < tol;
}); });
} }
@ -102,12 +108,13 @@ class HybridGaussianProductFactor
/** /**
* @brief Remove empty GaussianFactorGraphs from the decision tree * @brief Remove empty GaussianFactorGraphs from the decision tree
* @return A new HybridGaussianProductFactor with empty GaussianFactorGraphs removed * @return A new HybridGaussianProductFactor with empty GaussianFactorGraphs
* removed
* *
* If any GaussianFactorGraph in the decision tree contains a nullptr, convert * If any GaussianFactorGraph in the decision tree contains a nullptr, convert
* that leaf to an empty GaussianFactorGraph with zero scalar sum. This is needed because the * that leaf to an empty GaussianFactorGraph with zero scalar sum. This is
* DecisionTree will otherwise create a GaussianFactorGraph with a single (null) factor, which * needed because the DecisionTree will otherwise create a GaussianFactorGraph
* doesn't register as null. * with a single (null) factor, which doesn't register as null.
*/ */
HybridGaussianProductFactor removeEmpty() const; HybridGaussianProductFactor removeEmpty() const;
@ -116,6 +123,7 @@ class HybridGaussianProductFactor
// Testable traits // Testable traits
template <> template <>
struct traits<HybridGaussianProductFactor> : public Testable<HybridGaussianProductFactor> {}; struct traits<HybridGaussianProductFactor>
: public Testable<HybridGaussianProductFactor> {};
} // namespace gtsam } // namespace gtsam

6
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -181,19 +181,19 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
/* ************************************************************************* */ /* ************************************************************************* */
AlgebraicDecisionTree<Key> HybridNonlinearFactorGraph::errorTree( AlgebraicDecisionTree<Key> HybridNonlinearFactorGraph::errorTree(
const Values& continuousValues) const { const Values& values) const {
AlgebraicDecisionTree<Key> result(0.0); AlgebraicDecisionTree<Key> result(0.0);
// Iterate over each factor. // Iterate over each factor.
for (auto& factor : factors_) { for (auto& factor : factors_) {
if (auto hnf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) { if (auto hnf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
// Compute factor error and add it. // Compute factor error and add it.
result = result + hnf->errorTree(continuousValues); result = result + hnf->errorTree(values);
} else if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(factor)) { } else if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
// If continuous only, get the (double) error // If continuous only, get the (double) error
// and add it to every leaf of the result // and add it to every leaf of the result
result = result + nf->error(continuousValues); result = result + nf->error(values);
} else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) { } else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
// If discrete, just add its errorTree as well // If discrete, just add its errorTree as well

4
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -98,10 +98,10 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
* *
* @note: Gaussian and hybrid Gaussian factors are not considered! * @note: Gaussian and hybrid Gaussian factors are not considered!
* *
* @param continuousValues Manifold values at which to compute the error. * @param values Manifold values at which to compute the error.
* @return AlgebraicDecisionTree<Key> * @return AlgebraicDecisionTree<Key>
*/ */
AlgebraicDecisionTree<Key> errorTree(const Values& continuousValues) const; AlgebraicDecisionTree<Key> errorTree(const Values& values) const;
/** /**
* @brief Computer posterior P(M|X=x) when all continuous values X are given. * @brief Computer posterior P(M|X=x) when all continuous values X are given.

30
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -40,7 +40,8 @@ const DiscreteKey m(M(0), 2);
const DiscreteValues m1Assignment{{M(0), 1}}; const DiscreteValues m1Assignment{{M(0), 1}};
// Define a 50/50 prior on the mode // Define a 50/50 prior on the mode
DiscreteConditional::shared_ptr mixing = std::make_shared<DiscreteConditional>(m, "60/40"); DiscreteConditional::shared_ptr mixing =
std::make_shared<DiscreteConditional>(m, "60/40");
/// Gaussian density function /// Gaussian density function
double Gaussian(double mu, double sigma, double z) { double Gaussian(double mu, double sigma, double z) {
@ -52,7 +53,8 @@ double Gaussian(double mu, double sigma, double z) {
* If sigma0 == sigma1, it simplifies to a sigmoid function. * If sigma0 == sigma1, it simplifies to a sigmoid function.
* Hardcodes 60/40 prior on mode. * Hardcodes 60/40 prior on mode.
*/ */
double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, double z) { double prob_m_z(double mu0, double mu1, double sigma0, double sigma1,
double z) {
const double p0 = 0.6 * Gaussian(mu0, sigma0, z); const double p0 = 0.6 * Gaussian(mu0, sigma0, z);
const double p1 = 0.4 * Gaussian(mu1, sigma1, z); const double p1 = 0.4 * Gaussian(mu1, sigma1, z);
return p1 / (p0 + p1); return p1 / (p0 + p1);
@ -68,13 +70,15 @@ TEST(GaussianMixture, GaussianMixtureModel) {
// Create a Gaussian mixture model p(z|m) with same sigma. // Create a Gaussian mixture model p(z|m) with same sigma.
HybridBayesNet gmm; HybridBayesNet gmm;
std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma}, {Vector1(mu1), sigma}}; std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma},
{Vector1(mu1), sigma}};
gmm.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters); gmm.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
gmm.push_back(mixing); gmm.push_back(mixing);
// At the halfway point between the means, we should get P(m|z)=0.5 // At the halfway point between the means, we should get P(m|z)=0.5
double midway = mu1 - mu0; double midway = mu1 - mu0;
auto eliminationResult = gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential(); auto eliminationResult =
gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
auto pMid = *eliminationResult->at(0)->asDiscrete(); auto pMid = *eliminationResult->at(0)->asDiscrete();
EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid));
@ -84,7 +88,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); const double expected = prob_m_z(mu0, mu1, sigma, sigma, z);
// Workflow 1: convert HBN to HFG and solve // Workflow 1: convert HBN to HFG and solve
auto eliminationResult1 = gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential(); auto eliminationResult1 =
gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
auto posterior1 = *eliminationResult1->at(0)->asDiscrete(); auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
@ -109,7 +114,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
// Create a Gaussian mixture model p(z|m) with same sigma. // Create a Gaussian mixture model p(z|m) with same sigma.
HybridBayesNet gmm; HybridBayesNet gmm;
std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma0}, {Vector1(mu1), sigma1}}; std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma0},
{Vector1(mu1), sigma1}};
gmm.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters); gmm.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
gmm.push_back(mixing); gmm.push_back(mixing);
@ -119,15 +125,18 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
const VectorValues vv{{Z(0), Vector1(zMax)}}; const VectorValues vv{{Z(0), Vector1(zMax)}};
auto gfg = gmm.toFactorGraph(vv); auto gfg = gmm.toFactorGraph(vv);
// Equality of posteriors asserts that the elimination is correct (same ratios for all modes) // Equality of posteriors asserts that the elimination is correct (same ratios
// for all modes)
const auto& expectedDiscretePosterior = gmm.discretePosterior(vv); const auto& expectedDiscretePosterior = gmm.discretePosterior(vv);
EXPECT(assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv))); EXPECT(assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
// Eliminate the graph! // Eliminate the graph!
auto eliminationResultMax = gfg.eliminateSequential(); auto eliminationResultMax = gfg.eliminateSequential();
// Equality of posteriors asserts that the elimination is correct (same ratios for all modes) // Equality of posteriors asserts that the elimination is correct (same ratios
EXPECT(assert_equal(expectedDiscretePosterior, eliminationResultMax->discretePosterior(vv))); // for all modes)
EXPECT(assert_equal(expectedDiscretePosterior,
eliminationResultMax->discretePosterior(vv)));
auto pMax = *eliminationResultMax->at(0)->asDiscrete(); auto pMax = *eliminationResultMax->at(0)->asDiscrete();
EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
@ -138,7 +147,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z);
// Workflow 1: convert HBN to HFG and solve // Workflow 1: convert HBN to HFG and solve
auto eliminationResult1 = gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential(); auto eliminationResult1 =
gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
auto posterior1 = *eliminationResult1->at(0)->asDiscrete(); auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);

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

@ -168,8 +168,10 @@ TEST(HybridBayesNet, Tiny) {
EXPECT(!pruned.equals(bayesNet)); EXPECT(!pruned.equals(bayesNet));
// error // error
const double error0 = chosen0.error(vv) + gc0->negLogConstant() - px->negLogConstant() - log(0.4); const double error0 = chosen0.error(vv) + gc0->negLogConstant() -
const double error1 = chosen1.error(vv) + gc1->negLogConstant() - px->negLogConstant() - log(0.6); px->negLogConstant() - log(0.4);
const double error1 = chosen1.error(vv) + gc1->negLogConstant() -
px->negLogConstant() - log(0.6);
// print errors: // print errors:
EXPECT_DOUBLES_EQUAL(error0, bayesNet.error(zero), 1e-9); EXPECT_DOUBLES_EQUAL(error0, bayesNet.error(zero), 1e-9);
EXPECT_DOUBLES_EQUAL(error1, bayesNet.error(one), 1e-9); EXPECT_DOUBLES_EQUAL(error1, bayesNet.error(one), 1e-9);
@ -190,7 +192,6 @@ TEST(HybridBayesNet, Tiny) {
// toFactorGraph // toFactorGraph
auto fg = bayesNet.toFactorGraph({{Z(0), Vector1(5.0)}}); auto fg = bayesNet.toFactorGraph({{Z(0), Vector1(5.0)}});
EXPECT_LONGS_EQUAL(3, fg.size()); EXPECT_LONGS_EQUAL(3, fg.size());
GTSAM_PRINT(fg);
// Create the product factor for eliminating x0: // Create the product factor for eliminating x0:
HybridGaussianFactorGraph factors_x0; HybridGaussianFactorGraph factors_x0;
@ -204,9 +205,7 @@ TEST(HybridBayesNet, Tiny) {
// Call eliminate and check scalar: // Call eliminate and check scalar:
auto result = factors_x0.eliminate({X(0)}); auto result = factors_x0.eliminate({X(0)});
GTSAM_PRINT(*result.first);
auto df = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second); auto df = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
GTSAM_PRINT(df->errorTree());
// Check that the ratio of probPrime to evaluate is the same for all modes. // Check that the ratio of probPrime to evaluate is the same for all modes.
std::vector<double> ratio(2); std::vector<double> ratio(2);
@ -228,13 +227,17 @@ TEST(HybridBayesNet, Tiny) {
/* ****************************************************************************/ /* ****************************************************************************/
// Hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). // Hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia).
namespace different_sigmas { namespace different_sigmas {
const auto gc = GaussianConditional::sharedMeanAndStddev(X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0); const auto gc = GaussianConditional::sharedMeanAndStddev(X(0), 2 * I_1x1, X(1),
Vector1(-4.0), 5.0);
const std::vector<std::pair<Vector, double>> parms{{Vector1(5), 2.0}, {Vector1(2), 3.0}}; const std::vector<std::pair<Vector, double>> parms{{Vector1(5), 2.0},
{Vector1(2), 3.0}};
const auto hgc = std::make_shared<HybridGaussianConditional>(Asia, X(1), parms); const auto hgc = std::make_shared<HybridGaussianConditional>(Asia, X(1), parms);
const auto prior = std::make_shared<DiscreteConditional>(Asia, "99/1"); const auto prior = std::make_shared<DiscreteConditional>(Asia, "99/1");
auto wrap = [](const auto& c) { return std::make_shared<HybridConditional>(c); }; auto wrap = [](const auto& c) {
return std::make_shared<HybridConditional>(c);
};
const HybridBayesNet bayesNet{wrap(gc), wrap(hgc), wrap(prior)}; const HybridBayesNet bayesNet{wrap(gc), wrap(hgc), wrap(prior)};
// Create values at which to evaluate. // Create values at which to evaluate.
@ -248,8 +251,8 @@ TEST(HybridBayesNet, evaluateHybrid) {
const double conditionalProbability = gc->evaluate(values.continuous()); const double conditionalProbability = gc->evaluate(values.continuous());
const double mixtureProbability = hgc->evaluate(values); const double mixtureProbability = hgc->evaluate(values);
EXPECT_DOUBLES_EQUAL( EXPECT_DOUBLES_EQUAL(conditionalProbability * mixtureProbability * 0.99,
conditionalProbability * mixtureProbability * 0.99, bayesNet.evaluate(values), 1e-9); bayesNet.evaluate(values), 1e-9);
} }
/* ****************************************************************************/ /* ****************************************************************************/
@ -271,10 +274,14 @@ TEST(HybridBayesNet, Choose) {
EXPECT_LONGS_EQUAL(4, gbn.size()); EXPECT_LONGS_EQUAL(4, gbn.size());
EXPECT(assert_equal(*(*hybridBayesNet->at(0)->asHybrid())(assignment), *gbn.at(0))); EXPECT(assert_equal(*(*hybridBayesNet->at(0)->asHybrid())(assignment),
EXPECT(assert_equal(*(*hybridBayesNet->at(1)->asHybrid())(assignment), *gbn.at(1))); *gbn.at(0)));
EXPECT(assert_equal(*(*hybridBayesNet->at(2)->asHybrid())(assignment), *gbn.at(2))); EXPECT(assert_equal(*(*hybridBayesNet->at(1)->asHybrid())(assignment),
EXPECT(assert_equal(*(*hybridBayesNet->at(3)->asHybrid())(assignment), *gbn.at(3))); *gbn.at(1)));
EXPECT(assert_equal(*(*hybridBayesNet->at(2)->asHybrid())(assignment),
*gbn.at(2)));
EXPECT(assert_equal(*(*hybridBayesNet->at(3)->asHybrid())(assignment),
*gbn.at(3)));
} }
/* ****************************************************************************/ /* ****************************************************************************/
@ -311,7 +318,8 @@ TEST(HybridBayesNet, OptimizeAssignment) {
TEST(HybridBayesNet, Optimize) { TEST(HybridBayesNet, Optimize) {
Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1"); Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1");
HybridBayesNet::shared_ptr hybridBayesNet = s.linearizedFactorGraph.eliminateSequential(); HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential();
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
@ -338,7 +346,8 @@ TEST(HybridBayesNet, Pruning) {
// ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x0;z0) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1) // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x0;z0) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1)
Switching s(3); Switching s(3);
HybridBayesNet::shared_ptr posterior = s.linearizedFactorGraph.eliminateSequential(); HybridBayesNet::shared_ptr posterior =
s.linearizedFactorGraph.eliminateSequential();
EXPECT_LONGS_EQUAL(5, posterior->size()); EXPECT_LONGS_EQUAL(5, posterior->size());
// Optimize // Optimize
@ -364,9 +373,12 @@ TEST(HybridBayesNet, Pruning) {
logProbability += posterior->at(0)->asHybrid()->logProbability(hybridValues); logProbability += posterior->at(0)->asHybrid()->logProbability(hybridValues);
logProbability += posterior->at(1)->asHybrid()->logProbability(hybridValues); logProbability += posterior->at(1)->asHybrid()->logProbability(hybridValues);
logProbability += posterior->at(2)->asHybrid()->logProbability(hybridValues); logProbability += posterior->at(2)->asHybrid()->logProbability(hybridValues);
logProbability += posterior->at(3)->asDiscrete()->logProbability(hybridValues); logProbability +=
logProbability += posterior->at(4)->asDiscrete()->logProbability(hybridValues); posterior->at(3)->asDiscrete()->logProbability(hybridValues);
EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues), 1e-9); logProbability +=
posterior->at(4)->asDiscrete()->logProbability(hybridValues);
EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues),
1e-9);
// Check agreement with discrete posterior // Check agreement with discrete posterior
// double density = exp(logProbability); // double density = exp(logProbability);
@ -387,7 +399,8 @@ TEST(HybridBayesNet, Pruning) {
TEST(HybridBayesNet, Prune) { TEST(HybridBayesNet, Prune) {
Switching s(4); Switching s(4);
HybridBayesNet::shared_ptr posterior = s.linearizedFactorGraph.eliminateSequential(); HybridBayesNet::shared_ptr posterior =
s.linearizedFactorGraph.eliminateSequential();
EXPECT_LONGS_EQUAL(7, posterior->size()); EXPECT_LONGS_EQUAL(7, posterior->size());
HybridValues delta = posterior->optimize(); HybridValues delta = posterior->optimize();
@ -404,7 +417,8 @@ TEST(HybridBayesNet, Prune) {
TEST(HybridBayesNet, UpdateDiscreteConditionals) { TEST(HybridBayesNet, UpdateDiscreteConditionals) {
Switching s(4); Switching s(4);
HybridBayesNet::shared_ptr posterior = s.linearizedFactorGraph.eliminateSequential(); HybridBayesNet::shared_ptr posterior =
s.linearizedFactorGraph.eliminateSequential();
EXPECT_LONGS_EQUAL(7, posterior->size()); EXPECT_LONGS_EQUAL(7, posterior->size());
DiscreteConditional joint; DiscreteConditional joint;
@ -416,7 +430,8 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
auto prunedDecisionTree = joint.prune(maxNrLeaves); auto prunedDecisionTree = joint.prune(maxNrLeaves);
#ifdef GTSAM_DT_MERGING #ifdef GTSAM_DT_MERGING
EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/, prunedDecisionTree.nrLeaves()); EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/,
prunedDecisionTree.nrLeaves());
#else #else
EXPECT_LONGS_EQUAL(8 /*full tree*/, prunedDecisionTree.nrLeaves()); EXPECT_LONGS_EQUAL(8 /*full tree*/, prunedDecisionTree.nrLeaves());
#endif #endif
@ -424,14 +439,16 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
// regression // regression
// NOTE(Frank): I had to include *three* non-zeroes here now. // NOTE(Frank): I had to include *three* non-zeroes here now.
DecisionTreeFactor::ADT potentials( DecisionTreeFactor::ADT potentials(
s.modes, std::vector<double>{0, 0, 0, 0.28739288, 0, 0.43106901, 0, 0.2815381}); s.modes,
std::vector<double>{0, 0, 0, 0.28739288, 0, 0.43106901, 0, 0.2815381});
DiscreteConditional expectedConditional(3, s.modes, potentials); DiscreteConditional expectedConditional(3, s.modes, potentials);
// Prune! // Prune!
auto pruned = posterior->prune(maxNrLeaves); auto pruned = posterior->prune(maxNrLeaves);
// Functor to verify values against the expectedConditional // Functor to verify values against the expectedConditional
auto checker = [&](const Assignment<Key>& assignment, double probability) -> double { auto checker = [&](const Assignment<Key>& assignment,
double probability) -> double {
// typecast so we can use this to get probability value // typecast so we can use this to get probability value
DiscreteValues choices(assignment); DiscreteValues choices(assignment);
if (prunedDecisionTree(choices) == 0) { if (prunedDecisionTree(choices) == 0) {
@ -446,7 +463,8 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
CHECK(pruned.at(0)->asDiscrete()); CHECK(pruned.at(0)->asDiscrete());
auto pruned_discrete_conditionals = pruned.at(0)->asDiscrete(); auto pruned_discrete_conditionals = pruned.at(0)->asDiscrete();
auto discrete_conditional_tree = auto discrete_conditional_tree =
std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(pruned_discrete_conditionals); std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(
pruned_discrete_conditionals);
// The checker functor verifies the values for us. // The checker functor verifies the values for us.
discrete_conditional_tree->apply(checker); discrete_conditional_tree->apply(checker);
@ -460,10 +478,13 @@ TEST(HybridBayesNet, Sampling) {
auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0));
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
auto zero_motion = std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); auto zero_motion =
auto one_motion = std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>( nfg.emplace_shared<HybridNonlinearFactor>(
DiscreteKey(M(0), 2), std::vector<NoiseModelFactor::shared_ptr>{zero_motion, one_motion}); DiscreteKey(M(0), 2),
std::vector<NoiseModelFactor::shared_ptr>{zero_motion, one_motion});
DiscreteKey mode(M(0), 2); DiscreteKey mode(M(0), 2);
nfg.emplace_shared<DiscreteDistribution>(mode, "1/1"); nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");
@ -535,17 +556,18 @@ TEST(HybridBayesNet, ErrorTreeWithConditional) {
hbn.emplace_shared<GaussianConditional>(x0, Vector1(0.0), I_1x1, prior_model); hbn.emplace_shared<GaussianConditional>(x0, Vector1(0.0), I_1x1, prior_model);
// Add measurement P(z0 | x0) // Add measurement P(z0 | x0)
hbn.emplace_shared<GaussianConditional>(z0, Vector1(0.0), -I_1x1, x0, I_1x1, measurement_model); hbn.emplace_shared<GaussianConditional>(z0, Vector1(0.0), -I_1x1, x0, I_1x1,
measurement_model);
// Add hybrid motion model // Add hybrid motion model
double mu = 0.0; double mu = 0.0;
double sigma0 = 1e2, sigma1 = 1e-2; double sigma0 = 1e2, sigma1 = 1e-2;
auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
auto c0 = auto c0 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1,
make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1, x0, -I_1x1, model0), x0, -I_1x1, model0),
c1 = c1 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1,
make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1, x0, -I_1x1, model1); x0, -I_1x1, model1);
DiscreteKey m1(M(2), 2); DiscreteKey m1(M(2), 2);
hbn.emplace_shared<HybridGaussianConditional>(m1, std::vector{c0, c1}); hbn.emplace_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});

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

@ -142,9 +142,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg, void dotPrint(const HybridGaussianFactorGraph::shared_ptr& hfg,
const HybridBayesTree::shared_ptr &hbt, const HybridBayesTree::shared_ptr& hbt,
const Ordering &ordering) { const Ordering& ordering) {
DotWriter dw; DotWriter dw;
dw.positionHints['c'] = 2; dw.positionHints['c'] = 2;
dw.positionHints['x'] = 1; dw.positionHints['x'] = 1;
@ -179,13 +179,15 @@ TEST(HybridGaussianFactorGraph, Switching) {
std::vector<int> naturalX(N); std::vector<int> naturalX(N);
std::iota(naturalX.begin(), naturalX.end(), 1); std::iota(naturalX.begin(), naturalX.end(), 1);
std::vector<Key> ordX; std::vector<Key> ordX;
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX), std::transform(
[](int x) { return X(x); }); naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) {
return X(x);
});
auto [ndX, lvls] = makeBinaryOrdering(ordX); auto [ndX, lvls] = makeBinaryOrdering(ordX);
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
// TODO(dellaert): this has no effect! // TODO(dellaert): this has no effect!
for (auto &l : lvls) { for (auto& l : lvls) {
l = -l; l = -l;
} }
} }
@ -193,8 +195,10 @@ TEST(HybridGaussianFactorGraph, Switching) {
std::vector<int> naturalC(N - 1); std::vector<int> naturalC(N - 1);
std::iota(naturalC.begin(), naturalC.end(), 1); std::iota(naturalC.begin(), naturalC.end(), 1);
std::vector<Key> ordC; std::vector<Key> ordC;
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), std::transform(
[](int x) { return M(x); }); naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) {
return M(x);
});
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
const auto [ndC, lvls] = makeBinaryOrdering(ordC); const auto [ndC, lvls] = makeBinaryOrdering(ordC);
@ -233,13 +237,15 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
std::vector<int> naturalX(N); std::vector<int> naturalX(N);
std::iota(naturalX.begin(), naturalX.end(), 1); std::iota(naturalX.begin(), naturalX.end(), 1);
std::vector<Key> ordX; std::vector<Key> ordX;
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX), std::transform(
[](int x) { return X(x); }); naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) {
return X(x);
});
auto [ndX, lvls] = makeBinaryOrdering(ordX); auto [ndX, lvls] = makeBinaryOrdering(ordX);
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
// TODO(dellaert): this has no effect! // TODO(dellaert): this has no effect!
for (auto &l : lvls) { for (auto& l : lvls) {
l = -l; l = -l;
} }
} }
@ -247,8 +253,10 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
std::vector<int> naturalC(N - 1); std::vector<int> naturalC(N - 1);
std::iota(naturalC.begin(), naturalC.end(), 1); std::iota(naturalC.begin(), naturalC.end(), 1);
std::vector<Key> ordC; std::vector<Key> ordC;
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), std::transform(
[](int x) { return M(x); }); naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) {
return M(x);
});
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
const auto [ndC, lvls] = makeBinaryOrdering(ordC); const auto [ndC, lvls] = makeBinaryOrdering(ordC);
@ -408,8 +416,7 @@ TEST(HybridBayesTree, OptimizeAssignment) {
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t k = 0; k < s.K; k++) for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
ordering.push_back(X(k));
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering); s.linearizedFactorGraph.eliminatePartialSequential(ordering);
@ -451,21 +458,20 @@ TEST(HybridBayesTree, Optimize) {
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t k = 0; k < s.K; k++) for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
ordering.push_back(X(k));
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering); s.linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto &&f : *remainingFactorGraph) { for (auto&& f : *remainingFactorGraph) {
auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(f); auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(f);
assert(discreteFactor); assert(discreteFactor);
dfg.push_back(discreteFactor); dfg.push_back(discreteFactor);
} }
// Add the probabilities for each branch // Add the probabilities for each branch
DiscreteKeys discrete_keys = {m0, m1, m2}; DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}};
vector<double> probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656, vector<double> probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656,
0.037152205, 0.12248971, 0.07349729, 0.08}; 0.037152205, 0.12248971, 0.07349729, 0.08};
dfg.emplace_shared<DecisionTreeFactor>(discrete_keys, probs); dfg.emplace_shared<DecisionTreeFactor>(discrete_keys, probs);

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

@ -37,8 +37,6 @@
// Include for test suite // Include for test suite
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <bitset>
#include "Switching.h" #include "Switching.h"
using namespace std; using namespace std;

83
gtsam/hybrid/tests/testHybridGaussianConditional.cpp
View File

@ -51,8 +51,10 @@ namespace equal_constants {
// Create a simple HybridGaussianConditional // Create a simple HybridGaussianConditional
const double commonSigma = 2.0; const double commonSigma = 2.0;
const std::vector<GaussianConditional::shared_ptr> conditionals{ const std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma)}; commonSigma),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
commonSigma)};
const HybridGaussianConditional hybrid_conditional(mode, conditionals); const HybridGaussianConditional hybrid_conditional(mode, conditionals);
} // namespace equal_constants } // namespace equal_constants
@ -77,8 +79,8 @@ TEST(HybridGaussianConditional, LogProbability) {
using namespace equal_constants; using namespace equal_constants;
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL( EXPECT_DOUBLES_EQUAL(conditionals[mode]->logProbability(vv),
conditionals[mode]->logProbability(vv), hybrid_conditional.logProbability(hv), 1e-8); hybrid_conditional.logProbability(hv), 1e-8);
} }
} }
@ -90,7 +92,8 @@ TEST(HybridGaussianConditional, Error) {
// Check result. // Check result.
DiscreteKeys discrete_keys{mode}; DiscreteKeys discrete_keys{mode};
std::vector<double> leaves = {conditionals[0]->error(vv), conditionals[1]->error(vv)}; std::vector<double> leaves = {conditionals[0]->error(vv),
conditionals[1]->error(vv)};
AlgebraicDecisionTree<Key> expected(discrete_keys, leaves); AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
@ -98,7 +101,8 @@ TEST(HybridGaussianConditional, Error) {
// Check for non-tree version. // Check for non-tree version.
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv), hybrid_conditional.error(hv), 1e-8); EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv),
hybrid_conditional.error(hv), 1e-8);
} }
} }
@ -113,8 +117,10 @@ TEST(HybridGaussianConditional, Likelihood) {
// Check that the hybrid conditional error and the likelihood error are the // Check that the hybrid conditional error and the likelihood error are the
// same. // same.
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0), 1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0),
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1), 1e-8); 1e-8);
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
1e-8);
// Check that likelihood error is as expected, i.e., just the errors of the // Check that likelihood error is as expected, i.e., just the errors of the
// individual likelihoods, in the `equal_constants` case. // individual likelihoods, in the `equal_constants` case.
@ -128,7 +134,8 @@ TEST(HybridGaussianConditional, Likelihood) {
std::vector<double> ratio(2); std::vector<double> ratio(2);
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
ratio[mode] = std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv); ratio[mode] =
std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
} }
EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
} }
@ -138,8 +145,10 @@ namespace mode_dependent_constants {
// Create a HybridGaussianConditional with mode-dependent noise models. // Create a HybridGaussianConditional with mode-dependent noise models.
// 0 is low-noise, 1 is high-noise. // 0 is low-noise, 1 is high-noise.
const std::vector<GaussianConditional::shared_ptr> conditionals{ const std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 0.5), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 3.0)}; 0.5),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
3.0)};
const HybridGaussianConditional hybrid_conditional(mode, conditionals); const HybridGaussianConditional hybrid_conditional(mode, conditionals);
} // namespace mode_dependent_constants } // namespace mode_dependent_constants
@ -171,7 +180,8 @@ TEST(HybridGaussianConditional, Error2) {
// Expected error is e(X) + log(sqrt(|2πΣ|)). // Expected error is e(X) + log(sqrt(|2πΣ|)).
// We normalize log(sqrt(|2πΣ|)) with min(negLogConstant) // We normalize log(sqrt(|2πΣ|)) with min(negLogConstant)
// so it is non-negative. // so it is non-negative.
std::vector<double> leaves = {conditionals[0]->error(vv) + negLogConstant0 - minErrorConstant, std::vector<double> leaves = {
conditionals[0]->error(vv) + negLogConstant0 - minErrorConstant,
conditionals[1]->error(vv) + negLogConstant1 - minErrorConstant}; conditionals[1]->error(vv) + negLogConstant1 - minErrorConstant};
AlgebraicDecisionTree<Key> expected(discrete_keys, leaves); AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
@ -180,10 +190,10 @@ TEST(HybridGaussianConditional, Error2) {
// Check for non-tree version. // Check for non-tree version.
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL( EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv) +
conditionals[mode]->error(vv) + conditionals[mode]->negLogConstant() - minErrorConstant, conditionals[mode]->negLogConstant() -
hybrid_conditional.error(hv), minErrorConstant,
1e-8); hybrid_conditional.error(hv), 1e-8);
} }
} }
@ -198,8 +208,10 @@ TEST(HybridGaussianConditional, Likelihood2) {
// Check that the hybrid conditional error and the likelihood error are as // Check that the hybrid conditional error and the likelihood error are as
// expected, this invariant is the same as the equal noise case: // expected, this invariant is the same as the equal noise case:
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0), 1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0),
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1), 1e-8); 1e-8);
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
1e-8);
// Check the detailed JacobianFactor calculation for mode==1. // Check the detailed JacobianFactor calculation for mode==1.
{ {
@ -208,10 +220,11 @@ TEST(HybridGaussianConditional, Likelihood2) {
const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1); const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1);
CHECK(jf1); CHECK(jf1);
// Check that the JacobianFactor error with constants is equal to the conditional error: // Check that the JacobianFactor error with constants is equal to the
EXPECT_DOUBLES_EQUAL( // conditional error:
hybrid_conditional.error(hv1), EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1),
jf1->error(hv1) + conditionals[1]->negLogConstant() - hybrid_conditional.negLogConstant(), jf1->error(hv1) + conditionals[1]->negLogConstant() -
hybrid_conditional.negLogConstant(),
1e-8); 1e-8);
} }
@ -219,7 +232,8 @@ TEST(HybridGaussianConditional, Likelihood2) {
std::vector<double> ratio(2); std::vector<double> ratio(2);
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
ratio[mode] = std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv); ratio[mode] =
std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
} }
EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
} }
@ -232,7 +246,8 @@ TEST(HybridGaussianConditional, Prune) {
DiscreteKeys modes{{M(1), 2}, {M(2), 2}}; DiscreteKeys modes{{M(1), 2}, {M(2), 2}};
std::vector<GaussianConditional::shared_ptr> gcs; std::vector<GaussianConditional::shared_ptr> gcs;
for (size_t i = 0; i < 4; i++) { for (size_t i = 0; i < 4; i++) {
gcs.push_back(GaussianConditional::sharedMeanAndStddev(Z(0), Vector1(i + 1), i + 1)); gcs.push_back(
GaussianConditional::sharedMeanAndStddev(Z(0), Vector1(i + 1), i + 1));
} }
auto empty = std::make_shared<GaussianConditional>(); auto empty = std::make_shared<GaussianConditional>();
HybridGaussianConditional::Conditionals conditionals(modes, gcs); HybridGaussianConditional::Conditionals conditionals(modes, gcs);
@ -252,14 +267,8 @@ TEST(HybridGaussianConditional, Prune) {
} }
} }
{ {
const std::vector<double> potentials{0, const std::vector<double> potentials{0, 0, 0.5, 0, //
0, 0, 0, 0.5, 0};
0.5,
0, //
0,
0,
0.5,
0};
const DecisionTreeFactor decisionTreeFactor(keys, potentials); const DecisionTreeFactor decisionTreeFactor(keys, potentials);
const auto pruned = hgc.prune(decisionTreeFactor); const auto pruned = hgc.prune(decisionTreeFactor);
@ -268,14 +277,8 @@ TEST(HybridGaussianConditional, Prune) {
EXPECT_LONGS_EQUAL(2, pruned->nrComponents()); EXPECT_LONGS_EQUAL(2, pruned->nrComponents());
} }
{ {
const std::vector<double> potentials{0.2, const std::vector<double> potentials{0.2, 0, 0.3, 0, //
0, 0, 0, 0.5, 0};
0.3,
0, //
0,
0,
0.5,
0};
const DecisionTreeFactor decisionTreeFactor(keys, potentials); const DecisionTreeFactor decisionTreeFactor(keys, potentials);
const auto pruned = hgc.prune(decisionTreeFactor); const auto pruned = hgc.prune(decisionTreeFactor);

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

@ -46,7 +46,6 @@
#include "Switching.h" #include "Switching.h"
#include "TinyHybridExample.h" #include "TinyHybridExample.h"
#include "gtsam/linear/GaussianFactorGraph.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -74,7 +73,8 @@ TEST(HybridGaussianFactorGraph, Creation) {
HybridGaussianConditional gm( HybridGaussianConditional gm(
m0, m0,
{std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3), {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)}); std::make_shared<GaussianConditional>(
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)});
hfg.add(gm); hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size()); EXPECT_LONGS_EQUAL(2, hfg.size());
@ -118,7 +118,8 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second); auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
CHECK(factor); CHECK(factor);
// regression test // regression test
EXPECT(assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5)); EXPECT(
assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -180,7 +181,8 @@ TEST(HybridBayesNet, Switching) {
EXPECT_LONGS_EQUAL(4, graph.size()); EXPECT_LONGS_EQUAL(4, graph.size());
// Create some continuous and discrete values // Create some continuous and discrete values
const VectorValues continuousValues{{X(0), Vector1(0.1)}, {X(1), Vector1(1.2)}}; const VectorValues continuousValues{{X(0), Vector1(0.1)},
{X(1), Vector1(1.2)}};
const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}}; const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}};
// Get the hybrid gaussian factor and check it is as expected // Get the hybrid gaussian factor and check it is as expected
@ -213,7 +215,8 @@ TEST(HybridBayesNet, Switching) {
// Check errorTree // Check errorTree
AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues); AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues);
// Create expected error tree // Create expected error tree
const AlgebraicDecisionTree<Key> expectedErrors(M(0), expectedError0, expectedError1); const AlgebraicDecisionTree<Key> expectedErrors(
M(0), expectedError0, expectedError1);
// Check that the actual error tree matches the expected one // Check that the actual error tree matches the expected one
EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5)); EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5));
@ -226,9 +229,11 @@ TEST(HybridBayesNet, Switching) {
EXPECT_DOUBLES_EQUAL(std::exp(-expectedError1), probPrime1, 1e-5); EXPECT_DOUBLES_EQUAL(std::exp(-expectedError1), probPrime1, 1e-5);
// Check discretePosterior // Check discretePosterior
const AlgebraicDecisionTree<Key> graphPosterior = graph.discretePosterior(continuousValues); const AlgebraicDecisionTree<Key> graphPosterior =
graph.discretePosterior(continuousValues);
const double sum = probPrime0 + probPrime1; const double sum = probPrime0 + probPrime1;
const AlgebraicDecisionTree<Key> expectedPosterior(M(0), probPrime0 / sum, probPrime1 / sum); const AlgebraicDecisionTree<Key> expectedPosterior(
M(0), probPrime0 / sum, probPrime1 / sum);
EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5)); EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5));
// Make the clique of factors connected to x0: // Make the clique of factors connected to x0:
@ -267,29 +272,37 @@ TEST(HybridBayesNet, Switching) {
EXPECT(std::dynamic_pointer_cast<HybridGaussianFactor>(factor)); EXPECT(std::dynamic_pointer_cast<HybridGaussianFactor>(factor));
auto phi_x1_m = std::dynamic_pointer_cast<HybridGaussianFactor>(factor); auto phi_x1_m = std::dynamic_pointer_cast<HybridGaussianFactor>(factor);
EXPECT_LONGS_EQUAL(2, phi_x1_m->keys().size()); // x1, m0 EXPECT_LONGS_EQUAL(2, phi_x1_m->keys().size()); // x1, m0
// Check that the scalars incorporate the negative log constant of the conditional // Check that the scalars incorporate the negative log constant of the
EXPECT_DOUBLES_EQUAL( // conditional
scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(), (*phi_x1_m)(modeZero).second, 1e-9); EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(),
EXPECT_DOUBLES_EQUAL( (*phi_x1_m)(modeZero).second,
scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(), (*phi_x1_m)(modeOne).second, 1e-9); 1e-9);
EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(),
(*phi_x1_m)(modeOne).second,
1e-9);
// Check that the conditional and remaining factor are consistent for both modes // Check that the conditional and remaining factor are consistent for both
// modes
for (auto&& mode : {modeZero, modeOne}) { for (auto&& mode : {modeZero, modeOne}) {
const auto gc = (*p_x0_given_x1_m)(mode); const auto gc = (*p_x0_given_x1_m)(mode);
const auto [gf, scalar] = (*phi_x1_m)(mode); const auto [gf, scalar] = (*phi_x1_m)(mode);
// The error of the original factors should equal the sum of errors of the conditional and // The error of the original factors should equal the sum of errors of the
// remaining factor, modulo the normalization constant of the conditional. // conditional and remaining factor, modulo the normalization constant of
// the conditional.
double originalError = factors_x0.error({continuousValues, mode}); double originalError = factors_x0.error({continuousValues, mode});
const double actualError = const double actualError = gc->negLogConstant() +
gc->negLogConstant() + gc->error(continuousValues) + gf->error(continuousValues) + scalar; gc->error(continuousValues) +
gf->error(continuousValues) + scalar;
EXPECT_DOUBLES_EQUAL(originalError, actualError, 1e-9); EXPECT_DOUBLES_EQUAL(originalError, actualError, 1e-9);
} }
// Create a clique for x1 // Create a clique for x1
HybridGaussianFactorGraph factors_x1; HybridGaussianFactorGraph factors_x1;
factors_x1.push_back(factor); // Use the remaining factor from previous elimination factors_x1.push_back(
factors_x1.push_back(graph.at(2)); // Add the factor for x1 from the original graph factor); // Use the remaining factor from previous elimination
factors_x1.push_back(
graph.at(2)); // Add the factor for x1 from the original graph
// Test collectProductFactor for x1 clique // Test collectProductFactor for x1 clique
auto productFactor_x1 = factors_x1.collectProductFactor(); auto productFactor_x1 = factors_x1.collectProductFactor();
@ -323,16 +336,18 @@ TEST(HybridBayesNet, Switching) {
auto phi_x1 = std::dynamic_pointer_cast<DecisionTreeFactor>(factor_x1); auto phi_x1 = std::dynamic_pointer_cast<DecisionTreeFactor>(factor_x1);
CHECK(phi_x1); CHECK(phi_x1);
EXPECT_LONGS_EQUAL(1, phi_x1->keys().size()); // m0 EXPECT_LONGS_EQUAL(1, phi_x1->keys().size()); // m0
// We can't really check the error of the decision tree factor phi_x1, because the continuos // We can't really check the error of the decision tree factor phi_x1, because
// factor whose error(kEmpty) we need is not available.. // the continuos factor whose error(kEmpty) we need is not available..
// However, we can still check the total error for the clique factors_x1 and the elimination // However, we can still check the total error for the clique factors_x1 and
// results are equal, modulo -again- the negative log constant of the conditional. // the elimination results are equal, modulo -again- the negative log constant
// of the conditional.
for (auto&& mode : {modeZero, modeOne}) { for (auto&& mode : {modeZero, modeOne}) {
auto gc_x1 = (*p_x1_given_m)(mode); auto gc_x1 = (*p_x1_given_m)(mode);
double originalError_x1 = factors_x1.error({continuousValues, mode}); double originalError_x1 = factors_x1.error({continuousValues, mode});
const double actualError = const double actualError = gc_x1->negLogConstant() +
gc_x1->negLogConstant() + gc_x1->error(continuousValues) + phi_x1->error(mode); gc_x1->error(continuousValues) +
phi_x1->error(mode);
EXPECT_DOUBLES_EQUAL(originalError_x1, actualError, 1e-9); EXPECT_DOUBLES_EQUAL(originalError_x1, actualError, 1e-9);
} }
@ -340,9 +355,11 @@ TEST(HybridBayesNet, Switching) {
auto hybridBayesNet = graph.eliminateSequential(); auto hybridBayesNet = graph.eliminateSequential();
CHECK(hybridBayesNet); CHECK(hybridBayesNet);
// Check that the posterior P(M|X=continuousValues) from the Bayes net is the same as the // Check that the posterior P(M|X=continuousValues) from the Bayes net is the
// same posterior from the graph. This is a sanity check that the elimination is done correctly. // same as the same posterior from the graph. This is a sanity check that the
AlgebraicDecisionTree<Key> bnPosterior = hybridBayesNet->discretePosterior(continuousValues); // elimination is done correctly.
AlgebraicDecisionTree<Key> bnPosterior =
hybridBayesNet->discretePosterior(continuousValues);
EXPECT(assert_equal(graphPosterior, bnPosterior)); EXPECT(assert_equal(graphPosterior, bnPosterior));
} }
@ -367,9 +384,11 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
// Check that the probability prime is the exponential of the error // Check that the probability prime is the exponential of the error
EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7)); EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7));
// Check that the posterior P(M|X=continuousValues) from the Bayes net is the same as the // Check that the posterior P(M|X=continuousValues) from the Bayes net is the
// same posterior from the graph. This is a sanity check that the elimination is done correctly. // same as the same posterior from the graph. This is a sanity check that the
const AlgebraicDecisionTree<Key> graphPosterior = graph.discretePosterior(delta.continuous()); // elimination is done correctly.
const AlgebraicDecisionTree<Key> graphPosterior =
graph.discretePosterior(delta.continuous());
const AlgebraicDecisionTree<Key> bnPosterior = const AlgebraicDecisionTree<Key> bnPosterior =
hybridBayesNet->discretePosterior(delta.continuous()); hybridBayesNet->discretePosterior(delta.continuous());
EXPECT(assert_equal(graphPosterior, bnPosterior)); EXPECT(assert_equal(graphPosterior, bnPosterior));
@ -491,7 +510,8 @@ TEST(HybridGaussianFactorGraph, Conditionals) {
expected_continuous.insert<double>(X(1), 1); expected_continuous.insert<double>(X(1), 1);
expected_continuous.insert<double>(X(2), 2); expected_continuous.insert<double>(X(2), 2);
expected_continuous.insert<double>(X(3), 4); expected_continuous.insert<double>(X(3), 4);
Values result_continuous = switching.linearizationPoint.retract(result.continuous()); Values result_continuous =
switching.linearizationPoint.retract(result.continuous());
EXPECT(assert_equal(expected_continuous, result_continuous)); EXPECT(assert_equal(expected_continuous, result_continuous));
DiscreteValues expected_discrete; DiscreteValues expected_discrete;
@ -519,8 +539,10 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
// Check discrete posterior at optimum // Check discrete posterior at optimum
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
AlgebraicDecisionTree<Key> graphPosterior = graph.discretePosterior(delta.continuous()); AlgebraicDecisionTree<Key> graphPosterior =
AlgebraicDecisionTree<Key> bnPosterior = hybridBayesNet->discretePosterior(delta.continuous()); graph.discretePosterior(delta.continuous());
AlgebraicDecisionTree<Key> bnPosterior =
hybridBayesNet->discretePosterior(delta.continuous());
EXPECT(assert_equal(graphPosterior, bnPosterior)); EXPECT(assert_equal(graphPosterior, bnPosterior));
graph = HybridGaussianFactorGraph(); graph = HybridGaussianFactorGraph();
@ -579,11 +601,9 @@ TEST(HybridGaussianFactorGraph, collectProductFactor) {
/* ****************************************************************************/ /* ****************************************************************************/
// Check that the factor graph unnormalized probability is proportional to the // Check that the factor graph unnormalized probability is proportional to the
// Bayes net probability for the given measurements. // Bayes net probability for the given measurements.
bool ratioTest(const HybridBayesNet& bn, bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
const VectorValues& measurements, const HybridGaussianFactorGraph &fg, size_t num_samples = 100) {
const HybridGaussianFactorGraph& fg, auto compute_ratio = [&](HybridValues *sample) -> double {
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); return bn.evaluate(*sample) / fg.probPrime(*sample);
}; };
@ -602,11 +622,9 @@ bool ratioTest(const HybridBayesNet& bn,
/* ****************************************************************************/ /* ****************************************************************************/
// Check that the bayes net unnormalized probability is proportional to the // Check that the bayes net unnormalized probability is proportional to the
// Bayes net probability for the given measurements. // Bayes net probability for the given measurements.
bool ratioTest(const HybridBayesNet& bn, bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
const VectorValues& measurements, const HybridBayesNet &posterior, size_t num_samples = 100) {
const HybridBayesNet& posterior, auto compute_ratio = [&](HybridValues *sample) -> double {
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); return bn.evaluate(*sample) / posterior.evaluate(*sample);
}; };
@ -640,10 +658,10 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
// Create hybrid Gaussian factor on X(0). // Create hybrid Gaussian factor on X(0).
using tiny::mode; using tiny::mode;
// regression, but mean checked to be 5.0 in both cases: // regression, but mean checked to be 5.0 in both cases:
const auto conditional0 = const auto conditional0 = std::make_shared<GaussianConditional>(
std::make_shared<GaussianConditional>(X(0), Vector1(14.1421), I_1x1 * 2.82843), X(0), Vector1(14.1421), I_1x1 * 2.82843),
conditional1 = conditional1 = std::make_shared<GaussianConditional>(
std::make_shared<GaussianConditional>(X(0), Vector1(10.1379), I_1x1 * 2.02759); X(0), Vector1(10.1379), I_1x1 * 2.02759);
expectedBayesNet.emplace_shared<HybridGaussianConditional>( expectedBayesNet.emplace_shared<HybridGaussianConditional>(
mode, std::vector{conditional0, conditional1}); mode, std::vector{conditional0, conditional1});
@ -671,7 +689,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
bn.emplace_shared<HybridGaussianConditional>(m1, Z(0), I_1x1, X(0), parms); bn.emplace_shared<HybridGaussianConditional>(m1, Z(0), I_1x1, X(0), parms);
// Create prior on X(0). // Create prior on X(0).
bn.push_back(GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5)); bn.push_back(
GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5));
// Add prior on m1. // Add prior on m1.
bn.emplace_shared<DiscreteConditional>(m1, "1/1"); bn.emplace_shared<DiscreteConditional>(m1, "1/1");
@ -689,10 +708,10 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
// Create hybrid Gaussian factor on X(0). // Create hybrid Gaussian factor on X(0).
// regression, but mean checked to be 5.0 in both cases: // regression, but mean checked to be 5.0 in both cases:
const auto conditional0 = const auto conditional0 = std::make_shared<GaussianConditional>(
std::make_shared<GaussianConditional>(X(0), Vector1(10.1379), I_1x1 * 2.02759), X(0), Vector1(10.1379), I_1x1 * 2.02759),
conditional1 = conditional1 = std::make_shared<GaussianConditional>(
std::make_shared<GaussianConditional>(X(0), Vector1(14.1421), I_1x1 * 2.82843); X(0), Vector1(14.1421), I_1x1 * 2.82843);
expectedBayesNet.emplace_shared<HybridGaussianConditional>( expectedBayesNet.emplace_shared<HybridGaussianConditional>(
m1, std::vector{conditional0, conditional1}); m1, std::vector{conditional0, conditional1});
@ -725,10 +744,10 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
// Create hybrid Gaussian factor on X(0). // Create hybrid Gaussian factor on X(0).
using tiny::mode; using tiny::mode;
// regression, but mean checked to be 5.0 in both cases: // regression, but mean checked to be 5.0 in both cases:
const auto conditional0 = const auto conditional0 = std::make_shared<GaussianConditional>(
std::make_shared<GaussianConditional>(X(0), Vector1(17.3205), I_1x1 * 3.4641), X(0), Vector1(17.3205), I_1x1 * 3.4641),
conditional1 = conditional1 = std::make_shared<GaussianConditional>(
std::make_shared<GaussianConditional>(X(0), Vector1(10.274), I_1x1 * 2.0548); X(0), Vector1(10.274), I_1x1 * 2.0548);
expectedBayesNet.emplace_shared<HybridGaussianConditional>( expectedBayesNet.emplace_shared<HybridGaussianConditional>(
mode, std::vector{conditional0, conditional1}); mode, std::vector{conditional0, conditional1});
@ -772,25 +791,27 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
// NOTE: we add reverse topological so we can sample from the Bayes net.: // NOTE: we add reverse topological so we can sample from the Bayes net.:
// Add measurements: // Add measurements:
std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3}, {Z_1x1, 0.5}}; std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3},
{Z_1x1, 0.5}};
for (size_t t : {0, 1, 2}) { for (size_t t : {0, 1, 2}) {
// Create hybrid Gaussian factor on Z(t) conditioned on X(t) and mode N(t): // Create hybrid Gaussian factor on Z(t) conditioned on X(t) and mode N(t):
const auto noise_mode_t = DiscreteKey{N(t), 2}; const auto noise_mode_t = DiscreteKey{N(t), 2};
bn.emplace_shared<HybridGaussianConditional>( bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, Z(t), I_1x1,
noise_mode_t, Z(t), I_1x1, X(t), measurementModels); X(t), measurementModels);
// Create prior on discrete mode N(t): // Create prior on discrete mode N(t):
bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80"); bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
} }
// Add motion models. TODO(frank): why are they exactly the same? // Add motion models. TODO(frank): why are they exactly the same?
std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2}, {Z_1x1, 0.2}}; std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2},
{Z_1x1, 0.2}};
for (size_t t : {2, 1}) { for (size_t t : {2, 1}) {
// Create hybrid Gaussian factor on X(t) conditioned on X(t-1) // Create hybrid Gaussian factor on X(t) conditioned on X(t-1)
// and mode M(t-1): // and mode M(t-1):
const auto motion_model_t = DiscreteKey{M(t), 2}; const auto motion_model_t = DiscreteKey{M(t), 2};
bn.emplace_shared<HybridGaussianConditional>( bn.emplace_shared<HybridGaussianConditional>(motion_model_t, X(t), I_1x1,
motion_model_t, X(t), I_1x1, X(t - 1), motionModels); X(t - 1), motionModels);
// Create prior on motion model M(t): // Create prior on motion model M(t):
bn.emplace_shared<DiscreteConditional>(motion_model_t, "40/60"); bn.emplace_shared<DiscreteConditional>(motion_model_t, "40/60");
@ -803,7 +824,8 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
EXPECT_LONGS_EQUAL(6, bn.sample().continuous().size()); EXPECT_LONGS_EQUAL(6, bn.sample().continuous().size());
// Create measurements consistent with moving right every time: // Create measurements consistent with moving right every time:
const VectorValues measurements{{Z(0), Vector1(0.0)}, {Z(1), Vector1(1.0)}, {Z(2), Vector1(2.0)}}; const VectorValues measurements{
{Z(0), Vector1(0.0)}, {Z(1), Vector1(1.0)}, {Z(2), Vector1(2.0)}};
const HybridGaussianFactorGraph fg = bn.toFactorGraph(measurements); const HybridGaussianFactorGraph fg = bn.toFactorGraph(measurements);
// Factor graph is: // Factor graph is:

7
gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp
View File

@ -54,12 +54,15 @@ const auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
const HybridGaussianFactor hybridFactorB(m2, {{f20, 20}, {f21, 21}, {f22, 22}}); const HybridGaussianFactor hybridFactorB(m2, {{f20, 20}, {f21, 21}, {f22, 22}});
// Simulate a pruned hybrid factor, in this case m2==1 is nulled out. // Simulate a pruned hybrid factor, in this case m2==1 is nulled out.
const HybridGaussianFactor prunedFactorB(m2, {{f20, 20}, {nullptr, 1000}, {f22, 22}}); const HybridGaussianFactor prunedFactorB(
m2, {{f20, 20}, {nullptr, 1000}, {f22, 22}});
} // namespace examples } // namespace examples
/* ************************************************************************* */ /* ************************************************************************* */
// Constructor // Constructor
TEST(HybridGaussianProductFactor, Construct) { HybridGaussianProductFactor product; } TEST(HybridGaussianProductFactor, Construct) {
HybridGaussianProductFactor product;
}
/* ************************************************************************* */ /* ************************************************************************* */
// Add two Gaussian factors and check only one leaf in tree // Add two Gaussian factors and check only one leaf in tree

61
gtsam/hybrid/tests/testHybridMotionModel.cpp
View File

@ -46,24 +46,24 @@ using symbol_shorthand::Z;
DiscreteKey m1(M(1), 2); DiscreteKey m1(M(1), 2);
void addMeasurement(HybridBayesNet& hbn, Key z_key, Key x_key, double sigma) { void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) {
auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma); auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma);
hbn.emplace_shared<GaussianConditional>( hbn.emplace_shared<GaussianConditional>(z_key, Vector1(0.0), I_1x1, x_key,
z_key, Vector1(0.0), I_1x1, x_key, -I_1x1, measurement_model); -I_1x1, measurement_model);
} }
/// Create hybrid motion model p(x1 | x0, m1) /// Create hybrid motion model p(x1 | x0, m1)
static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(double mu0, static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
double mu1, double mu0, double mu1, double sigma0, double sigma1) {
double sigma0,
double sigma1) {
std::vector<std::pair<Vector, double>> motionModels{{Vector1(mu0), sigma0}, std::vector<std::pair<Vector, double>> motionModels{{Vector1(mu0), sigma0},
{Vector1(mu1), sigma1}}; {Vector1(mu1), sigma1}};
return std::make_shared<HybridGaussianConditional>(m1, X(1), I_1x1, X(0), motionModels); return std::make_shared<HybridGaussianConditional>(m1, X(1), I_1x1, X(0),
motionModels);
} }
/// Create two state Bayes network with 1 or two measurement models /// Create two state Bayes network with 1 or two measurement models
HybridBayesNet CreateBayesNet(const HybridGaussianConditional::shared_ptr& hybridMotionModel, HybridBayesNet CreateBayesNet(
const HybridGaussianConditional::shared_ptr &hybridMotionModel,
bool add_second_measurement = false) { bool add_second_measurement = false) {
HybridBayesNet hbn; HybridBayesNet hbn;
@ -86,10 +86,9 @@ HybridBayesNet CreateBayesNet(const HybridGaussianConditional::shared_ptr& hybri
/// Approximate the discrete marginal P(m1) using importance sampling /// Approximate the discrete marginal P(m1) using importance sampling
std::pair<double, double> approximateDiscreteMarginal( std::pair<double, double> approximateDiscreteMarginal(
const HybridBayesNet& hbn, const HybridBayesNet &hbn,
const HybridGaussianConditional::shared_ptr& hybridMotionModel, const HybridGaussianConditional::shared_ptr &hybridMotionModel,
const VectorValues& given, const VectorValues &given, size_t N = 100000) {
size_t N = 100000) {
/// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1), /// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1),
/// using q(x0) = N(z0, sigmaQ) to sample x0. /// using q(x0) = N(z0, sigmaQ) to sample x0.
HybridBayesNet q; HybridBayesNet q;
@ -195,16 +194,20 @@ TEST(HybridGaussianFactor, TwoStateModel2) {
HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential(); HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential();
for (VectorValues vv : {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, for (VectorValues vv :
{VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
vv.insert(given); // add measurements for HBN vv.insert(given); // add measurements for HBN
const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
// Equality of posteriors asserts that the factor graph is correct (same ratios for all modes) // Equality of posteriors asserts that the factor graph is correct (same
EXPECT(assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv))); // ratios for all modes)
EXPECT(
assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
// This one asserts that HBN resulting from elimination is correct. // This one asserts that HBN resulting from elimination is correct.
EXPECT(assert_equal(expectedDiscretePosterior, eliminated->discretePosterior(vv))); EXPECT(assert_equal(expectedDiscretePosterior,
eliminated->discretePosterior(vv)));
} }
// Importance sampling run with 100k samples gives 50.095/49.905 // Importance sampling run with 100k samples gives 50.095/49.905
@ -227,16 +230,20 @@ TEST(HybridGaussianFactor, TwoStateModel2) {
// Check that ratio of Bayes net and factor graph for different modes is // Check that ratio of Bayes net and factor graph for different modes is
// equal for several values of {x0,x1}. // equal for several values of {x0,x1}.
for (VectorValues vv : {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, for (VectorValues vv :
{VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
vv.insert(given); // add measurements for HBN vv.insert(given); // add measurements for HBN
const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
// Equality of posteriors asserts that the factor graph is correct (same ratios for all modes) // Equality of posteriors asserts that the factor graph is correct (same
EXPECT(assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv))); // ratios for all modes)
EXPECT(
assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
// This one asserts that HBN resulting from elimination is correct. // This one asserts that HBN resulting from elimination is correct.
EXPECT(assert_equal(expectedDiscretePosterior, eliminated->discretePosterior(vv))); EXPECT(assert_equal(expectedDiscretePosterior,
eliminated->discretePosterior(vv)));
} }
// Values taken from an importance sampling run with 100k samples: // Values taken from an importance sampling run with 100k samples:
@ -290,11 +297,13 @@ TEST(HybridGaussianFactor, TwoStateModel3) {
// Check that ratio of Bayes net and factor graph for different modes is // Check that ratio of Bayes net and factor graph for different modes is
// equal for several values of {x0,x1}. // equal for several values of {x0,x1}.
for (VectorValues vv : {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, for (VectorValues vv :
{VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
vv.insert(given); // add measurements for HBN vv.insert(given); // add measurements for HBN
HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), gfg.error(hv1) / hbn.error(hv1), 1e-9); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
gfg.error(hv1) / hbn.error(hv1), 1e-9);
} }
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
@ -318,11 +327,13 @@ TEST(HybridGaussianFactor, TwoStateModel3) {
// Check that ratio of Bayes net and factor graph for different modes is // Check that ratio of Bayes net and factor graph for different modes is
// equal for several values of {x0,x1}. // equal for several values of {x0,x1}.
for (VectorValues vv : {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, for (VectorValues vv :
{VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
vv.insert(given); // add measurements for HBN vv.insert(given); // add measurements for HBN
HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), gfg.error(hv1) / hbn.error(hv1), 1e-9); EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
gfg.error(hv1) / hbn.error(hv1), 1e-9);
} }
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();

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

@ -34,7 +34,6 @@
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include "Switching.h" #include "Switching.h"
#include "Test.h"
// Include for test suite // Include for test suite
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>