update variables and docstrings to remove the mixture terminology

release/4.3a0
Varun Agrawal 2024-09-16 14:33:24 -04:00
parent 2c140df196
commit 4016de7942
27 changed files with 153 additions and 146 deletions

View File

@ -49,7 +49,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
const DecisionTreeFactor &prunedDiscreteProbs, const DecisionTreeFactor &prunedDiscreteProbs,
const HybridConditional &conditional) { const HybridConditional &conditional) {
// Get the discrete keys as sets for the decision tree // Get the discrete keys as sets for the decision tree
// and the Gaussian mixture. // and the hybrid Gaussian conditional.
std::set<DiscreteKey> discreteProbsKeySet = std::set<DiscreteKey> discreteProbsKeySet =
DiscreteKeysAsSet(prunedDiscreteProbs.discreteKeys()); DiscreteKeysAsSet(prunedDiscreteProbs.discreteKeys());
std::set<DiscreteKey> conditionalKeySet = std::set<DiscreteKey> conditionalKeySet =
@ -63,7 +63,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
// typecast so we can use this to get probability value // typecast so we can use this to get probability value
DiscreteValues values(choices); DiscreteValues values(choices);
// Case where the Gaussian mixture has the same // Case where the hybrid Gaussian conditional has the same
// discrete keys as the decision tree. // discrete keys as the decision tree.
if (conditionalKeySet == discreteProbsKeySet) { if (conditionalKeySet == discreteProbsKeySet) {
if (prunedDiscreteProbs(values) == 0) { if (prunedDiscreteProbs(values) == 0) {
@ -181,7 +181,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
// Bayes Net and prune them as per prunedDiscreteProbs. // Bayes Net and prune them as per prunedDiscreteProbs.
for (auto &&conditional : *this) { for (auto &&conditional : *this) {
if (auto gm = conditional->asMixture()) { if (auto gm = conditional->asMixture()) {
// Make a copy of the Gaussian mixture and prune it! // Make a copy of the hybrid Gaussian conditional and prune it!
auto prunedHybridGaussianConditional = auto prunedHybridGaussianConditional =
std::make_shared<HybridGaussianConditional>(*gm); std::make_shared<HybridGaussianConditional>(*gm);
prunedHybridGaussianConditional->prune( prunedHybridGaussianConditional->prune(

View File

@ -28,7 +28,8 @@ namespace gtsam {
/** /**
* A hybrid Bayes net is a collection of HybridConditionals, which can have * A hybrid Bayes net is a collection of HybridConditionals, which can have
* discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals. * discrete conditionals, hybrid Gaussian conditionals,
* or pure Gaussian conditionals.
* *
* @ingroup hybrid * @ingroup hybrid
*/ */

View File

@ -205,9 +205,9 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
// If conditional is hybrid, we prune it. // If conditional is hybrid, we prune it.
if (conditional->isHybrid()) { if (conditional->isHybrid()) {
auto gaussianMixture = conditional->asMixture(); auto hybridGaussianCond = conditional->asMixture();
gaussianMixture->prune(parentData.prunedDiscreteProbs); hybridGaussianCond->prune(parentData.prunedDiscreteProbs);
} }
return parentData; return parentData;
} }

View File

@ -99,8 +99,8 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
/** /**
* @brief Recursively optimize the BayesTree to produce a vector solution. * @brief Recursively optimize the BayesTree to produce a vector solution.
* *
* @param assignment The discrete values assignment to select the Gaussian * @param assignment The discrete values assignment to select
* mixtures. * the hybrid conditional.
* @return VectorValues * @return VectorValues
*/ */
VectorValues optimize(const DiscreteValues& assignment) const; VectorValues optimize(const DiscreteValues& assignment) const;
@ -170,7 +170,7 @@ class BayesTreeOrphanWrapper<HybridBayesTreeClique> : public HybridConditional {
void print( void print(
const std::string& s = "", const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override { const KeyFormatter& formatter = DefaultKeyFormatter) const override {
clique->print(s + "stored clique", formatter); clique->print(s + " stored clique ", formatter);
} }
}; };

View File

@ -50,11 +50,11 @@ HybridConditional::HybridConditional(
/* ************************************************************************ */ /* ************************************************************************ */
HybridConditional::HybridConditional( HybridConditional::HybridConditional(
const std::shared_ptr<HybridGaussianConditional> &gaussianMixture) const std::shared_ptr<HybridGaussianConditional> &hybridGaussianCond)
: BaseFactor(gaussianMixture->continuousKeys(), : BaseFactor(hybridGaussianCond->continuousKeys(),
gaussianMixture->discreteKeys()), hybridGaussianCond->discreteKeys()),
BaseConditional(gaussianMixture->nrFrontals()) { BaseConditional(hybridGaussianCond->nrFrontals()) {
inner_ = gaussianMixture; inner_ = hybridGaussianCond;
} }
/* ************************************************************************ */ /* ************************************************************************ */

View File

@ -124,11 +124,11 @@ class GTSAM_EXPORT HybridConditional
/** /**
* @brief Construct a new Hybrid Conditional object * @brief Construct a new Hybrid Conditional object
* *
* @param gaussianMixture Gaussian Mixture Conditional used to create the * @param hybridGaussianCond Hybrid Gaussian Conditional used to create the
* HybridConditional. * HybridConditional.
*/ */
HybridConditional( HybridConditional(
const std::shared_ptr<HybridGaussianConditional>& gaussianMixture); const std::shared_ptr<HybridGaussianConditional>& hybridGaussianCond);
/// @} /// @}
/// @name Testable /// @name Testable
@ -148,7 +148,7 @@ class GTSAM_EXPORT HybridConditional
/** /**
* @brief Return HybridConditional as a HybridGaussianConditional * @brief Return HybridConditional as a HybridGaussianConditional
* @return nullptr if not a mixture * @return nullptr if not a conditional
* @return HybridGaussianConditional::shared_ptr otherwise * @return HybridGaussianConditional::shared_ptr otherwise
*/ */
HybridGaussianConditional::shared_ptr asMixture() const { HybridGaussianConditional::shared_ptr asMixture() const {

View File

@ -249,20 +249,20 @@ std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)> const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) { HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) {
// Get the discrete keys as sets for the decision tree // Get the discrete keys as sets for the decision tree
// and the gaussian mixture. // and the hybrid gaussian conditional.
auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys());
auto gaussianMixtureKeySet = DiscreteKeysAsSet(this->discreteKeys()); auto hybridGaussianCondKeySet = DiscreteKeysAsSet(this->discreteKeys());
auto pruner = [discreteProbs, discreteProbsKeySet, gaussianMixtureKeySet]( auto pruner = [discreteProbs, discreteProbsKeySet, hybridGaussianCondKeySet](
const Assignment<Key> &choices, const Assignment<Key> &choices,
const GaussianConditional::shared_ptr &conditional) const GaussianConditional::shared_ptr &conditional)
-> GaussianConditional::shared_ptr { -> GaussianConditional::shared_ptr {
// typecast so we can use this to get probability value // typecast so we can use this to get probability value
const DiscreteValues values(choices); const DiscreteValues values(choices);
// Case where the gaussian mixture has the same // Case where the hybrid gaussian conditional has the same
// discrete keys as the decision tree. // discrete keys as the decision tree.
if (gaussianMixtureKeySet == discreteProbsKeySet) { if (hybridGaussianCondKeySet == discreteProbsKeySet) {
if (discreteProbs(values) == 0.0) { if (discreteProbs(values) == 0.0) {
// empty aka null pointer // empty aka null pointer
std::shared_ptr<GaussianConditional> null; std::shared_ptr<GaussianConditional> null;
@ -274,7 +274,7 @@ HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) {
std::vector<DiscreteKey> set_diff; std::vector<DiscreteKey> set_diff;
std::set_difference( std::set_difference(
discreteProbsKeySet.begin(), discreteProbsKeySet.end(), discreteProbsKeySet.begin(), discreteProbsKeySet.end(),
gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(), hybridGaussianCondKeySet.begin(), hybridGaussianCondKeySet.end(),
std::back_inserter(set_diff)); std::back_inserter(set_diff));
const std::vector<DiscreteValues> assignments = const std::vector<DiscreteValues> assignments =

View File

@ -33,8 +33,8 @@ namespace gtsam {
class HybridValues; class HybridValues;
/** /**
* @brief A conditional of gaussian mixtures indexed by discrete variables, as * @brief A conditional of gaussian conditionals indexed by discrete variables,
* part of a Bayes Network. This is the result of the elimination of a * as part of a Bayes Network. This is the result of the elimination of a
* continuous variable in a hybrid scheme, such that the remaining variables are * continuous variable in a hybrid scheme, such that the remaining variables are
* discrete+continuous. * discrete+continuous.
* *
@ -107,7 +107,7 @@ class GTSAM_EXPORT HybridGaussianConditional
const Conditionals &conditionals); const Conditionals &conditionals);
/** /**
* @brief Make a Gaussian Mixture from a vector of Gaussian conditionals. * @brief Make a Hybrid Gaussian Conditional from a vector of Gaussian conditionals.
* The DecisionTree-based constructor is preferred over this one. * The DecisionTree-based constructor is preferred over this one.
* *
* @param continuousFrontals The continuous frontal variables * @param continuousFrontals The continuous frontal variables
@ -152,8 +152,8 @@ class GTSAM_EXPORT HybridGaussianConditional
double logNormalizationConstant() const override { return logConstant_; } double logNormalizationConstant() const override { return logConstant_; }
/** /**
* Create a likelihood factor for a Gaussian mixture, return a Mixture factor * Create a likelihood factor for a hybrid Gaussian conditional,
* on the parents. * return a hybrid Gaussian factor on the parents.
*/ */
std::shared_ptr<HybridGaussianFactor> likelihood( std::shared_ptr<HybridGaussianFactor> likelihood(
const VectorValues &given) const; const VectorValues &given) const;
@ -172,9 +172,9 @@ class GTSAM_EXPORT HybridGaussianConditional
const VectorValues &continuousValues) const; const VectorValues &continuousValues) const;
/** /**
* @brief Compute the error of this Gaussian Mixture. * @brief Compute the error of this hybrid Gaussian conditional.
* *
* This requires some care, as different mixture components may have * This requires some care, as different components may have
* different normalization constants. Let's consider p(x|y,m), where m is * different normalization constants. Let's consider p(x|y,m), where m is
* discrete. We need the error to satisfy the invariant: * discrete. We need the error to satisfy the invariant:
* *
@ -209,7 +209,7 @@ class GTSAM_EXPORT HybridGaussianConditional
const VectorValues &continuousValues) const; const VectorValues &continuousValues) const;
/** /**
* @brief Compute the logProbability of this Gaussian Mixture. * @brief Compute the logProbability of this hybrid Gaussian conditional.
* *
* @param values Continuous values and discrete assignment. * @param values Continuous values and discrete assignment.
* @return double * @return double

View File

@ -37,13 +37,13 @@ class VectorValues;
using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>; using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
/** /**
* @brief Implementation of a discrete conditional mixture factor. * @brief Implementation of a discrete-conditioned hybrid factor.
* Implements a joint discrete-continuous factor where the discrete variable * Implements a joint discrete-continuous factor where the discrete variable
* serves to "select" a mixture component corresponding to a GaussianFactor type * serves to "select" a component corresponding to a GaussianFactor.
* of measurement.
* *
* Represents the underlying Gaussian mixture as a Decision Tree, where the set * Represents the underlying hybrid Gaussian components as a Decision Tree,
* of discrete variables indexes to the continuous gaussian distribution. * where the set of discrete variables indexes to
* the continuous gaussian distribution.
* *
* @ingroup hybrid * @ingroup hybrid
*/ */
@ -80,7 +80,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
HybridGaussianFactor() = default; HybridGaussianFactor() = default;
/** /**
* @brief Construct a new Gaussian mixture factor. * @brief Construct a new hybrid Gaussian factor.
* *
* @param continuousKeys A vector of keys representing continuous variables. * @param continuousKeys A vector of keys representing continuous variables.
* @param discreteKeys A vector of keys representing discrete variables and * @param discreteKeys A vector of keys representing discrete variables and

View File

@ -437,8 +437,8 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
// NOTE: Because we are in the Conditional Gaussian regime there are only // NOTE: Because we are in the Conditional Gaussian regime there are only
// a few cases: // a few cases:
// 1. continuous variable, make a Gaussian Mixture if there are hybrid // 1. continuous variable, make a hybrid Gaussian conditional if there are
// factors; // hybrid factors;
// 2. continuous variable, we make a Gaussian Factor if there are no hybrid // 2. continuous variable, we make a Gaussian Factor if there are no hybrid
// factors; // factors;
// 3. discrete variable, no continuous factor is allowed // 3. discrete variable, no continuous factor is allowed
@ -550,9 +550,10 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
f = hc->inner(); f = hc->inner();
} }
if (auto gaussianMixture = dynamic_pointer_cast<HybridGaussianFactor>(f)) { if (auto hybridGaussianCond =
dynamic_pointer_cast<HybridGaussianFactor>(f)) {
// Compute factor error and add it. // Compute factor error and add it.
error_tree = error_tree + gaussianMixture->errorTree(continuousValues); error_tree = error_tree + hybridGaussianCond->errorTree(continuousValues);
} else if (auto gaussian = dynamic_pointer_cast<GaussianFactor>(f)) { } else if (auto gaussian = dynamic_pointer_cast<GaussianFactor>(f)) {
// If continuous only, get the (double) error // If continuous only, get the (double) error
// and add it to the error_tree // and add it to the error_tree

View File

@ -210,7 +210,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
* @brief Create a decision tree of factor graphs out of this hybrid factor * @brief Create a decision tree of factor graphs out of this hybrid factor
* graph. * graph.
* *
* For example, if there are two mixture factors, one with a discrete key A * For example, if there are two hybrid factors, one with a discrete key A
* and one with a discrete key B, then the decision tree will have two levels, * and one with a discrete key B, then the decision tree will have two levels,
* one for A and one for B. The leaves of the tree will be the Gaussian * one for A and one for B. The leaves of the tree will be the Gaussian
* factors that have only continuous keys. * factors that have only continuous keys.

View File

@ -37,11 +37,10 @@ namespace gtsam {
using NonlinearFactorValuePair = std::pair<NonlinearFactor::shared_ptr, double>; using NonlinearFactorValuePair = std::pair<NonlinearFactor::shared_ptr, double>;
/** /**
* @brief Implementation of a discrete conditional mixture factor. * @brief Implementation of a discrete-conditioned hybrid factor.
* *
* Implements a joint discrete-continuous factor where the discrete variable * Implements a joint discrete-continuous factor where the discrete variable
* serves to "select" a mixture component corresponding to a NonlinearFactor * serves to "select" a hybrid component corresponding to a NonlinearFactor.
* type of measurement.
* *
* This class stores all factors as HybridFactors which can then be typecast to * This class stores all factors as HybridFactors which can then be typecast to
* one of (NonlinearFactor, GaussianFactor) which can then be checked to perform * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform

View File

@ -151,7 +151,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
if (!f) { if (!f) {
continue; continue;
} }
// Check if it is a nonlinear mixture factor // Check if it is a hybrid nonlinear factor
if (auto mf = dynamic_pointer_cast<HybridNonlinearFactor>(f)) { if (auto mf = dynamic_pointer_cast<HybridNonlinearFactor>(f)) {
const HybridGaussianFactor::shared_ptr& gmf = const HybridGaussianFactor::shared_ptr& gmf =
mf->linearize(continuousValues); mf->linearize(continuousValues);

View File

@ -100,7 +100,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
// If hybridBayesNet is not empty, // If hybridBayesNet is not empty,
// it means we have conditionals to add to the factor graph. // it means we have conditionals to add to the factor graph.
if (!hybridBayesNet.empty()) { if (!hybridBayesNet.empty()) {
// We add all relevant conditional mixtures on the last continuous variable // We add all relevant hybrid conditionals on the last continuous variable
// in the previous `hybridBayesNet` to the graph // in the previous `hybridBayesNet` to the graph
// Conditionals to remove from the bayes net // Conditionals to remove from the bayes net

View File

@ -34,7 +34,7 @@ class GTSAM_EXPORT HybridSmoother {
* Given new factors, perform an incremental update. * Given new factors, perform an incremental update.
* The relevant densities in the `hybridBayesNet` will be added to the input * The relevant densities in the `hybridBayesNet` will be added to the input
* graph (fragment), and then eliminated according to the `ordering` * graph (fragment), and then eliminated according to the `ordering`
* presented. The remaining factor graph contains Gaussian mixture factors * presented. The remaining factor graph contains hybrid Gaussian factors
* that are not connected to the variables in the ordering, or a single * that are not connected to the variables in the ordering, or a single
* discrete factor on all discrete keys, plus all discrete factors in the * discrete factor on all discrete keys, plus all discrete factors in the
* original graph. * original graph.
@ -68,7 +68,13 @@ class GTSAM_EXPORT HybridSmoother {
const HybridGaussianFactorGraph& graph, const HybridGaussianFactorGraph& graph,
const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const; const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const;
/// Get the Gaussian Mixture from the Bayes Net posterior at `index`. /**
* @brief Get the hybrid Gaussian conditional from
* the Bayes Net posterior at `index`.
*
* @param index Indexing value.
* @return HybridGaussianConditional::shared_ptr
*/
HybridGaussianConditional::shared_ptr gaussianMixture(size_t index) const; HybridGaussianConditional::shared_ptr gaussianMixture(size_t index) const;
/// Return the Bayes Net posterior. /// Return the Bayes Net posterior.

View File

@ -40,7 +40,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
bool manyModes = false) { bool manyModes = false) {
HybridBayesNet bayesNet; HybridBayesNet bayesNet;
// Create Gaussian mixture z_i = x0 + noise for each measurement. // Create hybrid Gaussian factor z_i = x0 + noise for each measurement.
for (size_t i = 0; i < num_measurements; i++) { for (size_t i = 0; i < num_measurements; i++) {
const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
std::vector<GaussianConditional::shared_ptr> conditionals{ std::vector<GaussianConditional::shared_ptr> conditionals{

View File

@ -44,8 +44,8 @@ TEST(HybridConditional, Invariants) {
CHECK(hc0->isHybrid()); CHECK(hc0->isHybrid());
// Check invariants as a HybridGaussianConditional. // Check invariants as a HybridGaussianConditional.
const auto mixture = hc0->asMixture(); const auto conditional = hc0->asMixture();
EXPECT(HybridGaussianConditional::CheckInvariants(*mixture, values)); EXPECT(HybridGaussianConditional::CheckInvariants(*conditional, values));
// Check invariants as a HybridConditional. // Check invariants as a HybridConditional.
EXPECT(HybridConditional::CheckInvariants(*hc0, values)); EXPECT(HybridConditional::CheckInvariants(*hc0, values));

View File

@ -429,7 +429,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_shared<PriorFactor<double>>(X(1), 1.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(1), 1.0, noise_model);
// Add mixture factor: // Add hybrid nonlinear factor:
DiscreteKey m(M(0), 2); DiscreteKey m(M(0), 2);
const auto zero_motion = const auto zero_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);

View File

@ -50,7 +50,7 @@ TEST(HybridFactorGraph, Keys) {
// Add factor between x0 and x1 // Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Add a gaussian mixture factor ϕ(x1, c1) // Add a hybrid Gaussian factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2); DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactorValuePair> dt( DecisionTree<Key, GaussianFactorValuePair> dt(
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0}, M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},

View File

@ -52,9 +52,8 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
commonSigma), commonSigma),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
commonSigma)}; commonSigma)};
const HybridGaussianConditional mixture( const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode,
{Z(0)}, {X(0)}, {mode}, conditionals);
HybridGaussianConditional::Conditionals({mode}, conditionals));
} // namespace equal_constants } // namespace equal_constants
/* ************************************************************************* */ /* ************************************************************************* */
@ -62,21 +61,21 @@ const HybridGaussianConditional mixture(
TEST(HybridGaussianConditional, Invariants) { TEST(HybridGaussianConditional, Invariants) {
using namespace equal_constants; using namespace equal_constants;
// Check that the mixture normalization constant is the max of all constants // Check that the conditional normalization constant is the max of all
// which are all equal, in this case, hence: // constants which are all equal, in this case, hence:
const double K = mixture.logNormalizationConstant(); const double K = hybrid_conditional.logNormalizationConstant();
EXPECT_DOUBLES_EQUAL(K, conditionals[0]->logNormalizationConstant(), 1e-8); EXPECT_DOUBLES_EQUAL(K, conditionals[0]->logNormalizationConstant(), 1e-8);
EXPECT_DOUBLES_EQUAL(K, conditionals[1]->logNormalizationConstant(), 1e-8); EXPECT_DOUBLES_EQUAL(K, conditionals[1]->logNormalizationConstant(), 1e-8);
EXPECT(HybridGaussianConditional::CheckInvariants(mixture, hv0)); EXPECT(HybridGaussianConditional::CheckInvariants(hybrid_conditional, hv0));
EXPECT(HybridGaussianConditional::CheckInvariants(mixture, hv1)); EXPECT(HybridGaussianConditional::CheckInvariants(hybrid_conditional, hv1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
/// Check LogProbability. /// Check LogProbability.
TEST(HybridGaussianConditional, LogProbability) { TEST(HybridGaussianConditional, LogProbability) {
using namespace equal_constants; using namespace equal_constants;
auto actual = mixture.logProbability(vv); auto actual = hybrid_conditional.logProbability(vv);
// Check result. // Check result.
std::vector<DiscreteKey> discrete_keys = {mode}; std::vector<DiscreteKey> discrete_keys = {mode};
@ -90,7 +89,7 @@ TEST(HybridGaussianConditional, LogProbability) {
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL(conditionals[mode]->logProbability(vv), EXPECT_DOUBLES_EQUAL(conditionals[mode]->logProbability(vv),
mixture.logProbability(hv), 1e-8); hybrid_conditional.logProbability(hv), 1e-8);
} }
} }
@ -98,7 +97,7 @@ TEST(HybridGaussianConditional, LogProbability) {
/// Check error. /// Check error.
TEST(HybridGaussianConditional, Error) { TEST(HybridGaussianConditional, Error) {
using namespace equal_constants; using namespace equal_constants;
auto actual = mixture.errorTree(vv); auto actual = hybrid_conditional.errorTree(vv);
// Check result. // Check result.
std::vector<DiscreteKey> discrete_keys = {mode}; std::vector<DiscreteKey> discrete_keys = {mode};
@ -111,8 +110,8 @@ TEST(HybridGaussianConditional, Error) {
// Check for non-tree version. // Check for non-tree version.
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv), mixture.error(hv), EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv),
1e-8); hybrid_conditional.error(hv), 1e-8);
} }
} }
@ -123,11 +122,14 @@ TEST(HybridGaussianConditional, Likelihood) {
using namespace equal_constants; using namespace equal_constants;
// Compute likelihood // Compute likelihood
auto likelihood = mixture.likelihood(vv); auto likelihood = hybrid_conditional.likelihood(vv);
// Check that the mixture error and the likelihood error are the same. // Check that the hybrid conditional error and the likelihood error are the
EXPECT_DOUBLES_EQUAL(mixture.error(hv0), likelihood->error(hv0), 1e-8); // same.
EXPECT_DOUBLES_EQUAL(mixture.error(hv1), likelihood->error(hv1), 1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0),
1e-8);
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
1e-8);
// Check that likelihood error is as expected, i.e., just the errors of the // Check that likelihood error is as expected, i.e., just the errors of the
// individual likelihoods, in the `equal_constants` case. // individual likelihoods, in the `equal_constants` case.
@ -141,7 +143,8 @@ TEST(HybridGaussianConditional, Likelihood) {
std::vector<double> ratio(2); std::vector<double> ratio(2);
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
ratio[mode] = std::exp(-likelihood->error(hv)) / mixture.evaluate(hv); ratio[mode] =
std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
} }
EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
} }
@ -155,16 +158,15 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
0.5), 0.5),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
3.0)}; 3.0)};
const HybridGaussianConditional mixture( const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode,
{Z(0)}, {X(0)}, {mode}, conditionals);
HybridGaussianConditional::Conditionals({mode}, conditionals));
} // namespace mode_dependent_constants } // namespace mode_dependent_constants
/* ************************************************************************* */ /* ************************************************************************* */
// Create a test for continuousParents. // Create a test for continuousParents.
TEST(HybridGaussianConditional, ContinuousParents) { TEST(HybridGaussianConditional, ContinuousParents) {
using namespace mode_dependent_constants; using namespace mode_dependent_constants;
const KeyVector continuousParentKeys = mixture.continuousParents(); const KeyVector continuousParentKeys = hybrid_conditional.continuousParents();
// Check that the continuous parent keys are correct: // Check that the continuous parent keys are correct:
EXPECT(continuousParentKeys.size() == 1); EXPECT(continuousParentKeys.size() == 1);
EXPECT(continuousParentKeys[0] == X(0)); EXPECT(continuousParentKeys[0] == X(0));
@ -177,12 +179,14 @@ TEST(HybridGaussianConditional, Likelihood2) {
using namespace mode_dependent_constants; using namespace mode_dependent_constants;
// Compute likelihood // Compute likelihood
auto likelihood = mixture.likelihood(vv); auto likelihood = hybrid_conditional.likelihood(vv);
// Check that the mixture error and the likelihood error are as expected, // Check that the hybrid conditional error and the likelihood error are as
// this invariant is the same as the equal noise case: // expected, this invariant is the same as the equal noise case:
EXPECT_DOUBLES_EQUAL(mixture.error(hv0), likelihood->error(hv0), 1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0),
EXPECT_DOUBLES_EQUAL(mixture.error(hv1), likelihood->error(hv1), 1e-8); 1e-8);
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
1e-8);
// Check the detailed JacobianFactor calculation for mode==1. // Check the detailed JacobianFactor calculation for mode==1.
{ {
@ -195,7 +199,7 @@ TEST(HybridGaussianConditional, Likelihood2) {
CHECK(jf1->rows() == 2); CHECK(jf1->rows() == 2);
// Check that the constant C1 is properly encoded in the JacobianFactor. // Check that the constant C1 is properly encoded in the JacobianFactor.
const double C1 = mixture.logNormalizationConstant() - const double C1 = hybrid_conditional.logNormalizationConstant() -
conditionals[1]->logNormalizationConstant(); conditionals[1]->logNormalizationConstant();
const double c1 = std::sqrt(2.0 * C1); const double c1 = std::sqrt(2.0 * C1);
Vector expected_unwhitened(2); Vector expected_unwhitened(2);
@ -209,15 +213,16 @@ TEST(HybridGaussianConditional, Likelihood2) {
Vector actual_whitened = jf1->error_vector(vv); Vector actual_whitened = jf1->error_vector(vv);
EXPECT(assert_equal(expected_whitened, actual_whitened)); EXPECT(assert_equal(expected_whitened, actual_whitened));
// Check that the error is equal to the mixture error: // Check that the error is equal to the conditional error:
EXPECT_DOUBLES_EQUAL(mixture.error(hv1), jf1->error(hv1), 1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), jf1->error(hv1), 1e-8);
} }
// Check that the ratio of probPrime to evaluate is the same for all modes. // Check that the ratio of probPrime to evaluate is the same for all modes.
std::vector<double> ratio(2); std::vector<double> ratio(2);
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
ratio[mode] = std::exp(-likelihood->error(hv)) / mixture.evaluate(hv); ratio[mode] =
std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
} }
EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
} }

View File

@ -45,7 +45,7 @@ using symbol_shorthand::X;
using symbol_shorthand::Z; using symbol_shorthand::Z;
/* ************************************************************************* */ /* ************************************************************************* */
// Check iterators of empty mixture. // Check iterators of empty hybrid factor.
TEST(HybridGaussianFactor, Constructor) { TEST(HybridGaussianFactor, Constructor) {
HybridGaussianFactor factor; HybridGaussianFactor factor;
HybridGaussianFactor::const_iterator const_it = factor.begin(); HybridGaussianFactor::const_iterator const_it = factor.begin();
@ -55,7 +55,7 @@ TEST(HybridGaussianFactor, Constructor) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// "Add" two mixture factors together. // "Add" two hybrid factors together.
TEST(HybridGaussianFactor, Sum) { TEST(HybridGaussianFactor, Sum) {
DiscreteKey m1(1, 2), m2(2, 3); DiscreteKey m1(1, 2), m2(2, 3);
@ -78,20 +78,20 @@ TEST(HybridGaussianFactor, Sum) {
// TODO(Frank): why specify keys at all? And: keys in factor should be *all* // 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? // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
// Design review! // Design review!
HybridGaussianFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA); HybridGaussianFactor hybridFactorA({X(1), X(2)}, {m1}, factorsA);
HybridGaussianFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB); HybridGaussianFactor hybridFactorB({X(1), X(3)}, {m2}, factorsB);
// Check that number of keys is 3 // Check that number of keys is 3
EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size()); EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size());
// Check that number of discrete keys is 1 // Check that number of discrete keys is 1
EXPECT_LONGS_EQUAL(1, mixtureFactorA.discreteKeys().size()); EXPECT_LONGS_EQUAL(1, hybridFactorA.discreteKeys().size());
// Create sum of two mixture factors: it will be a decision tree now on both // Create sum of two hybrid factors: it will be a decision tree now on both
// discrete variables m1 and m2: // discrete variables m1 and m2:
GaussianFactorGraphTree sum; GaussianFactorGraphTree sum;
sum += mixtureFactorA; sum += hybridFactorA;
sum += mixtureFactorB; sum += hybridFactorB;
// Let's check that this worked: // Let's check that this worked:
Assignment<Key> mode; Assignment<Key> mode;
@ -112,7 +112,7 @@ TEST(HybridGaussianFactor, Printing) {
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b); auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
std::vector<GaussianFactorValuePair> factors{{f10, 0.0}, {f11, 0.0}}; std::vector<GaussianFactorValuePair> factors{{f10, 0.0}, {f11, 0.0}};
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors);
std::string expected = std::string expected =
R"(HybridGaussianFactor R"(HybridGaussianFactor
@ -144,7 +144,7 @@ Hybrid [x1 x2; 1]{
} }
)"; )";
EXPECT(assert_print_equal(expected, mixtureFactor)); EXPECT(assert_print_equal(expected, hybridFactor));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -181,7 +181,7 @@ TEST(HybridGaussianFactor, Error) {
auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b); auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}}; std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors);
VectorValues continuousValues; VectorValues continuousValues;
continuousValues.insert(X(1), Vector2(0, 0)); continuousValues.insert(X(1), Vector2(0, 0));
@ -189,7 +189,7 @@ TEST(HybridGaussianFactor, Error) {
// error should return a tree of errors, with nodes for each discrete value. // error should return a tree of errors, with nodes for each discrete value.
AlgebraicDecisionTree<Key> error_tree = AlgebraicDecisionTree<Key> error_tree =
mixtureFactor.errorTree(continuousValues); hybridFactor.errorTree(continuousValues);
std::vector<DiscreteKey> discrete_keys = {m1}; std::vector<DiscreteKey> discrete_keys = {m1};
// Error values for regression test // Error values for regression test
@ -202,7 +202,7 @@ TEST(HybridGaussianFactor, Error) {
DiscreteValues discreteValues; DiscreteValues discreteValues;
discreteValues[m1.first] = 1; discreteValues[m1.first] = 1;
EXPECT_DOUBLES_EQUAL( EXPECT_DOUBLES_EQUAL(
4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); 4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9);
} }
namespace test_gmm { namespace test_gmm {

View File

@ -69,8 +69,8 @@ TEST(HybridGaussianFactorGraph, Creation) {
hfg.emplace_shared<JacobianFactor>(X(0), I_3x3, Z_3x1); hfg.emplace_shared<JacobianFactor>(X(0), I_3x3, Z_3x1);
// Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // Define a hybrid gaussian conditional P(x0|x1, c0)
// graph // and add it to the factor graph.
HybridGaussianConditional gm( HybridGaussianConditional gm(
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}),
HybridGaussianConditional::Conditionals( HybridGaussianConditional::Conditionals(
@ -125,7 +125,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
// Add factor between x0 and x1 // Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Add a gaussian mixture factor ϕ(x1, c1) // Add a hybrid gaussian factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2); DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactorValuePair> dt( DecisionTree<Key, GaussianFactorValuePair> dt(
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0}, M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
@ -720,9 +720,9 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
// Create expected decision tree with two factor graphs: // Create expected decision tree with two factor graphs:
// Get mixture factor: // Get hybrid factor:
auto mixture = fg.at<HybridGaussianFactor>(0); auto hybrid = fg.at<HybridGaussianFactor>(0);
CHECK(mixture); CHECK(hybrid);
// Get prior factor: // Get prior factor:
const auto gf = fg.at<HybridConditional>(1); const auto gf = fg.at<HybridConditional>(1);
@ -737,8 +737,8 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
// Expected decision tree with two factor graphs: // Expected decision tree with two factor graphs:
// f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0)
GaussianFactorGraphTree expected{ GaussianFactorGraphTree expected{
M(0), GaussianFactorGraph(std::vector<GF>{(*mixture)(d0), prior}), M(0), GaussianFactorGraph(std::vector<GF>{(*hybrid)(d0), prior}),
GaussianFactorGraph(std::vector<GF>{(*mixture)(d1), prior})}; GaussianFactorGraph(std::vector<GF>{(*hybrid)(d1), prior})};
EXPECT(assert_equal(expected(d0), actual(d0), 1e-5)); EXPECT(assert_equal(expected(d0), actual(d0), 1e-5));
EXPECT(assert_equal(expected(d1), actual(d1), 1e-5)); EXPECT(assert_equal(expected(d1), actual(d1), 1e-5));
@ -802,7 +802,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
// Create expected Bayes Net: // Create expected Bayes Net:
HybridBayesNet expectedBayesNet; HybridBayesNet expectedBayesNet;
// Create Gaussian mixture on X(0). // Create hybrid Gaussian factor on X(0).
using tiny::mode; using tiny::mode;
// regression, but mean checked to be 5.0 in both cases: // regression, but mean checked to be 5.0 in both cases:
const auto conditional0 = std::make_shared<GaussianConditional>( const auto conditional0 = std::make_shared<GaussianConditional>(
@ -835,7 +835,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
const DiscreteKey mode{M(0), 2}; const DiscreteKey mode{M(0), 2};
HybridBayesNet bn; HybridBayesNet bn;
// Create Gaussian mixture z_0 = x0 + noise for each measurement. // Create hybrid Gaussian factor z_0 = x0 + noise for each measurement.
std::vector<GaussianConditional::shared_ptr> conditionals{ 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, 3),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)}; GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)};
@ -863,7 +863,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
// Create expected Bayes Net: // Create expected Bayes Net:
HybridBayesNet expectedBayesNet; HybridBayesNet expectedBayesNet;
// Create Gaussian mixture on X(0). // Create hybrid Gaussian factor on X(0).
// regression, but mean checked to be 5.0 in both cases: // regression, but mean checked to be 5.0 in both cases:
const auto conditional0 = std::make_shared<GaussianConditional>( const auto conditional0 = std::make_shared<GaussianConditional>(
X(0), Vector1(10.1379), I_1x1 * 2.02759), X(0), Vector1(10.1379), I_1x1 * 2.02759),
@ -900,7 +900,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
// Create expected Bayes Net: // Create expected Bayes Net:
HybridBayesNet expectedBayesNet; HybridBayesNet expectedBayesNet;
// Create Gaussian mixture on X(0). // Create hybrid Gaussian factor on X(0).
using tiny::mode; using tiny::mode;
// regression, but mean checked to be 5.0 in both cases: // regression, but mean checked to be 5.0 in both cases:
const auto conditional0 = std::make_shared<GaussianConditional>( const auto conditional0 = std::make_shared<GaussianConditional>(
@ -953,7 +953,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
// Add measurements: // Add measurements:
for (size_t t : {0, 1, 2}) { for (size_t t : {0, 1, 2}) {
// Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t): // Create hybrid Gaussian factor on Z(t) conditioned on X(t) and mode N(t):
const auto noise_mode_t = DiscreteKey{N(t), 2}; const auto noise_mode_t = DiscreteKey{N(t), 2};
std::vector<GaussianConditional::shared_ptr> conditionals{ std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5), GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5),
@ -970,7 +970,8 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
// Add motion models: // Add motion models:
for (size_t t : {2, 1}) { for (size_t t : {2, 1}) {
// Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1): // Create hybrid Gaussian factor on X(t) conditioned on X(t-1)
// and mode M(t-1):
const auto motion_model_t = DiscreteKey{M(t), 2}; const auto motion_model_t = DiscreteKey{M(t), 2};
std::vector<GaussianConditional::shared_ptr> conditionals{ std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), Z_1x1, GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), Z_1x1,

View File

@ -422,9 +422,8 @@ TEST(HybridGaussianISAM, NonTrivial) {
noise_model); noise_model);
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = { std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
{moving, 0.0}, {still, 0.0}}; {moving, 0.0}, {still, 0.0}};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components); contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@ -462,9 +461,8 @@ TEST(HybridGaussianISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}}; components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components); contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@ -505,9 +503,8 @@ TEST(HybridGaussianISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}}; components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components); contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

View File

@ -35,7 +35,7 @@ using symbol_shorthand::M;
using symbol_shorthand::X; using symbol_shorthand::X;
/* ************************************************************************* */ /* ************************************************************************* */
// Check iterators of empty mixture. // Check iterators of empty hybrid factor.
TEST(HybridNonlinearFactor, Constructor) { TEST(HybridNonlinearFactor, Constructor) {
HybridNonlinearFactor factor; HybridNonlinearFactor factor;
HybridNonlinearFactor::const_iterator const_it = factor.begin(); HybridNonlinearFactor::const_iterator const_it = factor.begin();
@ -60,7 +60,7 @@ TEST(HybridNonlinearFactor, Printing) {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}}; std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors); HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, factors);
std::string expected = std::string expected =
R"(Hybrid [x1 x2; 1] R"(Hybrid [x1 x2; 1]
@ -69,7 +69,7 @@ HybridNonlinearFactor
0 Leaf Nonlinear factor on 2 keys 0 Leaf Nonlinear factor on 2 keys
1 Leaf Nonlinear factor on 2 keys 1 Leaf Nonlinear factor on 2 keys
)"; )";
EXPECT(assert_print_equal(expected, mixtureFactor)); EXPECT(assert_print_equal(expected, hybridFactor));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -94,14 +94,14 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
/* ************************************************************************* */ /* ************************************************************************* */
// Test the error of the HybridNonlinearFactor // Test the error of the HybridNonlinearFactor
TEST(HybridNonlinearFactor, Error) { TEST(HybridNonlinearFactor, Error) {
auto mixtureFactor = getHybridNonlinearFactor(); auto hybridFactor = getHybridNonlinearFactor();
Values continuousValues; Values continuousValues;
continuousValues.insert<double>(X(1), 0); continuousValues.insert<double>(X(1), 0);
continuousValues.insert<double>(X(2), 1); continuousValues.insert<double>(X(2), 1);
AlgebraicDecisionTree<Key> error_tree = AlgebraicDecisionTree<Key> error_tree =
mixtureFactor.errorTree(continuousValues); hybridFactor.errorTree(continuousValues);
DiscreteKey m1(1, 2); DiscreteKey m1(1, 2);
std::vector<DiscreteKey> discrete_keys = {m1}; std::vector<DiscreteKey> discrete_keys = {m1};
@ -114,8 +114,8 @@ TEST(HybridNonlinearFactor, Error) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test dim of the HybridNonlinearFactor // Test dim of the HybridNonlinearFactor
TEST(HybridNonlinearFactor, Dim) { TEST(HybridNonlinearFactor, Dim) {
auto mixtureFactor = getHybridNonlinearFactor(); auto hybridFactor = getHybridNonlinearFactor();
EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); EXPECT_LONGS_EQUAL(1, hybridFactor.dim());
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -281,7 +281,7 @@ TEST(HybridFactorGraph, EliminationTree) {
TEST(GaussianElimination, Eliminate_x0) { TEST(GaussianElimination, Eliminate_x0) {
Switching self(3); Switching self(3);
// Gather factors on x1, has a simple Gaussian and a mixture factor. // Gather factors on x1, has a simple Gaussian and a hybrid factor.
HybridGaussianFactorGraph factors; HybridGaussianFactorGraph factors;
// Add gaussian prior // Add gaussian prior
factors.push_back(self.linearizedFactorGraph[0]); factors.push_back(self.linearizedFactorGraph[0]);
@ -306,7 +306,7 @@ TEST(GaussianElimination, Eliminate_x0) {
TEST(HybridsGaussianElimination, Eliminate_x1) { TEST(HybridsGaussianElimination, Eliminate_x1) {
Switching self(3); Switching self(3);
// Gather factors on x1, will be two mixture factors (with x0 and x2, resp.). // Gather factors on x1, will be two hybrid factors (with x0 and x2, resp.).
HybridGaussianFactorGraph factors; HybridGaussianFactorGraph factors;
factors.push_back(self.linearizedFactorGraph[1]); // involves m0 factors.push_back(self.linearizedFactorGraph[1]); // involves m0
factors.push_back(self.linearizedFactorGraph[2]); // involves m1 factors.push_back(self.linearizedFactorGraph[2]); // involves m1
@ -349,18 +349,18 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
// Eliminate x0 // Eliminate x0
const Ordering ordering{X(0), X(1)}; const Ordering ordering{X(0), X(1)};
const auto [hybridConditionalMixture, factorOnModes] = const auto [hybridConditional, factorOnModes] =
EliminateHybrid(factors, ordering); EliminateHybrid(factors, ordering);
auto gaussianConditionalMixture = auto hybridGaussianConditional =
dynamic_pointer_cast<HybridGaussianConditional>( dynamic_pointer_cast<HybridGaussianConditional>(
hybridConditionalMixture->inner()); hybridConditional->inner());
CHECK(gaussianConditionalMixture); CHECK(hybridGaussianConditional);
// Frontals = [x0, x1] // Frontals = [x0, x1]
EXPECT_LONGS_EQUAL(2, gaussianConditionalMixture->nrFrontals()); EXPECT_LONGS_EQUAL(2, hybridGaussianConditional->nrFrontals());
// 1 parent, which is the mode // 1 parent, which is the mode
EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents()); EXPECT_LONGS_EQUAL(1, hybridGaussianConditional->nrParents());
// This is now a discreteFactor // This is now a discreteFactor
auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes); auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
@ -849,8 +849,8 @@ namespace test_relinearization {
* This way we can directly provide the likelihoods and * This way we can directly provide the likelihoods and
* then perform (re-)linearization. * then perform (re-)linearization.
* *
* @param means The means of the GaussianMixtureFactor components. * @param means The means of the HybridGaussianFactor components.
* @param sigmas The covariances of the GaussianMixtureFactor components. * @param sigmas The covariances of the HybridGaussianFactor components.
* @param m1 The discrete key. * @param m1 The discrete key.
* @param x0_measurement A measurement on X0 * @param x0_measurement A measurement on X0
* @return HybridGaussianFactorGraph * @return HybridGaussianFactorGraph

View File

@ -441,9 +441,8 @@ TEST(HybridNonlinearISAM, NonTrivial) {
noise_model); noise_model);
std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = { std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
{moving, 0.0}, {still, 0.0}}; {moving, 0.0}, {still, 0.0}};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(1), 2), components); contKeys, gtsam::DiscreteKey(M(1), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@ -481,9 +480,8 @@ TEST(HybridNonlinearISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}}; components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(2), 2), components); contKeys, gtsam::DiscreteKey(M(2), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@ -524,9 +522,8 @@ TEST(HybridNonlinearISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {{moving, 0.0}, {still, 0.0}}; components = {{moving, 0.0}, {still, 0.0}};
mixtureFactor = std::make_shared<HybridNonlinearFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, gtsam::DiscreteKey(M(3), 2), components); contKeys, gtsam::DiscreteKey(M(3), 2), components);
fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

View File

@ -46,9 +46,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
self.assertEqual(hbn.size(), 2) self.assertEqual(hbn.size(), 2)
mixture = hbn.at(0).inner() hybridCond = hbn.at(0).inner()
self.assertIsInstance(mixture, HybridGaussianConditional) self.assertIsInstance(hybridCond, HybridGaussianConditional)
self.assertEqual(len(mixture.keys()), 2) self.assertEqual(len(hybridCond.keys()), 2)
discrete_conditional = hbn.at(hbn.size() - 1).inner() discrete_conditional = hbn.at(hbn.size() - 1).inner()
self.assertIsInstance(discrete_conditional, DiscreteConditional) self.assertIsInstance(discrete_conditional, DiscreteConditional)
@ -90,7 +90,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
# Create mode key: 0 is low-noise, 1 is high-noise. # Create mode key: 0 is low-noise, 1 is high-noise.
mode = (M(0), 2) mode = (M(0), 2)
# Create Gaussian mixture Z(0) = X(0) + noise for each measurement. # Create hybrid Gaussian conditional Z(0) = X(0) + noise for each measurement.
I_1x1 = np.eye(1) I_1x1 = np.eye(1)
for i in range(num_measurements): for i in range(num_measurements):
conditional0 = GaussianConditional.FromMeanAndStddev(Z(i), conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),