Frank Dellaert
GitHub
commit
59f97d64eb
|
|
@ -22,14 +22,13 @@
|
|||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/inference/BayesTree-inst.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "gtsam/hybrid/HybridConditional.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file HybridConditional.cpp
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file HybridConditional.h
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ struct HybridGaussianConditional::Helper {
|
|||
explicit Helper(const Conditionals &conditionals)
|
||||
: 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 (!nrFrontals) nrFrontals = gc->nrFrontals();
|
||||
double value = gc->negLogConstant();
|
||||
|
|
@ -97,10 +97,10 @@ struct HybridGaussianConditional::Helper {
|
|||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianConditional::HybridGaussianConditional(
|
||||
const DiscreteKeys& discreteParents, const Helper& helper)
|
||||
const DiscreteKeys &discreteParents, const Helper &helper)
|
||||
: BaseFactor(discreteParents,
|
||||
FactorValuePairs(helper.pairs,
|
||||
[&](const GaussianFactorValuePair&
|
||||
[&](const GaussianFactorValuePair &
|
||||
pair) { // subtract minNegLogConstant
|
||||
return GaussianFactorValuePair{
|
||||
pair.first,
|
||||
|
|
@ -183,10 +183,12 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf,
|
|||
|
||||
// Check the base and the factors:
|
||||
return BaseFactor::equals(*e, tol) &&
|
||||
conditionals_.equals(
|
||||
e->conditionals_, [tol](const auto &f1, const auto &f2) {
|
||||
return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
|
||||
});
|
||||
conditionals_.equals(e->conditionals_,
|
||||
[tol](const GaussianConditional::shared_ptr &f1,
|
||||
const GaussianConditional::shared_ptr &f2) {
|
||||
return (!f1 && !f2) ||
|
||||
(f1 && f2 && f1->equals(*f2, tol));
|
||||
});
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
|
|
@ -225,7 +227,7 @@ KeyVector HybridGaussianConditional::continuousParents() const {
|
|||
// remove that key from continuousParentKeys:
|
||||
continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
|
||||
continuousParentKeys.end(), key),
|
||||
continuousParentKeys.end());
|
||||
continuousParentKeys.end());
|
||||
}
|
||||
return continuousParentKeys;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,9 +24,9 @@
|
|||
#include <gtsam/discrete/DecisionTree.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ struct HybridGaussianFactor::ConstructorHelper {
|
|||
// Build the FactorValuePairs DecisionTree
|
||||
pairs = FactorValuePairs(
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors),
|
||||
[](const auto& f) {
|
||||
[](const sharedFactor& f) {
|
||||
return std::pair{f,
|
||||
f ? 0.0 : std::numeric_limits<double>::infinity()};
|
||||
});
|
||||
|
|
@ -63,7 +63,7 @@ struct HybridGaussianFactor::ConstructorHelper {
|
|||
const std::vector<GaussianFactorValuePair>& factorPairs)
|
||||
: discreteKeys({discreteKey}) {
|
||||
// Extract continuous keys from the first non-null factor
|
||||
for (const auto& pair : factorPairs) {
|
||||
for (const GaussianFactorValuePair& pair : factorPairs) {
|
||||
if (pair.first && continuousKeys.empty()) {
|
||||
continuousKeys = pair.first->keys();
|
||||
break;
|
||||
|
|
@ -93,27 +93,27 @@ struct HybridGaussianFactor::ConstructorHelper {
|
|||
};
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
|
||||
HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper& helper)
|
||||
: Base(helper.continuousKeys, helper.discreteKeys),
|
||||
factors_(helper.pairs) {}
|
||||
|
||||
HybridGaussianFactor::HybridGaussianFactor(
|
||||
const DiscreteKey &discreteKey,
|
||||
const std::vector<GaussianFactor::shared_ptr> &factors)
|
||||
const DiscreteKey& discreteKey,
|
||||
const std::vector<GaussianFactor::shared_ptr>& factors)
|
||||
: HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
|
||||
|
||||
HybridGaussianFactor::HybridGaussianFactor(
|
||||
const DiscreteKey &discreteKey,
|
||||
const std::vector<GaussianFactorValuePair> &factorPairs)
|
||||
const DiscreteKey& discreteKey,
|
||||
const std::vector<GaussianFactorValuePair>& factorPairs)
|
||||
: HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
|
||||
|
||||
HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
|
||||
const FactorValuePairs &factors)
|
||||
HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys& discreteKeys,
|
||||
const FactorValuePairs& factors)
|
||||
: HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
|
||||
|
||||
/* *******************************************************************************/
|
||||
bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
|
||||
const This *e = dynamic_cast<const This *>(&lf);
|
||||
bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&lf);
|
||||
if (e == nullptr) return false;
|
||||
|
||||
// This will return false if either factors_ is empty or e->factors_ is
|
||||
|
|
@ -121,7 +121,8 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
|
|||
if (factors_.empty() ^ e->factors_.empty()) return false;
|
||||
|
||||
// Check the base and the factors:
|
||||
auto compareFunc = [tol](const auto& pair1, const auto& pair2) {
|
||||
auto compareFunc = [tol](const GaussianFactorValuePair& pair1,
|
||||
const GaussianFactorValuePair& pair2) {
|
||||
auto f1 = pair1.first, f2 = pair2.first;
|
||||
bool match = (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
|
||||
return match && gtsam::equal(pair1.second, pair2.second, tol);
|
||||
|
|
@ -130,8 +131,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");
|
||||
HybridFactor::print("", formatter);
|
||||
std::cout << "{\n";
|
||||
|
|
@ -139,9 +140,8 @@ void HybridGaussianFactor::print(const std::string &s,
|
|||
std::cout << " empty" << std::endl;
|
||||
} else {
|
||||
factors_.print(
|
||||
"",
|
||||
[&](Key k) { return formatter(k); },
|
||||
[&](const auto& pair) -> std::string {
|
||||
"", [&](Key k) { return formatter(k); },
|
||||
[&](const GaussianFactorValuePair& pair) -> std::string {
|
||||
RedirectCout rd;
|
||||
std::cout << ":\n";
|
||||
if (pair.first) {
|
||||
|
|
@ -158,7 +158,7 @@ void HybridGaussianFactor::print(const std::string &s,
|
|||
|
||||
/* *******************************************************************************/
|
||||
GaussianFactorValuePair HybridGaussianFactor::operator()(
|
||||
const DiscreteValues &assignment) const {
|
||||
const DiscreteValues& assignment) const {
|
||||
return factors_(assignment);
|
||||
}
|
||||
|
||||
|
|
@ -169,18 +169,25 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
|
|||
// - Each leaf converted to a GaussianFactorGraph with just the factor and its
|
||||
// scalar.
|
||||
return {{factors_,
|
||||
[](const auto& pair) -> std::pair<GaussianFactorGraph, double> {
|
||||
[](const GaussianFactorValuePair& pair)
|
||||
-> std::pair<GaussianFactorGraph, double> {
|
||||
return {GaussianFactorGraph{pair.first}, pair.second};
|
||||
}}};
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
inline static double PotentiallyPrunedComponentError(
|
||||
const GaussianFactorValuePair& pair, const VectorValues& continuousValues) {
|
||||
return pair.first ? pair.first->error(continuousValues) + pair.second
|
||||
: std::numeric_limits<double>::infinity();
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
|
||||
const VectorValues &continuousValues) const {
|
||||
const VectorValues& continuousValues) const {
|
||||
// functor to convert from sharedFactor to double error value.
|
||||
auto errorFunc = [&continuousValues](const auto& pair) {
|
||||
return pair.first ? pair.first->error(continuousValues) + pair.second
|
||||
: std::numeric_limits<double>::infinity();
|
||||
auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) {
|
||||
return PotentiallyPrunedComponentError(pair, continuousValues);
|
||||
};
|
||||
DecisionTree<Key, double> error_tree(factors_, errorFunc);
|
||||
return error_tree;
|
||||
|
|
@ -189,9 +196,8 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
|
|||
/* *******************************************************************************/
|
||||
double HybridGaussianFactor::error(const HybridValues& values) const {
|
||||
// Directly index to get the component, no need to build the whole tree.
|
||||
const auto pair = factors_(values.discrete());
|
||||
return pair.first ? pair.first->error(values.continuous()) + pair.second
|
||||
: std::numeric_limits<double>::infinity();
|
||||
const GaussianFactorValuePair pair = factors_(values.discrete());
|
||||
return PotentiallyPrunedComponentError(pair, values.continuous());
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@ using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
|
|||
* @ingroup hybrid
|
||||
*/
|
||||
class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
||||
public:
|
||||
public:
|
||||
using Base = HybridFactor;
|
||||
using This = HybridGaussianFactor;
|
||||
using shared_ptr = std::shared_ptr<This>;
|
||||
|
|
@ -68,11 +68,11 @@ public:
|
|||
/// typedef for Decision Tree of Gaussian factors and arbitrary value.
|
||||
using FactorValuePairs = DecisionTree<Key, GaussianFactorValuePair>;
|
||||
|
||||
private:
|
||||
private:
|
||||
/// Decision tree of Gaussian factors indexed by discrete keys.
|
||||
FactorValuePairs factors_;
|
||||
|
||||
public:
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -120,9 +120,8 @@ public:
|
|||
|
||||
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
|
||||
|
||||
void
|
||||
print(const std::string &s = "",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
void print(const std::string &s = "", const KeyFormatter &formatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// @}
|
||||
/// @name Standard API
|
||||
|
|
@ -138,8 +137,8 @@ public:
|
|||
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
|
||||
* as the factors involved, and leaf values as the error.
|
||||
*/
|
||||
AlgebraicDecisionTree<Key>
|
||||
errorTree(const VectorValues &continuousValues) const override;
|
||||
AlgebraicDecisionTree<Key> errorTree(
|
||||
const VectorValues &continuousValues) const override;
|
||||
|
||||
/**
|
||||
* @brief Compute the log-likelihood, including the log-normalizing constant.
|
||||
|
|
@ -159,7 +158,7 @@ public:
|
|||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
private:
|
||||
/**
|
||||
* @brief Helper function to augment the [A|b] matrices in the factor
|
||||
* components with the additional scalar values. This is done by storing the
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteJunctionTree.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridEliminationTree.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
|
|
@ -39,7 +40,6 @@
|
|||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include "gtsam/discrete/DiscreteValues.h"
|
||||
|
||||
#include <cstddef>
|
||||
#include <iostream>
|
||||
|
|
@ -57,15 +57,16 @@ using std::dynamic_pointer_cast;
|
|||
using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
|
||||
using Result =
|
||||
std::pair<std::shared_ptr<GaussianConditional>, GaussianFactor::shared_ptr>;
|
||||
using ResultTree = DecisionTree<Key, std::pair<Result, double>>;
|
||||
using ResultValuePair = std::pair<Result, double>;
|
||||
using ResultTree = DecisionTree<Key, ResultValuePair>;
|
||||
|
||||
static const VectorValues kEmpty;
|
||||
|
||||
/* ************************************************************************ */
|
||||
// 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) {
|
||||
auto& fr = *f;
|
||||
static void throwRuntimeError(const std::string &s,
|
||||
const std::shared_ptr<Factor> &f) {
|
||||
auto &fr = *f;
|
||||
throw std::runtime_error(s + " not implemented for factor type " +
|
||||
demangle(typeid(fr).name()) + ".");
|
||||
}
|
||||
|
|
@ -83,11 +84,12 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
|
|||
const DiscreteValues &assignment,
|
||||
const KeyFormatter &keyFormatter) {
|
||||
if (auto hgf = dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
|
||||
if (assignment.empty())
|
||||
if (assignment.empty()) {
|
||||
hgf->print("HybridGaussianFactor:", keyFormatter);
|
||||
else
|
||||
} else {
|
||||
hgf->operator()(assignment)
|
||||
.first->print("HybridGaussianFactor, component:", keyFormatter);
|
||||
}
|
||||
} else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
|
||||
factor->print("GaussianFactor:\n", keyFormatter);
|
||||
|
||||
|
|
@ -99,12 +101,13 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
|
|||
} else if (hc->isDiscrete()) {
|
||||
factor->print("DiscreteConditional:\n", keyFormatter);
|
||||
} else {
|
||||
if (assignment.empty())
|
||||
if (assignment.empty()) {
|
||||
hc->print("HybridConditional:", keyFormatter);
|
||||
else
|
||||
} else {
|
||||
hc->asHybrid()
|
||||
->choose(assignment)
|
||||
->print("HybridConditional, component:\n", keyFormatter);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
factor->print("Unknown factor type\n", keyFormatter);
|
||||
|
|
@ -112,13 +115,13 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
|
|||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void HybridGaussianFactorGraph::print(const std::string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
void HybridGaussianFactorGraph::print(const std::string &s,
|
||||
const KeyFormatter &keyFormatter) const {
|
||||
std::cout << (s.empty() ? "" : s + " ") << std::endl;
|
||||
std::cout << "size: " << size() << std::endl;
|
||||
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
auto&& factor = factors_[i];
|
||||
auto &&factor = factors_[i];
|
||||
if (factor == nullptr) {
|
||||
std::cout << "Factor " << i << ": nullptr\n";
|
||||
continue;
|
||||
|
|
@ -163,7 +166,7 @@ HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor()
|
|||
const {
|
||||
HybridGaussianProductFactor result;
|
||||
|
||||
for (auto& f : factors_) {
|
||||
for (auto &f : factors_) {
|
||||
// TODO(dellaert): can we make this cleaner and less error-prone?
|
||||
if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
|
||||
continue; // Ignore OrphanWrapper
|
||||
|
|
@ -235,7 +238,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
|
|||
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
// Case where we have a HybridGaussianFactor with no continuous keys.
|
||||
// In this case, compute discrete probabilities.
|
||||
auto potential = [&](const auto& pair) -> double {
|
||||
auto potential = [&](const auto &pair) -> double {
|
||||
auto [factor, scalar] = pair;
|
||||
// If factor is null, it has been pruned, hence return potential zero
|
||||
if (!factor) return 0.0;
|
||||
|
|
@ -270,10 +273,10 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
|
|||
* depends on the discrete separator if present.
|
||||
*/
|
||||
static std::shared_ptr<Factor> createDiscreteFactor(
|
||||
const ResultTree& eliminationResults,
|
||||
const ResultTree &eliminationResults,
|
||||
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;
|
||||
if (conditional && factor) {
|
||||
// `error` has the following contributions:
|
||||
|
|
@ -303,7 +306,7 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
|
|||
const ResultTree &eliminationResults,
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
// Correct for the normalization constant used up by the conditional
|
||||
auto correct = [&](const auto &pair) -> GaussianFactorValuePair {
|
||||
auto correct = [&](const ResultValuePair &pair) -> GaussianFactorValuePair {
|
||||
const auto &[conditional, factor] = pair.first;
|
||||
const double scalar = pair.second;
|
||||
if (conditional && factor) {
|
||||
|
|
@ -350,9 +353,9 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
|
|||
|
||||
// This is the elimination method on the leaf nodes
|
||||
bool someContinuousLeft = false;
|
||||
auto eliminate = [&](const std::pair<GaussianFactorGraph, double>& pair)
|
||||
auto eliminate = [&](const std::pair<GaussianFactorGraph, double> &pair)
|
||||
-> std::pair<Result, double> {
|
||||
const auto& [graph, scalar] = pair;
|
||||
const auto &[graph, scalar] = pair;
|
||||
|
||||
if (graph.empty()) {
|
||||
return {{nullptr, nullptr}, 0.0};
|
||||
|
|
@ -382,7 +385,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
|
|||
|
||||
// Create the HybridGaussianConditional from the conditionals
|
||||
HybridGaussianConditional::Conditionals conditionals(
|
||||
eliminationResults, [](const auto& pair) { return pair.first.first; });
|
||||
eliminationResults,
|
||||
[](const ResultValuePair &pair) { return pair.first.first; });
|
||||
auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
|
||||
discreteSeparator, conditionals);
|
||||
|
||||
|
|
|
|||
|
|
@ -145,9 +145,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
|
|||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
void
|
||||
print(const std::string &s = "HybridGaussianFactorGraph",
|
||||
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
|
||||
void print(
|
||||
const std::string& s = "HybridGaussianFactorGraph",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/**
|
||||
* @brief Print the errors of each factor in the hybrid factor graph.
|
||||
|
|
|
|||
|
|
@ -22,43 +22,52 @@
|
|||
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using Y = HybridGaussianProductFactor::Y;
|
||||
using Y = GaussianFactorGraphValuePair;
|
||||
|
||||
/* *******************************************************************************/
|
||||
static Y add(const Y& y1, const Y& y2) {
|
||||
GaussianFactorGraph result = y1.first;
|
||||
result.push_back(y2.first);
|
||||
return {result, y1.second + y2.second};
|
||||
};
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a,
|
||||
const HybridGaussianProductFactor& b) {
|
||||
return a.empty() ? b : HybridGaussianProductFactor(a.apply(b, add));
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianProductFactor HybridGaussianProductFactor::operator+(
|
||||
const HybridGaussianFactor& factor) const {
|
||||
return *this + factor.asProductFactor();
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianProductFactor HybridGaussianProductFactor::operator+(
|
||||
const GaussianFactor::shared_ptr& factor) const {
|
||||
return *this + HybridGaussianProductFactor(factor);
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
|
||||
const GaussianFactor::shared_ptr& factor) {
|
||||
*this = *this + factor;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
|
||||
const HybridGaussianFactor& factor) {
|
||||
*this = *this + factor;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
void HybridGaussianProductFactor::print(const std::string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
KeySet keys;
|
||||
|
|
@ -69,18 +78,25 @@ void HybridGaussianProductFactor::print(const std::string& s,
|
|||
};
|
||||
Base::print(s, formatter, printer);
|
||||
if (!keys.empty()) {
|
||||
std::stringstream ss;
|
||||
ss << s << " Keys:";
|
||||
for (auto&& key : keys) ss << " " << formatter(key);
|
||||
std::cout << ss.str() << "." << std::endl;
|
||||
std::cout << s << " Keys:";
|
||||
for (auto&& key : keys) std::cout << " " << formatter(key);
|
||||
std::cout << "." << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
bool HybridGaussianProductFactor::equals(
|
||||
const HybridGaussianProductFactor& other, double tol) const {
|
||||
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;
|
||||
});
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const {
|
||||
auto emptyGaussian = [](const Y& y) {
|
||||
bool hasNull =
|
||||
std::any_of(y.first.begin(),
|
||||
y.first.end(),
|
||||
std::any_of(y.first.begin(), y.first.end(),
|
||||
[](const GaussianFactor::shared_ptr& ptr) { return !ptr; });
|
||||
return hasNull ? Y{GaussianFactorGraph(), 0.0} : y;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -26,12 +26,13 @@ namespace gtsam {
|
|||
|
||||
class HybridGaussianFactor;
|
||||
|
||||
using GaussianFactorGraphValuePair = std::pair<GaussianFactorGraph, double>;
|
||||
|
||||
/// Alias for DecisionTree of GaussianFactorGraphs and their scalar sums
|
||||
class HybridGaussianProductFactor
|
||||
: public DecisionTree<Key, std::pair<GaussianFactorGraph, double>> {
|
||||
class GTSAM_EXPORT HybridGaussianProductFactor
|
||||
: public DecisionTree<Key, GaussianFactorGraphValuePair> {
|
||||
public:
|
||||
using Y = std::pair<GaussianFactorGraph, double>;
|
||||
using Base = DecisionTree<Key, Y>;
|
||||
using Base = DecisionTree<Key, GaussianFactorGraphValuePair>;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
|
@ -46,7 +47,7 @@ class HybridGaussianProductFactor
|
|||
*/
|
||||
template <class FACTOR>
|
||||
HybridGaussianProductFactor(const std::shared_ptr<FACTOR>& factor)
|
||||
: Base(Y{GaussianFactorGraph{factor}, 0.0}) {}
|
||||
: Base(GaussianFactorGraphValuePair{GaussianFactorGraph{factor}, 0.0}) {}
|
||||
|
||||
/**
|
||||
* @brief Construct from DecisionTree
|
||||
|
|
@ -94,12 +95,7 @@ class HybridGaussianProductFactor
|
|||
* @return true if equal, false otherwise
|
||||
*/
|
||||
bool equals(const HybridGaussianProductFactor& other,
|
||||
double tol = 1e-9) const {
|
||||
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;
|
||||
});
|
||||
}
|
||||
double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@ TEST(HybridBayesNet, Tiny) {
|
|||
factors_x0.push_back(fg.at(1));
|
||||
auto productFactor = factors_x0.collectProductFactor();
|
||||
|
||||
// Check that scalars are 0 and 1.79
|
||||
// Check that scalars are 0 and 1.79 (regression)
|
||||
EXPECT_DOUBLES_EQUAL(0.0, productFactor({{M(0), 0}}).second, 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(1.791759, productFactor({{M(0), 1}}).second, 1e-5);
|
||||
|
||||
|
|
|
|||
|
|
@ -22,6 +22,8 @@
|
|||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/inference/DotWriter.h>
|
||||
|
||||
#include <numeric>
|
||||
|
||||
#include "Switching.h"
|
||||
|
||||
// Include for test suite
|
||||
|
|
@ -62,7 +64,8 @@ std::vector<GaussianFactor::shared_ptr> components(Key key) {
|
|||
} // namespace two
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
|
||||
TEST(HybridGaussianFactorGraph,
|
||||
HybridGaussianFactorGraphEliminateFullMultifrontalSimple) {
|
||||
HybridGaussianFactorGraph hfg;
|
||||
|
||||
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
|
||||
|
|
@ -179,10 +182,8 @@ TEST(HybridGaussianFactorGraph, Switching) {
|
|||
std::vector<int> naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
std::vector<Key> ordX;
|
||||
std::transform(
|
||||
naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) {
|
||||
return X(x);
|
||||
});
|
||||
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
|
||||
[](int x) { return X(x); });
|
||||
|
||||
auto [ndX, lvls] = makeBinaryOrdering(ordX);
|
||||
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
|
||||
|
|
@ -195,10 +196,8 @@ TEST(HybridGaussianFactorGraph, Switching) {
|
|||
std::vector<int> naturalC(N - 1);
|
||||
std::iota(naturalC.begin(), naturalC.end(), 1);
|
||||
std::vector<Key> ordC;
|
||||
std::transform(
|
||||
naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) {
|
||||
return M(x);
|
||||
});
|
||||
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
|
||||
[](int x) { return M(x); });
|
||||
|
||||
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
|
||||
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
|
||||
|
|
@ -237,10 +236,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
|
|||
std::vector<int> naturalX(N);
|
||||
std::iota(naturalX.begin(), naturalX.end(), 1);
|
||||
std::vector<Key> ordX;
|
||||
std::transform(
|
||||
naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) {
|
||||
return X(x);
|
||||
});
|
||||
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
|
||||
[](int x) { return X(x); });
|
||||
|
||||
auto [ndX, lvls] = makeBinaryOrdering(ordX);
|
||||
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
|
||||
|
|
@ -253,10 +250,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
|
|||
std::vector<int> naturalC(N - 1);
|
||||
std::iota(naturalC.begin(), naturalC.end(), 1);
|
||||
std::vector<Key> ordC;
|
||||
std::transform(
|
||||
naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) {
|
||||
return M(x);
|
||||
});
|
||||
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
|
||||
[](int x) { return M(x); });
|
||||
|
||||
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
|
||||
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
|
@ -37,9 +39,6 @@
|
|||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
#include <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
|
@ -73,8 +72,8 @@ TEST(HybridGaussianFactorGraph, Creation) {
|
|||
HybridGaussianConditional gm(
|
||||
m0,
|
||||
{std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
|
||||
std::make_shared<GaussianConditional>(
|
||||
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)});
|
||||
std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
|
||||
I_3x3)});
|
||||
hfg.add(gm);
|
||||
|
||||
EXPECT_LONGS_EQUAL(2, hfg.size());
|
||||
|
|
@ -118,8 +117,7 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
|
|||
auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
|
||||
CHECK(factor);
|
||||
// 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -177,7 +175,7 @@ TEST(HybridBayesNet, Switching) {
|
|||
Switching s(2, betweenSigma, priorSigma);
|
||||
|
||||
// Check size of linearized factor graph
|
||||
const HybridGaussianFactorGraph& graph = s.linearizedFactorGraph;
|
||||
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
|
||||
EXPECT_LONGS_EQUAL(4, graph.size());
|
||||
|
||||
// Create some continuous and discrete values
|
||||
|
|
@ -203,20 +201,20 @@ TEST(HybridBayesNet, Switching) {
|
|||
// Check error for M(0) = 0
|
||||
const HybridValues values0{continuousValues, modeZero};
|
||||
double expectedError0 = 0;
|
||||
for (const auto& factor : graph) expectedError0 += factor->error(values0);
|
||||
for (const auto &factor : graph) expectedError0 += factor->error(values0);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError0, graph.error(values0), 1e-5);
|
||||
|
||||
// Check error for M(0) = 1
|
||||
const HybridValues values1{continuousValues, modeOne};
|
||||
double expectedError1 = 0;
|
||||
for (const auto& factor : graph) expectedError1 += factor->error(values1);
|
||||
for (const auto &factor : graph) expectedError1 += factor->error(values1);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError1, graph.error(values1), 1e-5);
|
||||
|
||||
// Check errorTree
|
||||
AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues);
|
||||
// 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
|
||||
EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5));
|
||||
|
|
@ -232,8 +230,8 @@ TEST(HybridBayesNet, Switching) {
|
|||
const AlgebraicDecisionTree<Key> graphPosterior =
|
||||
graph.discretePosterior(continuousValues);
|
||||
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));
|
||||
|
||||
// Make the clique of factors connected to x0:
|
||||
|
|
@ -275,15 +273,13 @@ TEST(HybridBayesNet, Switching) {
|
|||
// Check that the scalars incorporate the negative log constant of the
|
||||
// conditional
|
||||
EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(),
|
||||
(*phi_x1_m)(modeZero).second,
|
||||
1e-9);
|
||||
(*phi_x1_m)(modeZero).second, 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(),
|
||||
(*phi_x1_m)(modeOne).second,
|
||||
1e-9);
|
||||
(*phi_x1_m)(modeOne).second, 1e-9);
|
||||
|
||||
// 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 [gf, scalar] = (*phi_x1_m)(mode);
|
||||
|
||||
|
|
@ -342,7 +338,7 @@ TEST(HybridBayesNet, Switching) {
|
|||
// However, we can still check the total error for the clique factors_x1 and
|
||||
// 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);
|
||||
double originalError_x1 = factors_x1.error({continuousValues, mode});
|
||||
const double actualError = gc_x1->negLogConstant() +
|
||||
|
|
@ -372,7 +368,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
|
|||
Switching s(3);
|
||||
|
||||
// Check size of linearized factor graph
|
||||
const HybridGaussianFactorGraph& graph = s.linearizedFactorGraph;
|
||||
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
|
||||
EXPECT_LONGS_EQUAL(7, graph.size());
|
||||
|
||||
// Eliminate the graph
|
||||
|
|
|
|||
Loading…
Reference in New Issue