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

Merge pull request #1848 from borglab/feature/simpler_constructors 

Simpler HybridGaussianFoo constructors
release/4.3a0
Frank Dellaert 2024-09-27 14:37:12 -07:00 committed by GitHub
parent de23ebbd16 ce45bb6f2e
commit 31eade543d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
26 changed files with 353 additions and 363 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -29,8 +29,7 @@ std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key);
}
}
if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
} else if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key);
}

89
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -27,41 +27,59 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <cstddef>
namespace gtsam {
HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
const HybridGaussianConditional::Conditionals &conditionals) {
auto func = [](const GaussianConditional::shared_ptr &conditional)
-> GaussianFactorValuePair {
double value = 0.0;
// Check if conditional is pruned
if (conditional) {
// Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
value = conditional->negLogConstant();
/* *******************************************************************************/
struct HybridGaussianConditional::ConstructorHelper {
std::optional<size_t> nrFrontals;
HybridGaussianFactor::FactorValuePairs pairs;
double minNegLogConstant;
/// Compute all variables needed for the private constructor below.
ConstructorHelper(const Conditionals &conditionals)
: minNegLogConstant(std::numeric_limits<double>::infinity()) {
auto func = [this](const GaussianConditional::shared_ptr &c)
-> GaussianFactorValuePair {
double value = 0.0;
if (c) {
if (!nrFrontals.has_value()) {
nrFrontals = c->nrFrontals();
}
value = c->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value);
}
return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
};
pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
if (!nrFrontals.has_value()) {
throw std::runtime_error(
"HybridGaussianConditional: need at least one frontal variable.");
}
return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
};
return HybridGaussianFactor::FactorValuePairs(conditionals, func);
}
}
};
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals &conditionals,
const ConstructorHelper &helper)
: BaseFactor(discreteParents, helper.pairs),
BaseConditional(*helper.nrFrontals),
conditionals_(conditionals),
negLogConstant_(helper.minNegLogConstant) {}
HybridGaussianConditional::HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals &conditionals)
: BaseFactor(CollectKeys(continuousFrontals, continuousParents),
discreteParents, GetFactorValuePairs(conditionals)),
BaseConditional(continuousFrontals.size()),
conditionals_(conditionals) {
// Calculate negLogConstant_ as the minimum of the negative-log normalizers of
// the conditionals, by visiting the decision tree:
negLogConstant_ = std::numeric_limits<double>::infinity();
conditionals_.visit(
[this](const GaussianConditional::shared_ptr &conditional) {
if (conditional) {
this->negLogConstant_ =
std::min(this->negLogConstant_, conditional->negLogConstant());
}
});
}
: HybridGaussianConditional(discreteParents, conditionals,
ConstructorHelper(conditionals)) {}
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Conditionals({discreteParent}, conditionals)) {}
/* *******************************************************************************/
const HybridGaussianConditional::Conditionals &
@ -69,15 +87,6 @@ HybridGaussianConditional::conditionals() const {
return conditionals_;
}
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(continuousFrontals, continuousParents,
DiscreteKeys{discreteParent},
Conditionals({discreteParent}, conditionals)) {}
/* *******************************************************************************/
GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
const {
@ -222,8 +231,8 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
return {likelihood_m, Cgm_Kgcm};
}
});
return std::make_shared<HybridGaussianFactor>(
continuousParentKeys, discreteParentKeys, likelihoods);
return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
likelihoods);
}
/* ************************************************************************* */

70
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -64,27 +64,11 @@ class GTSAM_EXPORT HybridGaussianConditional
private:
Conditionals conditionals_; ///< a decision tree of Gaussian conditionals.
///< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))).
///< Take advantage of the neg-log space so everything is a minimization
double negLogConstant_;
/**
* @brief Convert a HybridGaussianConditional of conditionals into
* a DecisionTree of Gaussian factor graphs.
*/
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
/**
* @brief Helper function to get the pruner functor.
*
* @param discreteProbs The pruned discrete probabilities.
* @return std::function<GaussianConditional::shared_ptr(
* const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
*/
std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
prunerFunc(const DecisionTreeFactor &discreteProbs);
public:
/// @name Constructors
/// @{
@ -93,37 +77,28 @@ class GTSAM_EXPORT HybridGaussianConditional
HybridGaussianConditional() = default;
/**
* @brief Construct a new HybridGaussianConditional object.
* @brief Construct from one discrete key and vector of conditionals.
*
* @param discreteParent Single discrete parent variable
* @param conditionals Vector of conditionals with the same size as the
* cardinality of the discrete parent.
*/
HybridGaussianConditional(
const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr> &conditionals);
/**
* @brief Construct from multiple discrete keys and conditional tree.
*
* @param continuousFrontals the continuous frontals.
* @param continuousParents the continuous parents.
* @param discreteParents the discrete parents. Will be placed last.
* @param conditionals a decision tree of GaussianConditionals. The number of
* conditionals should be C^(number of discrete parents), where C is the
* cardinality of the DiscreteKeys in discreteParents, since the
* discreteParents will be used as the labels in the decision tree.
*/
HybridGaussianConditional(const KeyVector &continuousFrontals,
const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
HybridGaussianConditional(const DiscreteKeys &discreteParents,
const Conditionals &conditionals);
/**
* @brief Make a Hybrid Gaussian Conditional from
* a vector of Gaussian conditionals.
* The DecisionTree-based constructor is preferred over this one.
*
* @param continuousFrontals The continuous frontal variables
* @param continuousParents The continuous parent variables
* @param discreteParent Single discrete parent variable
* @param conditionals Vector of conditionals with the same size as the
* cardinality of the discrete parent.
*/
HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr> &conditionals);
/// @}
/// @name Testable
/// @{
@ -207,6 +182,23 @@ class GTSAM_EXPORT HybridGaussianConditional
/// @}
private:
/// Helper struct for private constructor.
struct ConstructorHelper;
/// Private constructor that uses helper struct above.
HybridGaussianConditional(
const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals &conditionals,
const ConstructorHelper &helper);
/// Convert to a DecisionTree of Gaussian factor graphs.
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
//// Get the pruner functor from pruned discrete probabilities.
std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
prunerFunc(const DecisionTreeFactor &prunedProbabilities);
/// Check whether `given` has values for all frontal keys.
bool allFrontalsGiven(const VectorValues &given) const;

96
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/base/utilities.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/linear/GaussianFactor.h>
@ -28,21 +29,12 @@
namespace gtsam {
/**
* @brief Helper function to augment the [A|b] matrices in the factor components
* with the additional scalar values.
* This is done by storing the value in
* the `b` vector as an additional row.
*
* @param factors DecisionTree of GaussianFactors and arbitrary scalars.
* Gaussian factor in factors.
* @return HybridGaussianFactor::Factors
*/
static HybridGaussianFactor::Factors augment(
const HybridGaussianFactor::FactorValuePairs &factors) {
/* *******************************************************************************/
HybridGaussianFactor::Factors HybridGaussianFactor::augment(
const FactorValuePairs &factors) {
// Find the minimum value so we can "proselytize" to positive values.
// Done because we can't have sqrt of negative numbers.
HybridGaussianFactor::Factors gaussianFactors;
Factors gaussianFactors;
AlgebraicDecisionTree<Key> valueTree;
std::tie(gaussianFactors, valueTree) = unzip(factors);
@ -73,22 +65,88 @@ static HybridGaussianFactor::Factors augment(
return std::dynamic_pointer_cast<GaussianFactor>(
std::make_shared<JacobianFactor>(gfg));
};
return HybridGaussianFactor::Factors(factors, update);
return Factors(factors, update);
}
/* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
struct HybridGaussianFactor::ConstructorHelper {
KeyVector continuousKeys; // Continuous keys extracted from factors
DiscreteKeys discreteKeys; // Discrete keys provided to the constructors
FactorValuePairs pairs; // Used only if factorsTree is empty
Factors factorsTree;
ConstructorHelper(const DiscreteKey &discreteKey,
const std::vector<GaussianFactor::shared_ptr> &factors)
: discreteKeys({discreteKey}) {
// Extract continuous keys from the first non-null factor
for (const auto &factor : factors) {
if (factor && continuousKeys.empty()) {
continuousKeys = factor->keys();
break;
}
}
// Build the DecisionTree from the factor vector
factorsTree = Factors(discreteKeys, factors);
}
ConstructorHelper(const DiscreteKey &discreteKey,
const std::vector<GaussianFactorValuePair> &factorPairs)
: discreteKeys({discreteKey}) {
// Extract continuous keys from the first non-null factor
for (const auto &pair : factorPairs) {
if (pair.first && continuousKeys.empty()) {
continuousKeys = pair.first->keys();
break;
}
}
// Build the FactorValuePairs DecisionTree
pairs = FactorValuePairs(discreteKeys, factorPairs);
}
ConstructorHelper(const DiscreteKeys &discreteKeys,
const FactorValuePairs &factorPairs)
: discreteKeys(discreteKeys) {
// Extract continuous keys from the first non-null factor
factorPairs.visit([&](const GaussianFactorValuePair &pair) {
if (pair.first && continuousKeys.empty()) {
continuousKeys = pair.first->keys();
}
});
// Build the FactorValuePairs DecisionTree
pairs = factorPairs;
}
};
/* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
: Base(helper.continuousKeys, helper.discreteKeys),
factors_(helper.factorsTree.empty() ? augment(helper.pairs)
: helper.factorsTree) {}
HybridGaussianFactor::HybridGaussianFactor(
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::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
const FactorValuePairs &factors)
: Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
: HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
/* *******************************************************************************/
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 empty,
// but not if both are empty or both are not empty:
// This will return false if either factors_ is empty or e->factors_ is
// empty, but not if both are empty or both are not empty:
if (factors_.empty() ^ e->factors_.empty()) return false;
// Check the base and the factors:

55
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -73,14 +73,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
/// Decision tree of Gaussian factors indexed by discrete keys.
Factors factors_;
/**
* @brief Helper function to return factors and functional to create a
* DecisionTree of Gaussian Factor Graphs.
*
* @return GaussianFactorGraphTree
*/
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
public:
/// @name Constructors
/// @{
@ -93,14 +85,11 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* providing the factors for each mode m as a vector of factors ϕ_m(x).
* The value ϕ(x,m) for the factor is simply ϕ_m(x).
*
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components.
* @param factors Vector of gaussian factors, one for each mode.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKey &discreteKey,
const std::vector<GaussianFactor::shared_ptr> &factors)
: Base(continuousKeys, {discreteKey}), factors_({discreteKey}, factors) {}
HybridGaussianFactor(const DiscreteKey &discreteKey,
const std::vector<GaussianFactor::shared_ptr> &factors);
/**
* @brief Construct a new HybridGaussianFactor on a single discrete key,
@ -108,15 +97,11 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* provided as a vector of pairs (ϕ_m(x), E_m).
* The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
*
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components.
* @param factors Vector of gaussian factor-scalar pairs, one per mode.
* @param factorPairs Vector of gaussian factor-scalar pairs, one per mode.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKey &discreteKey,
const std::vector<GaussianFactorValuePair> &factors)
: HybridGaussianFactor(continuousKeys, {discreteKey},
FactorValuePairs({discreteKey}, factors)) {}
HybridGaussianFactor(const DiscreteKey &discreteKey,
const std::vector<GaussianFactorValuePair> &factorPairs);
/**
* @brief Construct a new HybridGaussianFactor on a several discrete keys M,
@ -124,12 +109,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M).
* The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
*
* @param continuousKeys A vector of keys representing continuous variables.
* @param discreteKeys Discrete variables and their cardinalities.
* @param factors The decision tree of Gaussian factor/scalar pairs.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
HybridGaussianFactor(const DiscreteKeys &discreteKeys,
const FactorValuePairs &factors);
/// @}
@ -185,11 +168,37 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
}
/// @}
protected:
/**
* @brief Helper function to return factors and functional to create a
* DecisionTree of Gaussian Factor Graphs.
*
* @return GaussianFactorGraphTree
*/
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
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
* value in the `b` vector as an additional row.
*
* @param factors DecisionTree of GaussianFactors and arbitrary scalars.
* Gaussian factor in factors.
* @return HybridGaussianFactor::Factors
*/
static Factors augment(const FactorValuePairs &factors);
/// Helper method to compute the error of a component.
double potentiallyPrunedComponentError(
const sharedFactor &gf, const VectorValues &continuousValues) const;
/// Helper struct to assist private constructor below.
struct ConstructorHelper;
// Private constructor using ConstructorHelper above.
HybridGaussianFactor(const ConstructorHelper &helper);
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;

33
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -346,7 +346,6 @@ static std::shared_ptr<Factor> createDiscreteFactor(
// for conditional constants.
static std::shared_ptr<Factor> createHybridGaussianFactor(
const DecisionTree<Key, Result> &eliminationResults,
const KeyVector &continuousSeparator,
const DiscreteKeys &discreteSeparator) {
// Correct for the normalization constant used up by the conditional
auto correct = [&](const Result &pair) -> GaussianFactorValuePair {
@ -364,14 +363,12 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
correct);
return std::make_shared<HybridGaussianFactor>(continuousSeparator,
discreteSeparator, newFactors);
return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors);
}
static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
hybridElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys,
const KeyVector &continuousSeparator,
const std::set<DiscreteKey> &discreteSeparatorSet) {
// NOTE: since we use the special JunctionTree,
// only possibility is continuous conditioned on discrete.
@ -388,13 +385,18 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
factorGraphTree = removeEmpty(factorGraphTree);
// This is the elimination method on the leaf nodes
bool someContinuousLeft = false;
auto eliminate = [&](const GaussianFactorGraph &graph) -> Result {
if (graph.empty()) {
return {nullptr, nullptr};
}
// Expensive elimination of product factor.
auto result = EliminatePreferCholesky(graph, frontalKeys);
// Record whether there any continuous variables left
someContinuousLeft |= !result.second->empty();
return result;
};
@ -405,16 +407,15 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
// error for each discrete choice. Otherwise, create a HybridGaussianFactor
// on the separator, taking care to correct for conditional constants.
auto newFactor =
continuousSeparator.empty()
? createDiscreteFactor(eliminationResults, discreteSeparator)
: createHybridGaussianFactor(eliminationResults, continuousSeparator,
discreteSeparator);
someContinuousLeft
? createHybridGaussianFactor(eliminationResults, discreteSeparator)
: createDiscreteFactor(eliminationResults, discreteSeparator);
// Create the HybridGaussianConditional from the conditionals
HybridGaussianConditional::Conditionals conditionals(
eliminationResults, [](const Result &pair) { return pair.first; });
auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
frontalKeys, continuousSeparator, discreteSeparator, conditionals);
discreteSeparator, conditionals);
return {std::make_shared<HybridConditional>(hybridGaussian), newFactor};
}
@ -517,22 +518,12 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
// Case 3: We are now in the hybrid land!
KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end());
// Find all the keys in the set of continuous keys
// which are not in the frontal keys. This is our continuous separator.
KeyVector continuousSeparator;
auto continuousKeySet = factors.continuousKeySet();
std::set_difference(
continuousKeySet.begin(), continuousKeySet.end(),
frontalKeysSet.begin(), frontalKeysSet.end(),
std::inserter(continuousSeparator, continuousSeparator.begin()));
// Similarly for the discrete separator.
// Find all discrete keys.
// Since we eliminate all continuous variables first,
// the discrete separator will be *all* the discrete keys.
std::set<DiscreteKey> discreteSeparator = factors.discreteKeys();
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparator);
return hybridElimination(factors, frontalKeys, discreteSeparator);
}
}

92
gtsam/hybrid/HybridNonlinearFactor.cpp
View File

@ -17,56 +17,78 @@
*/
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <memory>
namespace gtsam {
/* *******************************************************************************/
static void checkKeys(const KeyVector& continuousKeys,
const std::vector<NonlinearFactorValuePair>& pairs) {
KeySet factor_keys_set;
for (const auto& pair : pairs) {
auto f = pair.first;
// Insert all factor continuous keys in the continuous keys set.
std::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.end()));
struct HybridNonlinearFactor::ConstructorHelper {
KeyVector continuousKeys; // Continuous keys extracted from factors
DiscreteKeys discreteKeys; // Discrete keys provided to the constructors
FactorValuePairs factorTree;
void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
if (!factor) return;
if (continuousKeys.empty()) {
continuousKeys = factor->keys();
} else if (factor->keys() != continuousKeys) {
throw std::runtime_error(
"HybridNonlinearFactor: all factors should have the same keys!");
}
}
KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
if (continuous_keys_set != factor_keys_set) {
throw std::runtime_error(
"HybridNonlinearFactor: The specified continuous keys and the keys in "
"the factors do not match!");
ConstructorHelper(const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors)
: discreteKeys({discreteKey}) {
std::vector<NonlinearFactorValuePair> pairs;
// Extract continuous keys from the first non-null factor
for (const auto& factor : factors) {
pairs.emplace_back(factor, 0.0);
copyOrCheckContinuousKeys(factor);
}
factorTree = FactorValuePairs({discreteKey}, pairs);
}
}
ConstructorHelper(const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& pairs)
: discreteKeys({discreteKey}) {
// Extract continuous keys from the first non-null factor
for (const auto& pair : pairs) {
copyOrCheckContinuousKeys(pair.first);
}
factorTree = FactorValuePairs({discreteKey}, pairs);
}
ConstructorHelper(const DiscreteKeys& discreteKeys,
const FactorValuePairs& factorPairs)
: discreteKeys(discreteKeys), factorTree(factorPairs) {
// Extract continuous keys from the first non-null factor
factorPairs.visit([&](const NonlinearFactorValuePair& pair) {
copyOrCheckContinuousKeys(pair.first);
});
}
};
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)
: Base(helper.continuousKeys, helper.discreteKeys),
factors_(helper.factorTree) {}
HybridNonlinearFactor::HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors)
: Base(continuousKeys, {discreteKey}) {
std::vector<NonlinearFactorValuePair> pairs;
for (auto&& f : factors) {
pairs.emplace_back(f, 0.0);
}
checkKeys(continuousKeys, pairs);
factors_ = FactorValuePairs({discreteKey}, pairs);
}
: HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& pairs)
: Base(continuousKeys, {discreteKey}) {
KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
checkKeys(continuousKeys, pairs);
factors_ = FactorValuePairs({discreteKey}, pairs);
}
: HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
/* *******************************************************************************/
HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKeys& discreteKeys,
HybridNonlinearFactor::HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
const FactorValuePairs& factors)
: Base(continuousKeys, discreteKeys), factors_(factors) {}
: HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(
@ -169,7 +191,7 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
linearized_factors(factors_, linearizeDT);
return std::make_shared<HybridGaussianFactor>(continuousKeys_, discreteKeys_,
return std::make_shared<HybridGaussianFactor>(discreteKeys_,
linearized_factors);
}

19
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -90,12 +90,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
* providing the factors for each mode m as a vector of factors ϕ_m(x).
* The value ϕ(x,m) for the factor is simply ϕ_m(x).
*
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components.
* @param factors Vector of gaussian factors, one for each mode.
*/
HybridNonlinearFactor(
const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
const DiscreteKey& discreteKey,
const std::vector<NonlinearFactor::shared_ptr>& factors);
/**
@ -104,12 +103,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
* provided as a vector of pairs (ϕ_m(x), E_m).
* The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
*
* @param continuousKeys Vector of keys for continuous factors.
* @param discreteKey The discrete key for the "mode", indexing components.
* @param pairs Vector of gaussian factor-scalar pairs, one per mode.
*/
HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKey& discreteKey,
HybridNonlinearFactor(const DiscreteKey& discreteKey,
const std::vector<NonlinearFactorValuePair>& pairs);
/**
@ -118,13 +115,12 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
* scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M).
* The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
*
* @param continuousKeys A vector of keys representing continuous variables.
* @param discreteKeys Discrete variables and their cardinalities.
* @param factors The decision tree of nonlinear factor/scalar pairs.
*/
HybridNonlinearFactor(const KeyVector& continuousKeys,
const DiscreteKeys& discreteKeys,
HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
const FactorValuePairs& factors);
/**
* @brief Compute error of the HybridNonlinearFactor as a tree.
*
@ -181,6 +177,13 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
/// Linearize all the continuous factors to get a HybridGaussianFactor.
std::shared_ptr<HybridGaussianFactor> linearize(
const Values& continuousValues) const;
private:
/// Helper struct to assist private constructor below.
struct ConstructorHelper;
// Private constructor using ConstructorHelper above.
HybridNonlinearFactor(const ConstructorHelper& helper);
};
// traits

16
gtsam/hybrid/hybrid.i
View File

@ -75,10 +75,12 @@ virtual class HybridConditional {
#include <gtsam/hybrid/HybridGaussianFactor.h>
class HybridGaussianFactor : gtsam::HybridFactor {
HybridGaussianFactor(
const gtsam::KeyVector& continuousKeys,
const gtsam::DiscreteKey& discreteKey,
const std::vector<gtsam::GaussianFactor::shared_ptr>& factors);
HybridGaussianFactor(
const gtsam::DiscreteKey& discreteKey,
const std::vector<std::pair<gtsam::GaussianFactor::shared_ptr, double>>&
factorsList);
factorPairs);
void print(string s = "HybridGaussianFactor\n",
const gtsam::KeyFormatter& keyFormatter =
@ -88,13 +90,9 @@ class HybridGaussianFactor : gtsam::HybridFactor {
#include <gtsam/hybrid/HybridGaussianConditional.h>
class HybridGaussianConditional : gtsam::HybridFactor {
HybridGaussianConditional(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const gtsam::HybridGaussianConditional::Conditionals& conditionals);
HybridGaussianConditional(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKey& discreteParent,
const std::vector<gtsam::GaussianConditional::shared_ptr>& conditionals);
@ -246,15 +244,15 @@ class HybridNonlinearFactorGraph {
#include <gtsam/hybrid/HybridNonlinearFactor.h>
class HybridNonlinearFactor : gtsam::HybridFactor {
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
const gtsam::DiscreteKey& discreteKey,
const std::vector<gtsam::NonlinearFactor*>& factors);
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
const gtsam::DiscreteKey& discreteKey,
const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<
gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);

5
gtsam/hybrid/tests/Switching.h
View File

@ -65,7 +65,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Z_3x1));
components.emplace_back(
new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Vector3::Ones()));
hfg.add(HybridGaussianFactor({x(t), x(t + 1)}, {m(t), 2}, components));
hfg.add(HybridGaussianFactor({m(t), 2}, components));
if (t > 1) {
hfg.add(DecisionTreeFactor({{m(t - 1), 2}, {m(t), 2}}, "0 1 1 3"));
@ -159,9 +159,8 @@ struct Switching {
// Add "motion models".
for (size_t k = 0; k < K - 1; k++) {
KeyVector keys = {X(k), X(k + 1)};
auto motion_models = motionModels(k, between_sigma);
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(modes[k],
motion_models);
}

3
gtsam/hybrid/tests/TinyHybridExample.h
View File

@ -46,8 +46,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
bayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals);
bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
}
// Create prior on X(0).

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

@ -43,9 +43,6 @@ const DiscreteValues m1Assignment{{M(0), 1}};
DiscreteConditional::shared_ptr mixing =
std::make_shared<DiscreteConditional>(m, "60/40");
// define Continuous keys
const KeyVector continuousKeys{Z(0)};
/**
* Create a simple Gaussian Mixture Model represented as p(z|m)P(m)
* where m is a discrete variable and z is a continuous variable.
@ -61,8 +58,7 @@ HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0,
model0),
c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1,
model1);
hbn.emplace_shared<HybridGaussianConditional>(continuousKeys, KeyVector{}, m,
std::vector{c0, c1});
hbn.emplace_shared<HybridGaussianConditional>(m, std::vector{c0, c1});
hbn.push_back(mixing);
return hbn;
}

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

@ -108,8 +108,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
HybridBayesNet bayesNet;
bayesNet.push_back(continuousConditional);
bayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(1)}, KeyVector{}, Asia,
std::vector{conditional0, conditional1});
Asia, std::vector{conditional0, conditional1});
bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1");
// Create values at which to evaluate.
@ -169,8 +168,7 @@ TEST(HybridBayesNet, Error) {
X(1), Vector1::Constant(2), I_1x1, model1);
auto gm = std::make_shared<HybridGaussianConditional>(
KeyVector{X(1)}, KeyVector{}, Asia,
std::vector{conditional0, conditional1});
Asia, std::vector{conditional0, conditional1});
// Create hybrid Bayes net.
HybridBayesNet bayesNet;
bayesNet.push_back(continuousConditional);
@ -390,7 +388,7 @@ TEST(HybridBayesNet, Sampling) {
auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
DiscreteKey(M(0), 2),
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
DiscreteKey mode(M(0), 2);

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

@ -437,8 +437,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
one_motion};
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
components);
nfg.emplace_shared<HybridNonlinearFactor>(m, components);
return nfg;
}
@ -566,8 +565,8 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
[](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
return {gf, 0.0};
});
return std::make_shared<HybridGaussianFactor>(
gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs);
return std::make_shared<HybridGaussianFactor>(gmf->discreteKeys(),
updated_pairs);
}
/****************************************************************************/
@ -591,9 +590,7 @@ TEST(HybridEstimation, ModeSelection) {
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
HybridNonlinearFactor mf(keys, modes, components);
HybridNonlinearFactor mf({M(0), 2}, components);
initial.insert(X(0), 0.0);
initial.insert(X(1), 0.0);
@ -623,10 +620,7 @@ TEST(HybridEstimation, ModeSelection) {
Z_1x1, noise_loose),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
Z_1x1, noise_tight)};
bn.emplace_shared<HybridGaussianConditional>(
KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
conditionals));
bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
VectorValues vv;
vv.insert(Z(0), Z_1x1);
@ -658,10 +652,7 @@ TEST(HybridEstimation, ModeSelection2) {
Z_3x1, noise_loose),
GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
Z_3x1, noise_tight)};
bn.emplace_shared<HybridGaussianConditional>(
KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
conditionals));
bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
VectorValues vv;
vv.insert(Z(0), Z_3x1);
@ -687,9 +678,7 @@ TEST(HybridEstimation, ModeSelection2) {
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
DiscreteKey modes(M(0), 2);
HybridNonlinearFactor mf(keys, modes, components);
HybridNonlinearFactor mf({M(0), 2}, components);
initial.insert<Vector3>(X(0), Z_3x1);
initial.insert<Vector3>(X(1), Z_3x1);

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

@ -55,7 +55,7 @@ TEST(HybridFactorGraph, Keys) {
std::vector<GaussianFactor::shared_ptr> components{
std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())};
hfg.add(HybridGaussianFactor({X(1)}, m1, components));
hfg.add(HybridGaussianFactor(m1, components));
KeySet expected_continuous{X(0), X(1)};
EXPECT(

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

@ -52,8 +52,7 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
commonSigma),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
commonSigma)};
const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode,
conditionals);
const HybridGaussianConditional hybrid_conditional(mode, conditionals);
} // namespace equal_constants
/* ************************************************************************* */
@ -158,8 +157,7 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
0.5),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
3.0)};
const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode,
conditionals);
const HybridGaussianConditional hybrid_conditional(mode, conditionals);
} // namespace mode_dependent_constants
/* ************************************************************************* */

32
gtsam/hybrid/tests/testHybridGaussianFactor.cpp
View File

@ -70,14 +70,14 @@ auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
// Test simple to complex constructors...
TEST(HybridGaussianFactor, ConstructorVariants) {
using namespace test_constructor;
HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11});
HybridGaussianFactor fromFactors(m1, {f10, f11});
std::vector<GaussianFactorValuePair> pairs{{f10, 0.0}, {f11, 0.0}};
HybridGaussianFactor fromPairs({X(1), X(2)}, m1, pairs);
HybridGaussianFactor fromPairs(m1, pairs);
assert_equal(fromFactors, fromPairs);
HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs);
HybridGaussianFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
HybridGaussianFactor fromDecisionTree({m1}, decisionTree);
assert_equal(fromDecisionTree, fromPairs);
}
@ -95,13 +95,12 @@ TEST(HybridGaussianFactor, Sum) {
// TODO(Frank): why specify keys at all? And: keys in factor should be *all*
// keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
// Design review!
HybridGaussianFactor hybridFactorA({X(1), X(2)}, m1, {f10, f11});
HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22});
HybridGaussianFactor hybridFactorA(m1, {f10, f11});
HybridGaussianFactor hybridFactorB(m2, {f20, f21, f22});
// Check that number of keys is 3
// Check the number of keys matches what we expect
EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size());
// Check that number of discrete keys is 1
EXPECT_LONGS_EQUAL(2, hybridFactorA.continuousKeys().size());
EXPECT_LONGS_EQUAL(1, hybridFactorA.discreteKeys().size());
// Create sum of two hybrid factors: it will be a decision tree now on both
@ -122,7 +121,7 @@ TEST(HybridGaussianFactor, Sum) {
/* ************************************************************************* */
TEST(HybridGaussianFactor, Printing) {
using namespace test_constructor;
HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f10, f11});
HybridGaussianFactor hybridFactor(m1, {f10, f11});
std::string expected =
R"(HybridGaussianFactor
@ -159,17 +158,13 @@ Hybrid [x1 x2; 1]{
/* ************************************************************************* */
TEST(HybridGaussianFactor, HybridGaussianConditional) {
KeyVector keys;
keys.push_back(X(0));
keys.push_back(X(1));
DiscreteKeys dKeys;
dKeys.emplace_back(M(0), 2);
dKeys.emplace_back(M(1), 2);
auto gaussians = std::make_shared<GaussianConditional>();
HybridGaussianConditional::Conditionals conditionals(gaussians);
HybridGaussianConditional gm({}, keys, dKeys, conditionals);
HybridGaussianConditional gm(dKeys, conditionals);
EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
}
@ -189,7 +184,7 @@ TEST(HybridGaussianFactor, Error) {
auto f0 = std::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f0, f1});
HybridGaussianFactor hybridFactor(m1, {f0, f1});
VectorValues continuousValues;
continuousValues.insert(X(1), Vector2(0, 0));
@ -234,9 +229,8 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
-I_1x1, model1);
DiscreteKeys discreteParents{m1};
return std::make_shared<HybridGaussianConditional>(
KeyVector{X(1)}, KeyVector{X(0)}, discreteParents,
HybridGaussianConditional::Conditionals(discreteParents,
std::vector{c0, c1}));
discreteParents, HybridGaussianConditional::Conditionals(
discreteParents, std::vector{c0, c1}));
}
/// Create two state Bayes network with 1 or two measurement models
@ -595,7 +589,7 @@ static HybridGaussianFactorGraph CreateFactorGraph(
// the underlying scalar to be log(\sqrt(|2πΣ|))
std::vector<GaussianFactorValuePair> factors{{f0, model0->negLogConstant()},
{f1, model1->negLogConstant()}};
HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
HybridGaussianFactor motionFactor(m1, factors);
HybridGaussianFactorGraph hfg;
hfg.push_back(motionFactor);

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

@ -72,13 +72,10 @@ TEST(HybridGaussianFactorGraph, Creation) {
// Define a hybrid gaussian conditional P(x0|x1, c0)
// and add it to the factor graph.
HybridGaussianConditional gm(
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}),
HybridGaussianConditional::Conditionals(
M(0),
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)));
{M(0), 2},
{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)});
hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size());
@ -135,7 +132,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
// Add a hybrid gaussian factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2);
hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1))));
hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
auto result = hfg.eliminateSequential();
@ -158,7 +155,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1))));
hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
// Discrete probability table for c1
hfg.add(DecisionTreeFactor(m1, {2, 8}));
@ -180,7 +177,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, two::components(X(1))));
hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(1))));
hfg.add(DecisionTreeFactor(m1, {2, 8}));
// TODO(Varun) Adding extra discrete variable not connected to continuous
@ -207,7 +204,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Hybrid factor P(x1|c1)
hfg.add(HybridGaussianFactor({X(1)}, m, two::components(X(1))));
hfg.add(HybridGaussianFactor(m, two::components(X(1))));
// Prior factor on c1
hfg.add(DecisionTreeFactor(m, {2, 8}));
@ -232,8 +229,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
{
hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, two::components(X(0))));
hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2))));
hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0))));
hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2))));
}
hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
@ -242,8 +239,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
{
hfg.add(HybridGaussianFactor({X(3)}, {M(3), 2}, two::components(X(3))));
hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5))));
hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3))));
hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5))));
}
auto ordering_full =
@ -528,7 +525,7 @@ TEST(HybridGaussianFactorGraph, optimize) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
hfg.add(HybridGaussianFactor({X(1)}, c1, two::components(X(1))));
hfg.add(HybridGaussianFactor(c1, two::components(X(1))));
auto result = hfg.eliminateSequential();
@ -654,11 +651,7 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) {
x0, -I_1x1, model0),
c1 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1,
x0, -I_1x1, model1);
DiscreteKeys discreteParents{m1};
hbn.emplace_shared<HybridGaussianConditional>(
KeyVector{f01}, KeyVector{x0, x1}, discreteParents,
HybridGaussianConditional::Conditionals(discreteParents,
std::vector{c0, c1}));
hbn.emplace_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});
// Discrete uniform prior.
hbn.emplace_shared<DiscreteConditional>(m1, "0.5/0.5");
@ -830,11 +823,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
X(0), Vector1(14.1421), I_1x1 * 2.82843),
conditional1 = std::make_shared<GaussianConditional>(
X(0), Vector1(10.1379), I_1x1 * 2.02759);
DiscreteKeys discreteParents{mode};
expectedBayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(0)}, KeyVector{}, discreteParents,
HybridGaussianConditional::Conditionals(
discreteParents, std::vector{conditional0, conditional1}));
mode, std::vector{conditional0, conditional1});
// Add prior on mode.
expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
@ -860,10 +850,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)};
auto gm = std::make_shared<HybridGaussianConditional>(
KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
conditionals));
auto gm = std::make_shared<HybridGaussianConditional>(mode, conditionals);
bn.push_back(gm);
// Create prior on X(0).
@ -891,9 +878,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
conditional1 = std::make_shared<GaussianConditional>(
X(0), Vector1(14.1421), I_1x1 * 2.82843);
expectedBayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(
DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
mode, std::vector{conditional0, conditional1});
// Add prior on mode.
expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "1/1");
@ -929,9 +914,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
conditional1 = std::make_shared<GaussianConditional>(
X(0), Vector1(10.274), I_1x1 * 2.0548);
expectedBayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(
DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
mode, std::vector{conditional0, conditional1});
// Add prior on mode.
expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
@ -980,10 +963,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5),
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1,
3.0)};
bn.emplace_shared<HybridGaussianConditional>(
KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t},
HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t},
conditionals));
bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, conditionals);
// Create prior on discrete mode N(t):
bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
@ -999,10 +979,8 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
0.2),
GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1,
0.2)};
auto gm = std::make_shared<HybridGaussianConditional>(
KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t},
HybridGaussianConditional::Conditionals(DiscreteKeys{motion_model_t},
conditionals));
auto gm = std::make_shared<HybridGaussianConditional>(motion_model_t,
conditionals);
bn.push_back(gm);
// Create prior on motion model M(t):

12
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -414,15 +414,13 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add odometry factor with discrete modes.
Pose2 odometry(1.0, 0.0, 0.0);
KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
std::vector<NonlinearFactor::shared_ptr> components;
components.emplace_back(
new PlanarMotionModel(W(0), W(1), odometry, noise_model)); // moving
components.emplace_back(
new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model)); // still
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@ -454,14 +452,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 3 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(1), W(2)};
components.clear();
components.emplace_back(
new PlanarMotionModel(W(1), W(2), odometry, noise_model)); // moving
components.emplace_back(
new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model)); // still
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@ -496,14 +492,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
/*************** Run Round 4 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(2), W(3)};
components.clear();
components.emplace_back(
new PlanarMotionModel(W(2), W(3), odometry, noise_model)); // moving
components.emplace_back(
new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model)); // still
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

10
gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
View File

@ -60,14 +60,14 @@ auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
// Test simple to complex constructors...
TEST(HybridGaussianFactor, ConstructorVariants) {
using namespace test_constructor;
HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
HybridNonlinearFactor fromFactors(m1, {f0, f1});
std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
HybridNonlinearFactor fromPairs(m1, pairs);
assert_equal(fromFactors, fromPairs);
HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
HybridNonlinearFactor fromDecisionTree({m1}, decisionTree);
assert_equal(fromDecisionTree, fromPairs);
}
@ -75,7 +75,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) {
// Test .print() output.
TEST(HybridNonlinearFactor, Printing) {
using namespace test_constructor;
HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
HybridNonlinearFactor hybridFactor({m1}, {f0, f1});
std::string expected =
R"(Hybrid [x1 x2; 1]
@ -101,7 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
return HybridNonlinearFactor(m1, {f0, f1});
}
/* ************************************************************************* */

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

@ -117,7 +117,6 @@ TEST(HybridNonlinearFactorGraph, Resize) {
/***************************************************************************/
namespace test_motion {
KeyVector contKeys = {X(0), X(1)};
gtsam::DiscreteKey m1(M(1), 2);
auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
std::vector<NonlinearFactor::shared_ptr> components = {
@ -139,8 +138,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
hnfg.push_back(discreteFactor);
auto dcFactor =
std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
auto dcFactor = std::make_shared<HybridNonlinearFactor>(m1, components);
hnfg.push_back(dcFactor);
Values linearizationPoint;
@ -156,26 +154,6 @@ TEST(HybridGaussianFactorGraph, Resize) {
EXPECT_LONGS_EQUAL(gfg.size(), 0);
}
/***************************************************************************
* Test that the HybridNonlinearFactor reports correctly if the number of
* continuous keys provided do not match the keys in the factors.
*/
TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
using namespace test_motion;
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
// Check for exception when number of continuous keys are under-specified.
THROWS_EXCEPTION(
std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
// Check for exception when number of continuous keys are too many.
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1), X(2)}, m1, components));
}
/*****************************************************************************
* Test push_back on HFG makes the correct distinction.
*/
@ -828,14 +806,12 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
// Add odometry factor
Pose2 odometry(2.0, 0.0, 0.0);
KeyVector contKeys = {X(0), X(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
std::vector<NonlinearFactor::shared_ptr> motion_models = {
std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
noise_model),
std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey{M(1), 2}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
// create a noise model for the landmark measurements
@ -901,7 +877,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
{f1, model1->negLogConstant()}};
HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
HybridNonlinearFactor mixtureFactor(m1, factors);
HybridNonlinearFactorGraph hfg;
hfg.push_back(mixtureFactor);

12
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -433,15 +433,13 @@ TEST(HybridNonlinearISAM, NonTrivial) {
/*************** Run Round 2 ***************/
// Add odometry factor with discrete modes.
Pose2 odometry(1.0, 0.0, 0.0);
KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
std::vector<NonlinearFactor::shared_ptr> components{moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@ -473,14 +471,12 @@ TEST(HybridNonlinearISAM, NonTrivial) {
/*************** Run Round 3 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(1), W(2)};
still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@ -515,14 +511,12 @@ TEST(HybridNonlinearISAM, NonTrivial) {
/*************** Run Round 4 ***************/
// Add odometry factor with discrete modes.
contKeys = {W(2), W(3)};
still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still};
fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

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

@ -75,7 +75,6 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
/* ****************************************************************************/
// Test HybridGaussianFactor serialization.
TEST(HybridSerialization, HybridGaussianFactor) {
KeyVector continuousKeys{X(0)};
DiscreteKey discreteKey{M(0), 2};
auto A = Matrix::Zero(2, 1);
@ -85,7 +84,7 @@ TEST(HybridSerialization, HybridGaussianFactor) {
auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);
const HybridGaussianFactor factor(discreteKey, factors);
EXPECT(equalsObj<HybridGaussianFactor>(factor));
EXPECT(equalsXML<HybridGaussianFactor>(factor));
@ -115,9 +114,7 @@ TEST(HybridSerialization, HybridGaussianConditional) {
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5));
const auto conditional1 = std::make_shared<GaussianConditional>(
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode},
HybridGaussianConditional::Conditionals(
{mode}, {conditional0, conditional1}));
const HybridGaussianConditional gm(mode, {conditional0, conditional1});
EXPECT(equalsObj<HybridGaussianConditional>(gm));
EXPECT(equalsXML<HybridGaussianConditional>(gm));

5
python/gtsam/tests/test_HybridBayesNet.py
View File

@ -17,7 +17,7 @@ import numpy as np
from gtsam.symbol_shorthand import A, X
from gtsam.utils.test_case import GtsamTestCase
from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues,
from gtsam import (DiscreteConditional, DiscreteValues,
GaussianConditional, HybridBayesNet,
HybridGaussianConditional, HybridValues, VectorValues,
noiseModel)
@ -48,8 +48,7 @@ class TestHybridBayesNet(GtsamTestCase):
bayesNet = HybridBayesNet()
bayesNet.push_back(conditional)
bayesNet.push_back(
HybridGaussianConditional([X(1)], [], Asia,
[conditional0, conditional1]))
HybridGaussianConditional(Asia, [conditional0, conditional1]))
bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
# Create values at which to evaluate.

9
python/gtsam/tests/test_HybridFactorGraph.py
View File

@ -17,7 +17,7 @@ from gtsam.symbol_shorthand import C, M, X, Z
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
from gtsam import (DiscreteConditional, GaussianConditional,
HybridBayesNet, HybridGaussianConditional,
HybridGaussianFactor, HybridGaussianFactorGraph,
HybridValues, JacobianFactor, noiseModel)
@ -35,7 +35,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
gmf = HybridGaussianFactor((C(0), 2), [(jf1, 0), (jf2, 0)])
hfg = HybridGaussianFactorGraph()
hfg.push_back(jf1)
@ -60,7 +60,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
gmf = HybridGaussianFactor((C(0), 2), [(jf1, 0), (jf2, 0)])
hfg = HybridGaussianFactorGraph()
hfg.push_back(jf1)
@ -102,8 +102,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
X(0), [0],
sigma=3)
bayesNet.push_back(
HybridGaussianConditional([Z(i)], [X(0)], mode,
[conditional0, conditional1]))
HybridGaussianConditional(mode, [conditional0, conditional1]))
# Create prior on X(0).
prior_on_x0 = GaussianConditional.FromMeanAndStddev(

7
python/gtsam/tests/test_HybridNonlinearFactorGraph.py
View File

@ -14,8 +14,6 @@ from __future__ import print_function
import unittest
import numpy as np
from gtsam.symbol_shorthand import C, X
from gtsam.utils.test_case import GtsamTestCase
import gtsam
@ -38,8 +36,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
noiseModel.Unit.Create(3)), 0.0),
(PriorFactorPoint3(1, Point3(1, 2, 1),
noiseModel.Unit.Create(3)), 0.0)]
nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors))
nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3"))
mode = (10, 2)
nlfg.push_back(gtsam.HybridNonlinearFactor(mode, factors))
nlfg.push_back(gtsam.DecisionTreeFactor(mode, "1 3"))
values = gtsam.Values()
values.insert_point3(1, Point3(0, 0, 0))
values.insert_point3(2, Point3(2, 3, 1))