Merge pull request #1830 from borglab/hybrid-renaming
Varun Agrawal
GitHub
commit
525ff7cc11
|
|
@ -191,17 +191,17 @@ E_{gc}(x,y)=\frac{1}{2}\|Rx+Sy-d\|_{\Sigma}^{2}.\label{eq:gc_error}
|
|||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
GaussianMixture
|
||||
HybridGaussianConditional
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
A
|
||||
\emph on
|
||||
GaussianMixture
|
||||
HybridGaussianConditional
|
||||
\emph default
|
||||
(maybe to be renamed to
|
||||
\emph on
|
||||
GaussianMixtureComponent
|
||||
HybridGaussianConditionalComponent
|
||||
\emph default
|
||||
) just indexes into a number of
|
||||
\emph on
|
||||
|
|
@ -233,7 +233,7 @@ GaussianConditional
|
|||
to a set of discrete variables.
|
||||
As
|
||||
\emph on
|
||||
GaussianMixture
|
||||
HybridGaussianConditional
|
||||
\emph default
|
||||
is a
|
||||
\emph on
|
||||
|
|
@ -324,7 +324,7 @@ The key point here is that
|
|||
\color inherit
|
||||
is the log-normalization constant for the complete
|
||||
\emph on
|
||||
GaussianMixture
|
||||
HybridGaussianConditional
|
||||
\emph default
|
||||
across all values of
|
||||
\begin_inset Formula $m$
|
||||
|
|
@ -548,15 +548,15 @@ with
|
|||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection*
|
||||
GaussianMixtureFactor
|
||||
HybridGaussianFactor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Analogously, a
|
||||
\emph on
|
||||
GaussianMixtureFactor
|
||||
HybridGaussianFactor
|
||||
\emph default
|
||||
typically results from a GaussianMixture by having known values
|
||||
typically results from a HybridGaussianConditional by having known values
|
||||
\begin_inset Formula $\bar{x}$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -817,7 +817,7 @@ E_{mf}(y,m)=\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=E_{gcm}(\bar{x},y)+K_
|
|||
|
||||
\end_inset
|
||||
|
||||
which is identical to the GaussianMixture error
|
||||
which is identical to the HybridGaussianConditional error
|
||||
\begin_inset CommandInset ref
|
||||
LatexCommand eqref
|
||||
reference "eq:gm_error"
|
||||
|
|
|
|||
|
|
@ -168,11 +168,11 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
|
|||
DecisionTreeFactor prunedDiscreteProbs =
|
||||
this->pruneDiscreteConditionals(maxNrLeaves);
|
||||
|
||||
/* To prune, we visitWith every leaf in the GaussianMixture.
|
||||
/* To prune, we visitWith every leaf in the HybridGaussianConditional.
|
||||
* For each leaf, using the assignment we can check the discrete decision tree
|
||||
* for 0.0 probability, then just set the leaf to a nullptr.
|
||||
*
|
||||
* We can later check the GaussianMixture for just nullptrs.
|
||||
* We can later check the HybridGaussianConditional for just nullptrs.
|
||||
*/
|
||||
|
||||
HybridBayesNet prunedBayesNetFragment;
|
||||
|
|
@ -182,14 +182,16 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
|
|||
for (auto &&conditional : *this) {
|
||||
if (auto gm = conditional->asMixture()) {
|
||||
// Make a copy of the Gaussian mixture and prune it!
|
||||
auto prunedGaussianMixture = std::make_shared<GaussianMixture>(*gm);
|
||||
prunedGaussianMixture->prune(prunedDiscreteProbs); // imperative :-(
|
||||
auto prunedHybridGaussianConditional =
|
||||
std::make_shared<HybridGaussianConditional>(*gm);
|
||||
prunedHybridGaussianConditional->prune(
|
||||
prunedDiscreteProbs); // imperative :-(
|
||||
|
||||
// Type-erase and add to the pruned Bayes Net fragment.
|
||||
prunedBayesNetFragment.push_back(prunedGaussianMixture);
|
||||
prunedBayesNetFragment.push_back(prunedHybridGaussianConditional);
|
||||
|
||||
} else {
|
||||
// Add the non-GaussianMixture conditional
|
||||
// Add the non-HybridGaussianConditional conditional
|
||||
prunedBayesNetFragment.push_back(conditional);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
|
|||
*
|
||||
* Example:
|
||||
* auto shared_ptr_to_a_conditional =
|
||||
* std::make_shared<GaussianMixture>(...);
|
||||
* std::make_shared<HybridGaussianConditional>(...);
|
||||
* hbn.push_back(shared_ptr_to_a_conditional);
|
||||
*/
|
||||
void push_back(HybridConditional &&conditional) {
|
||||
|
|
@ -106,7 +106,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
|
|||
* Preferred: Emplace a conditional directly using arguments.
|
||||
*
|
||||
* Examples:
|
||||
* hbn.emplace_shared<GaussianMixture>(...)));
|
||||
* hbn.emplace_shared<HybridGaussianConditional>(...)));
|
||||
* hbn.emplace_shared<GaussianConditional>(...)));
|
||||
* hbn.emplace_shared<DiscreteConditional>(...)));
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ HybridConditional::HybridConditional(
|
|||
|
||||
/* ************************************************************************ */
|
||||
HybridConditional::HybridConditional(
|
||||
const std::shared_ptr<GaussianMixture> &gaussianMixture)
|
||||
const std::shared_ptr<HybridGaussianConditional> &gaussianMixture)
|
||||
: BaseFactor(KeyVector(gaussianMixture->keys().begin(),
|
||||
gaussianMixture->keys().begin() +
|
||||
gaussianMixture->nrContinuous()),
|
||||
|
|
|
|||
|
|
@ -18,8 +18,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
|
@ -39,7 +39,7 @@ namespace gtsam {
|
|||
* As a type-erased variant of:
|
||||
* - DiscreteConditional
|
||||
* - GaussianConditional
|
||||
* - GaussianMixture
|
||||
* - HybridGaussianConditional
|
||||
*
|
||||
* The reason why this is important is that `Conditional<T>` is a CRTP class.
|
||||
* CRTP is static polymorphism such that all CRTP classes, while bearing the
|
||||
|
|
@ -127,7 +127,8 @@ class GTSAM_EXPORT HybridConditional
|
|||
* @param gaussianMixture Gaussian Mixture Conditional used to create the
|
||||
* HybridConditional.
|
||||
*/
|
||||
HybridConditional(const std::shared_ptr<GaussianMixture>& gaussianMixture);
|
||||
HybridConditional(
|
||||
const std::shared_ptr<HybridGaussianConditional>& gaussianMixture);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -146,12 +147,12 @@ class GTSAM_EXPORT HybridConditional
|
|||
/// @{
|
||||
|
||||
/**
|
||||
* @brief Return HybridConditional as a GaussianMixture
|
||||
* @brief Return HybridConditional as a HybridGaussianConditional
|
||||
* @return nullptr if not a mixture
|
||||
* @return GaussianMixture::shared_ptr otherwise
|
||||
* @return HybridGaussianConditional::shared_ptr otherwise
|
||||
*/
|
||||
GaussianMixture::shared_ptr asMixture() const {
|
||||
return std::dynamic_pointer_cast<GaussianMixture>(inner_);
|
||||
HybridGaussianConditional::shared_ptr asMixture() const {
|
||||
return std::dynamic_pointer_cast<HybridGaussianConditional>(inner_);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -222,8 +223,10 @@ class GTSAM_EXPORT HybridConditional
|
|||
boost::serialization::void_cast_register<GaussianConditional, Factor>(
|
||||
static_cast<GaussianConditional*>(NULL), static_cast<Factor*>(NULL));
|
||||
} else {
|
||||
boost::serialization::void_cast_register<GaussianMixture, Factor>(
|
||||
static_cast<GaussianMixture*>(NULL), static_cast<Factor*>(NULL));
|
||||
boost::serialization::void_cast_register<HybridGaussianConditional,
|
||||
Factor>(
|
||||
static_cast<HybridGaussianConditional*>(NULL),
|
||||
static_cast<Factor*>(NULL));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -45,9 +45,9 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
|
|||
* Base class for *truly* hybrid probabilistic factors
|
||||
*
|
||||
* Examples:
|
||||
* - MixtureFactor
|
||||
* - GaussianMixtureFactor
|
||||
* - GaussianMixture
|
||||
* - HybridNonlinearFactor
|
||||
* - HybridGaussianFactor
|
||||
* - HybridGaussianConditional
|
||||
*
|
||||
* @ingroup hybrid
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianMixture.cpp
|
||||
* @file HybridGaussianConditional.cpp
|
||||
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -20,8 +20,8 @@
|
|||
|
||||
#include <gtsam/base/utilities.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/inference/Conditional-inst.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
|
|
@ -29,10 +29,10 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
GaussianMixture::GaussianMixture(
|
||||
HybridGaussianConditional::HybridGaussianConditional(
|
||||
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const GaussianMixture::Conditionals &conditionals)
|
||||
const HybridGaussianConditional::Conditionals &conditionals)
|
||||
: BaseFactor(CollectKeys(continuousFrontals, continuousParents),
|
||||
discreteParents),
|
||||
BaseConditional(continuousFrontals.size()),
|
||||
|
|
@ -50,30 +50,33 @@ GaussianMixture::GaussianMixture(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
const GaussianMixture::Conditionals &GaussianMixture::conditionals() const {
|
||||
const HybridGaussianConditional::Conditionals &
|
||||
HybridGaussianConditional::conditionals() const {
|
||||
return conditionals_;
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixture::GaussianMixture(
|
||||
HybridGaussianConditional::HybridGaussianConditional(
|
||||
KeyVector &&continuousFrontals, KeyVector &&continuousParents,
|
||||
DiscreteKeys &&discreteParents,
|
||||
std::vector<GaussianConditional::shared_ptr> &&conditionals)
|
||||
: GaussianMixture(continuousFrontals, continuousParents, discreteParents,
|
||||
: HybridGaussianConditional(continuousFrontals, continuousParents,
|
||||
discreteParents,
|
||||
Conditionals(discreteParents, conditionals)) {}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixture::GaussianMixture(
|
||||
HybridGaussianConditional::HybridGaussianConditional(
|
||||
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const std::vector<GaussianConditional::shared_ptr> &conditionals)
|
||||
: GaussianMixture(continuousFrontals, continuousParents, discreteParents,
|
||||
: HybridGaussianConditional(continuousFrontals, continuousParents,
|
||||
discreteParents,
|
||||
Conditionals(discreteParents, conditionals)) {}
|
||||
|
||||
/* *******************************************************************************/
|
||||
// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from
|
||||
// GaussianMixtureFactor, no?
|
||||
GaussianFactorGraphTree GaussianMixture::add(
|
||||
// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
|
||||
// derived from HybridGaussianFactor, no?
|
||||
GaussianFactorGraphTree HybridGaussianConditional::add(
|
||||
const GaussianFactorGraphTree &sum) const {
|
||||
using Y = GaussianFactorGraph;
|
||||
auto add = [](const Y &graph1, const Y &graph2) {
|
||||
|
|
@ -86,7 +89,8 @@ GaussianFactorGraphTree GaussianMixture::add(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
|
||||
GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
|
||||
const {
|
||||
auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
|
||||
// First check if conditional has not been pruned
|
||||
if (gc) {
|
||||
|
|
@ -109,7 +113,7 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
size_t GaussianMixture::nrComponents() const {
|
||||
size_t HybridGaussianConditional::nrComponents() const {
|
||||
size_t total = 0;
|
||||
conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) {
|
||||
if (node) total += 1;
|
||||
|
|
@ -118,7 +122,7 @@ size_t GaussianMixture::nrComponents() const {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianConditional::shared_ptr GaussianMixture::operator()(
|
||||
GaussianConditional::shared_ptr HybridGaussianConditional::operator()(
|
||||
const DiscreteValues &discreteValues) const {
|
||||
auto &ptr = conditionals_(discreteValues);
|
||||
if (!ptr) return nullptr;
|
||||
|
|
@ -127,11 +131,12 @@ GaussianConditional::shared_ptr GaussianMixture::operator()(
|
|||
return conditional;
|
||||
else
|
||||
throw std::logic_error(
|
||||
"A GaussianMixture unexpectedly contained a non-conditional");
|
||||
"A HybridGaussianConditional unexpectedly contained a non-conditional");
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
|
||||
bool HybridGaussianConditional::equals(const HybridFactor &lf,
|
||||
double tol) const {
|
||||
const This *e = dynamic_cast<const This *>(&lf);
|
||||
if (e == nullptr) return false;
|
||||
|
||||
|
|
@ -149,7 +154,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
void GaussianMixture::print(const std::string &s,
|
||||
void HybridGaussianConditional::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << (s.empty() ? "" : s + "\n");
|
||||
if (isContinuous()) std::cout << "Continuous ";
|
||||
|
|
@ -177,7 +182,7 @@ void GaussianMixture::print(const std::string &s,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KeyVector GaussianMixture::continuousParents() const {
|
||||
KeyVector HybridGaussianConditional::continuousParents() const {
|
||||
// Get all parent keys:
|
||||
const auto range = parents();
|
||||
KeyVector continuousParentKeys(range.begin(), range.end());
|
||||
|
|
@ -193,7 +198,8 @@ KeyVector GaussianMixture::continuousParents() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const {
|
||||
bool HybridGaussianConditional::allFrontalsGiven(
|
||||
const VectorValues &given) const {
|
||||
for (auto &&kv : given) {
|
||||
if (given.find(kv.first) == given.end()) {
|
||||
return false;
|
||||
|
|
@ -203,16 +209,17 @@ bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
|
||||
std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
|
||||
const VectorValues &given) const {
|
||||
if (!allFrontalsGiven(given)) {
|
||||
throw std::runtime_error(
|
||||
"GaussianMixture::likelihood: given values are missing some frontals.");
|
||||
"HybridGaussianConditional::likelihood: given values are missing some "
|
||||
"frontals.");
|
||||
}
|
||||
|
||||
const DiscreteKeys discreteParentKeys = discreteKeys();
|
||||
const KeyVector continuousParentKeys = continuousParents();
|
||||
const GaussianMixtureFactor::Factors likelihoods(
|
||||
const HybridGaussianFactor::Factors likelihoods(
|
||||
conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
|
||||
const auto likelihood_m = conditional->likelihood(given);
|
||||
const double Cgm_Kgcm =
|
||||
|
|
@ -231,7 +238,7 @@ std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
|
|||
return std::make_shared<JacobianFactor>(gfg);
|
||||
}
|
||||
});
|
||||
return std::make_shared<GaussianMixtureFactor>(
|
||||
return std::make_shared<HybridGaussianFactor>(
|
||||
continuousParentKeys, discreteParentKeys, likelihoods);
|
||||
}
|
||||
|
||||
|
|
@ -252,7 +259,7 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
|
|||
*/
|
||||
std::function<GaussianConditional::shared_ptr(
|
||||
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
|
||||
GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) {
|
||||
HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) {
|
||||
// Get the discrete keys as sets for the decision tree
|
||||
// and the gaussian mixture.
|
||||
auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys());
|
||||
|
|
@ -303,7 +310,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) {
|
||||
void HybridGaussianConditional::prune(const DecisionTreeFactor &discreteProbs) {
|
||||
// Functional which loops over all assignments and create a set of
|
||||
// GaussianConditionals
|
||||
auto pruner = prunerFunc(discreteProbs);
|
||||
|
|
@ -313,7 +320,7 @@ void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
|
||||
AlgebraicDecisionTree<Key> HybridGaussianConditional::logProbability(
|
||||
const VectorValues &continuousValues) const {
|
||||
// functor to calculate (double) logProbability value from
|
||||
// GaussianConditional.
|
||||
|
|
@ -331,7 +338,7 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double GaussianMixture::conditionalError(
|
||||
double HybridGaussianConditional::conditionalError(
|
||||
const GaussianConditional::shared_ptr &conditional,
|
||||
const VectorValues &continuousValues) const {
|
||||
// Check if valid pointer
|
||||
|
|
@ -348,7 +355,7 @@ double GaussianMixture::conditionalError(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
|
||||
AlgebraicDecisionTree<Key> HybridGaussianConditional::errorTree(
|
||||
const VectorValues &continuousValues) const {
|
||||
auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
|
||||
return conditionalError(conditional, continuousValues);
|
||||
|
|
@ -358,20 +365,21 @@ AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
double GaussianMixture::error(const HybridValues &values) const {
|
||||
double HybridGaussianConditional::error(const HybridValues &values) const {
|
||||
// Directly index to get the conditional, no need to build the whole tree.
|
||||
auto conditional = conditionals_(values.discrete());
|
||||
return conditionalError(conditional, values.continuous());
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
double GaussianMixture::logProbability(const HybridValues &values) const {
|
||||
double HybridGaussianConditional::logProbability(
|
||||
const HybridValues &values) const {
|
||||
auto conditional = conditionals_(values.discrete());
|
||||
return conditional->logProbability(values.continuous());
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
double GaussianMixture::evaluate(const HybridValues &values) const {
|
||||
double HybridGaussianConditional::evaluate(const HybridValues &values) const {
|
||||
auto conditional = conditionals_(values.discrete());
|
||||
return conditional->evaluate(values.continuous());
|
||||
}
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianMixture.h
|
||||
* @file HybridGaussianConditional.h
|
||||
* @brief A hybrid conditional in the Conditional Linear Gaussian scheme
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -23,8 +23,8 @@
|
|||
#include <gtsam/discrete/DecisionTree.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
||||
|
|
@ -50,14 +50,14 @@ class HybridValues;
|
|||
*
|
||||
* @ingroup hybrid
|
||||
*/
|
||||
class GTSAM_EXPORT GaussianMixture
|
||||
class GTSAM_EXPORT HybridGaussianConditional
|
||||
: public HybridFactor,
|
||||
public Conditional<HybridFactor, GaussianMixture> {
|
||||
public Conditional<HybridFactor, HybridGaussianConditional> {
|
||||
public:
|
||||
using This = GaussianMixture;
|
||||
using shared_ptr = std::shared_ptr<GaussianMixture>;
|
||||
using This = HybridGaussianConditional;
|
||||
using shared_ptr = std::shared_ptr<HybridGaussianConditional>;
|
||||
using BaseFactor = HybridFactor;
|
||||
using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
|
||||
using BaseConditional = Conditional<HybridFactor, HybridGaussianConditional>;
|
||||
|
||||
/// typedef for Decision Tree of Gaussian Conditionals
|
||||
using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
|
||||
|
|
@ -67,7 +67,7 @@ class GTSAM_EXPORT GaussianMixture
|
|||
double logConstant_; ///< log of the normalization constant.
|
||||
|
||||
/**
|
||||
* @brief Convert a GaussianMixture of conditionals into
|
||||
* @brief Convert a HybridGaussianConditional of conditionals into
|
||||
* a DecisionTree of Gaussian factor graphs.
|
||||
*/
|
||||
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
|
||||
|
|
@ -88,10 +88,10 @@ class GTSAM_EXPORT GaussianMixture
|
|||
/// @{
|
||||
|
||||
/// Default constructor, mainly for serialization.
|
||||
GaussianMixture() = default;
|
||||
HybridGaussianConditional() = default;
|
||||
|
||||
/**
|
||||
* @brief Construct a new GaussianMixture object.
|
||||
* @brief Construct a new HybridGaussianConditional object.
|
||||
*
|
||||
* @param continuousFrontals the continuous frontals.
|
||||
* @param continuousParents the continuous parents.
|
||||
|
|
@ -101,7 +101,7 @@ class GTSAM_EXPORT GaussianMixture
|
|||
* cardinality of the DiscreteKeys in discreteParents, since the
|
||||
* discreteParents will be used as the labels in the decision tree.
|
||||
*/
|
||||
GaussianMixture(const KeyVector &continuousFrontals,
|
||||
HybridGaussianConditional(const KeyVector &continuousFrontals,
|
||||
const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const Conditionals &conditionals);
|
||||
|
|
@ -114,7 +114,8 @@ class GTSAM_EXPORT GaussianMixture
|
|||
* @param discreteParents Discrete parents variables
|
||||
* @param conditionals List of conditionals
|
||||
*/
|
||||
GaussianMixture(KeyVector &&continuousFrontals, KeyVector &&continuousParents,
|
||||
HybridGaussianConditional(
|
||||
KeyVector &&continuousFrontals, KeyVector &&continuousParents,
|
||||
DiscreteKeys &&discreteParents,
|
||||
std::vector<GaussianConditional::shared_ptr> &&conditionals);
|
||||
|
||||
|
|
@ -126,7 +127,7 @@ class GTSAM_EXPORT GaussianMixture
|
|||
* @param discreteParents Discrete parents variables
|
||||
* @param conditionals List of conditionals
|
||||
*/
|
||||
GaussianMixture(
|
||||
HybridGaussianConditional(
|
||||
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
|
||||
const DiscreteKeys &discreteParents,
|
||||
const std::vector<GaussianConditional::shared_ptr> &conditionals);
|
||||
|
|
@ -140,7 +141,7 @@ class GTSAM_EXPORT GaussianMixture
|
|||
|
||||
/// Print utility
|
||||
void print(
|
||||
const std::string &s = "GaussianMixture\n",
|
||||
const std::string &s = "HybridGaussianConditional\n",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// @}
|
||||
|
|
@ -165,14 +166,14 @@ class GTSAM_EXPORT GaussianMixture
|
|||
* Create a likelihood factor for a Gaussian mixture, return a Mixture factor
|
||||
* on the parents.
|
||||
*/
|
||||
std::shared_ptr<GaussianMixtureFactor> likelihood(
|
||||
std::shared_ptr<HybridGaussianFactor> likelihood(
|
||||
const VectorValues &given) const;
|
||||
|
||||
/// Getter for the underlying Conditionals DecisionTree
|
||||
const Conditionals &conditionals() const;
|
||||
|
||||
/**
|
||||
* @brief Compute logProbability of the GaussianMixture as a tree.
|
||||
* @brief Compute logProbability of the HybridGaussianConditional as a tree.
|
||||
*
|
||||
* @param continuousValues The continuous VectorValues.
|
||||
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
|
||||
|
|
@ -209,7 +210,7 @@ class GTSAM_EXPORT GaussianMixture
|
|||
double error(const HybridValues &values) const override;
|
||||
|
||||
/**
|
||||
* @brief Compute error of the GaussianMixture as a tree.
|
||||
* @brief Compute error of the HybridGaussianConditional as a tree.
|
||||
*
|
||||
* @param continuousValues The continuous VectorValues.
|
||||
* @return AlgebraicDecisionTree<Key> A decision tree on the discrete keys
|
||||
|
|
@ -277,6 +278,7 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
|
|||
|
||||
// traits
|
||||
template <>
|
||||
struct traits<GaussianMixture> : public Testable<GaussianMixture> {};
|
||||
struct traits<HybridGaussianConditional>
|
||||
: public Testable<HybridGaussianConditional> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianMixtureFactor.cpp
|
||||
* @file HybridGaussianFactor.cpp
|
||||
* @brief A set of Gaussian factors indexed by a set of discrete keys.
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -21,7 +21,7 @@
|
|||
#include <gtsam/base/utilities.h>
|
||||
#include <gtsam/discrete/DecisionTree-inl.h>
|
||||
#include <gtsam/discrete/DecisionTree.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
|
@ -29,13 +29,13 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
|
||||
HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys,
|
||||
const Factors &factors)
|
||||
: Base(continuousKeys, discreteKeys), factors_(factors) {}
|
||||
|
||||
/* *******************************************************************************/
|
||||
bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
|
||||
bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
|
||||
const This *e = dynamic_cast<const This *>(&lf);
|
||||
if (e == nullptr) return false;
|
||||
|
||||
|
|
@ -52,10 +52,10 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
void GaussianMixtureFactor::print(const std::string &s,
|
||||
void HybridGaussianFactor::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << (s.empty() ? "" : s + "\n");
|
||||
std::cout << "GaussianMixtureFactor" << std::endl;
|
||||
std::cout << "HybridGaussianFactor" << std::endl;
|
||||
HybridFactor::print("", formatter);
|
||||
std::cout << "{\n";
|
||||
if (factors_.empty()) {
|
||||
|
|
@ -78,13 +78,13 @@ void GaussianMixtureFactor::print(const std::string &s,
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixtureFactor::sharedFactor GaussianMixtureFactor::operator()(
|
||||
HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()(
|
||||
const DiscreteValues &assignment) const {
|
||||
return factors_(assignment);
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianFactorGraphTree GaussianMixtureFactor::add(
|
||||
GaussianFactorGraphTree HybridGaussianFactor::add(
|
||||
const GaussianFactorGraphTree &sum) const {
|
||||
using Y = GaussianFactorGraph;
|
||||
auto add = [](const Y &graph1, const Y &graph2) {
|
||||
|
|
@ -97,14 +97,14 @@ GaussianFactorGraphTree GaussianMixtureFactor::add(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianFactorGraphTree GaussianMixtureFactor::asGaussianFactorGraphTree()
|
||||
GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
|
||||
const {
|
||||
auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; };
|
||||
return {factors_, wrap};
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
AlgebraicDecisionTree<Key> GaussianMixtureFactor::errorTree(
|
||||
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
|
||||
const VectorValues &continuousValues) const {
|
||||
// functor to convert from sharedFactor to double error value.
|
||||
auto errorFunc = [&continuousValues](const sharedFactor &gf) {
|
||||
|
|
@ -115,7 +115,7 @@ AlgebraicDecisionTree<Key> GaussianMixtureFactor::errorTree(
|
|||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
double GaussianMixtureFactor::error(const HybridValues &values) const {
|
||||
double HybridGaussianFactor::error(const HybridValues &values) const {
|
||||
const sharedFactor gf = factors_(values.discrete());
|
||||
return gf->error(values.continuous());
|
||||
}
|
||||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianMixtureFactor.h
|
||||
* @file HybridGaussianFactor.h
|
||||
* @brief A set of GaussianFactors, indexed by a set of discrete keys.
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -44,10 +44,10 @@ class VectorValues;
|
|||
*
|
||||
* @ingroup hybrid
|
||||
*/
|
||||
class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
||||
class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
||||
public:
|
||||
using Base = HybridFactor;
|
||||
using This = GaussianMixtureFactor;
|
||||
using This = HybridGaussianFactor;
|
||||
using shared_ptr = std::shared_ptr<This>;
|
||||
|
||||
using sharedFactor = std::shared_ptr<GaussianFactor>;
|
||||
|
|
@ -72,7 +72,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
/// @{
|
||||
|
||||
/// Default constructor, mainly for serialization.
|
||||
GaussianMixtureFactor() = default;
|
||||
HybridGaussianFactor() = default;
|
||||
|
||||
/**
|
||||
* @brief Construct a new Gaussian mixture factor.
|
||||
|
|
@ -83,22 +83,22 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
* @param factors The decision tree of Gaussian factors stored
|
||||
* as the mixture density.
|
||||
*/
|
||||
GaussianMixtureFactor(const KeyVector &continuousKeys,
|
||||
HybridGaussianFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys,
|
||||
const Factors &factors);
|
||||
|
||||
/**
|
||||
* @brief Construct a new GaussianMixtureFactor object using a vector of
|
||||
* @brief Construct a new HybridGaussianFactor object using a vector of
|
||||
* GaussianFactor shared pointers.
|
||||
*
|
||||
* @param continuousKeys Vector of keys for continuous factors.
|
||||
* @param discreteKeys Vector of discrete keys.
|
||||
* @param factors Vector of gaussian factor shared pointers.
|
||||
*/
|
||||
GaussianMixtureFactor(const KeyVector &continuousKeys,
|
||||
HybridGaussianFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys,
|
||||
const std::vector<sharedFactor> &factors)
|
||||
: GaussianMixtureFactor(continuousKeys, discreteKeys,
|
||||
: HybridGaussianFactor(continuousKeys, discreteKeys,
|
||||
Factors(discreteKeys, factors)) {}
|
||||
|
||||
/// @}
|
||||
|
|
@ -128,7 +128,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
|
||||
|
||||
/**
|
||||
* @brief Compute error of the GaussianMixtureFactor as a tree.
|
||||
* @brief Compute error of the HybridGaussianFactor as a tree.
|
||||
*
|
||||
* @param continuousValues The continuous VectorValues.
|
||||
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
|
||||
|
|
@ -146,9 +146,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
/// Getter for GaussianFactor decision tree
|
||||
const Factors &factors() const { return factors_; }
|
||||
|
||||
/// Add MixtureFactor to a Sum, syntactic sugar.
|
||||
/// Add HybridNonlinearFactor to a Sum, syntactic sugar.
|
||||
friend GaussianFactorGraphTree &operator+=(
|
||||
GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) {
|
||||
GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) {
|
||||
sum = factor.add(sum);
|
||||
return sum;
|
||||
}
|
||||
|
|
@ -168,7 +168,6 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
|
||||
// traits
|
||||
template <>
|
||||
struct traits<GaussianMixtureFactor> : public Testable<GaussianMixtureFactor> {
|
||||
};
|
||||
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -23,11 +23,11 @@
|
|||
#include <gtsam/discrete/DiscreteEliminationTree.h>
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteJunctionTree.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridEliminationTree.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridJunctionTree.h>
|
||||
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
|
||||
|
|
@ -92,7 +92,7 @@ void HybridGaussianFactorGraph::printErrors(
|
|||
// Clear the stringstream
|
||||
ss.str(std::string());
|
||||
|
||||
if (auto gmf = std::dynamic_pointer_cast<GaussianMixtureFactor>(factor)) {
|
||||
if (auto gmf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
|
||||
if (factor == nullptr) {
|
||||
std::cout << "nullptr"
|
||||
<< "\n";
|
||||
|
|
@ -178,9 +178,9 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
|
|||
// TODO(dellaert): just use a virtual method defined in HybridFactor.
|
||||
if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
|
||||
result = addGaussian(result, gf);
|
||||
} else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
|
||||
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
result = gmf->add(result);
|
||||
} else if (auto gm = dynamic_pointer_cast<GaussianMixture>(f)) {
|
||||
} else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
|
||||
result = gm->add(result);
|
||||
} else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
|
||||
if (auto gm = hc->asMixture()) {
|
||||
|
|
@ -258,8 +258,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
|
|||
for (auto &f : factors) {
|
||||
if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
|
||||
dfg.push_back(df);
|
||||
} else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
|
||||
// Case where we have a GaussianMixtureFactor with no continuous keys.
|
||||
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
// Case where we have a HybridGaussianFactor with no continuous keys.
|
||||
// In this case, compute discrete probabilities.
|
||||
auto logProbability =
|
||||
[&](const GaussianFactor::shared_ptr &factor) -> double {
|
||||
|
|
@ -309,7 +309,7 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
|
|||
|
||||
/* ************************************************************************ */
|
||||
using Result = std::pair<std::shared_ptr<GaussianConditional>,
|
||||
GaussianMixtureFactor::sharedFactor>;
|
||||
HybridGaussianFactor::sharedFactor>;
|
||||
|
||||
/**
|
||||
* Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
|
||||
|
|
@ -341,9 +341,9 @@ static std::shared_ptr<Factor> createDiscreteFactor(
|
|||
return std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities);
|
||||
}
|
||||
|
||||
// Create GaussianMixtureFactor on the separator, taking care to correct
|
||||
// Create HybridGaussianFactor on the separator, taking care to correct
|
||||
// for conditional constants.
|
||||
static std::shared_ptr<Factor> createGaussianMixtureFactor(
|
||||
static std::shared_ptr<Factor> createHybridGaussianFactor(
|
||||
const DecisionTree<Key, Result> &eliminationResults,
|
||||
const KeyVector &continuousSeparator,
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
|
|
@ -362,7 +362,7 @@ static std::shared_ptr<Factor> createGaussianMixtureFactor(
|
|||
DecisionTree<Key, GaussianFactor::shared_ptr> newFactors(eliminationResults,
|
||||
correct);
|
||||
|
||||
return std::make_shared<GaussianMixtureFactor>(continuousSeparator,
|
||||
return std::make_shared<HybridGaussianFactor>(continuousSeparator,
|
||||
discreteSeparator, newFactors);
|
||||
}
|
||||
|
||||
|
|
@ -400,18 +400,18 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
|
|||
DecisionTree<Key, Result> eliminationResults(factorGraphTree, eliminate);
|
||||
|
||||
// If there are no more continuous parents we create a DiscreteFactor with the
|
||||
// error for each discrete choice. Otherwise, create a GaussianMixtureFactor
|
||||
// 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)
|
||||
: createGaussianMixtureFactor(eliminationResults, continuousSeparator,
|
||||
: createHybridGaussianFactor(eliminationResults, continuousSeparator,
|
||||
discreteSeparator);
|
||||
|
||||
// Create the GaussianMixture from the conditionals
|
||||
GaussianMixture::Conditionals conditionals(
|
||||
// Create the HybridGaussianConditional from the conditionals
|
||||
HybridGaussianConditional::Conditionals conditionals(
|
||||
eliminationResults, [](const Result &pair) { return pair.first; });
|
||||
auto gaussianMixture = std::make_shared<GaussianMixture>(
|
||||
auto gaussianMixture = std::make_shared<HybridGaussianConditional>(
|
||||
frontalKeys, continuousSeparator, discreteSeparator, conditionals);
|
||||
|
||||
return {std::make_shared<HybridConditional>(gaussianMixture), newFactor};
|
||||
|
|
@ -458,7 +458,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
|
|||
// Because of all these reasons, we carefully consider how to
|
||||
// implement the hybrid factors so that we do not get poor performance.
|
||||
|
||||
// The first thing is how to represent the GaussianMixture.
|
||||
// The first thing is how to represent the HybridGaussianConditional.
|
||||
// A very possible scenario is that the incoming factors will have different
|
||||
// levels of discrete keys. For example, imagine we are going to eliminate the
|
||||
// fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid.
|
||||
|
|
@ -549,7 +549,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
|
|||
f = hc->inner();
|
||||
}
|
||||
|
||||
if (auto gaussianMixture = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
|
||||
if (auto gaussianMixture = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
// Compute factor error and add it.
|
||||
error_tree = error_tree + gaussianMixture->errorTree(continuousValues);
|
||||
} else if (auto gaussian = dynamic_pointer_cast<GaussianFactor>(f)) {
|
||||
|
|
@ -597,9 +597,9 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()(
|
|||
gfg.push_back(gf);
|
||||
} else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) {
|
||||
gfg.push_back(gf);
|
||||
} else if (auto gmf = std::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
|
||||
} else if (auto gmf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
gfg.push_back((*gmf)(assignment));
|
||||
} else if (auto gm = dynamic_pointer_cast<GaussianMixture>(f)) {
|
||||
} else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
|
||||
gfg.push_back((*gm)(assignment));
|
||||
} else {
|
||||
continue;
|
||||
|
|
|
|||
|
|
@ -18,9 +18,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
|
|
@ -221,7 +221,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
|
|||
|
||||
/// Get the GaussianFactorGraph at a given discrete assignment.
|
||||
GaussianFactorGraph operator()(const DiscreteValues& assignment) const;
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ class HybridEliminationTree;
|
|||
*/
|
||||
class GTSAM_EXPORT HybridJunctionTree
|
||||
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
|
||||
|
||||
public:
|
||||
typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
|
||||
Base; ///< Base class
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file MixtureFactor.h
|
||||
* @file HybridNonlinearFactor.h
|
||||
* @brief Nonlinear Mixture factor of continuous and discrete.
|
||||
* @author Kevin Doherty, kdoherty@mit.edu
|
||||
* @author Varun Agrawal
|
||||
|
|
@ -20,7 +20,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
|
@ -44,11 +44,11 @@ namespace gtsam {
|
|||
* one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
|
||||
* the correct operation.
|
||||
*/
|
||||
class MixtureFactor : public HybridFactor {
|
||||
class HybridNonlinearFactor : public HybridFactor {
|
||||
public:
|
||||
using Base = HybridFactor;
|
||||
using This = MixtureFactor;
|
||||
using shared_ptr = std::shared_ptr<MixtureFactor>;
|
||||
using This = HybridNonlinearFactor;
|
||||
using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
|
||||
using sharedFactor = std::shared_ptr<NonlinearFactor>;
|
||||
|
||||
/**
|
||||
|
|
@ -63,7 +63,7 @@ class MixtureFactor : public HybridFactor {
|
|||
bool normalized_;
|
||||
|
||||
public:
|
||||
MixtureFactor() = default;
|
||||
HybridNonlinearFactor() = default;
|
||||
|
||||
/**
|
||||
* @brief Construct from Decision tree.
|
||||
|
|
@ -74,7 +74,7 @@ class MixtureFactor : public HybridFactor {
|
|||
* @param normalized Flag indicating if the factor error is already
|
||||
* normalized.
|
||||
*/
|
||||
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
|
||||
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
|
||||
const Factors& factors, bool normalized = false)
|
||||
: Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}
|
||||
|
||||
|
|
@ -95,7 +95,7 @@ class MixtureFactor : public HybridFactor {
|
|||
* normalized.
|
||||
*/
|
||||
template <typename FACTOR>
|
||||
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
|
||||
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
|
||||
const std::vector<std::shared_ptr<FACTOR>>& factors,
|
||||
bool normalized = false)
|
||||
: Base(keys, discreteKeys), normalized_(normalized) {
|
||||
|
|
@ -111,7 +111,7 @@ class MixtureFactor : public HybridFactor {
|
|||
nonlinear_factors.push_back(nf);
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"Factors passed into MixtureFactor need to be nonlinear!");
|
||||
"Factors passed into HybridNonlinearFactor need to be nonlinear!");
|
||||
}
|
||||
}
|
||||
factors_ = Factors(discreteKeys, nonlinear_factors);
|
||||
|
|
@ -124,7 +124,7 @@ class MixtureFactor : public HybridFactor {
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief Compute error of the MixtureFactor as a tree.
|
||||
* @brief Compute error of the HybridNonlinearFactor as a tree.
|
||||
*
|
||||
* @param continuousValues The continuous values for which to compute the
|
||||
* error.
|
||||
|
|
@ -188,7 +188,7 @@ class MixtureFactor : public HybridFactor {
|
|||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << (s.empty() ? "" : s + " ");
|
||||
Base::print("", keyFormatter);
|
||||
std::cout << "\nMixtureFactor\n";
|
||||
std::cout << "\nHybridNonlinearFactor\n";
|
||||
auto valueFormatter = [](const sharedFactor& v) {
|
||||
if (v) {
|
||||
return "Nonlinear factor on " + std::to_string(v->size()) + " keys";
|
||||
|
|
@ -201,15 +201,16 @@ class MixtureFactor : public HybridFactor {
|
|||
|
||||
/// Check equality
|
||||
bool equals(const HybridFactor& other, double tol = 1e-9) const override {
|
||||
// We attempt a dynamic cast from HybridFactor to MixtureFactor. If it
|
||||
// fails, return false.
|
||||
if (!dynamic_cast<const MixtureFactor*>(&other)) return false;
|
||||
// We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If
|
||||
// it fails, return false.
|
||||
if (!dynamic_cast<const HybridNonlinearFactor*>(&other)) return false;
|
||||
|
||||
// If the cast is successful, we'll properly construct a MixtureFactor
|
||||
// object from `other`
|
||||
const MixtureFactor& f(static_cast<const MixtureFactor&>(other));
|
||||
// If the cast is successful, we'll properly construct a
|
||||
// HybridNonlinearFactor object from `other`
|
||||
const HybridNonlinearFactor& f(
|
||||
static_cast<const HybridNonlinearFactor&>(other));
|
||||
|
||||
// Ensure that this MixtureFactor and `f` have the same `factors_`.
|
||||
// Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
|
||||
auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
|
||||
return traits<NonlinearFactor>::Equals(*a, *b, tol);
|
||||
};
|
||||
|
|
@ -233,8 +234,8 @@ class MixtureFactor : public HybridFactor {
|
|||
return factor->linearize(continuousValues);
|
||||
}
|
||||
|
||||
/// Linearize all the continuous factors to get a GaussianMixtureFactor.
|
||||
std::shared_ptr<GaussianMixtureFactor> linearize(
|
||||
/// Linearize all the continuous factors to get a HybridGaussianFactor.
|
||||
std::shared_ptr<HybridGaussianFactor> linearize(
|
||||
const Values& continuousValues) const {
|
||||
// functional to linearize each factor in the decision tree
|
||||
auto linearizeDT = [continuousValues](const sharedFactor& factor) {
|
||||
|
|
@ -244,7 +245,7 @@ class MixtureFactor : public HybridFactor {
|
|||
DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(
|
||||
factors_, linearizeDT);
|
||||
|
||||
return std::make_shared<GaussianMixtureFactor>(
|
||||
return std::make_shared<HybridGaussianFactor>(
|
||||
continuousKeys_, discreteKeys_, linearized_factors);
|
||||
}
|
||||
|
||||
|
|
@ -18,10 +18,10 @@
|
|||
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/TableFactor.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -59,7 +59,7 @@ void HybridNonlinearFactorGraph::printErrors(
|
|||
// Clear the stringstream
|
||||
ss.str(std::string());
|
||||
|
||||
if (auto mf = std::dynamic_pointer_cast<MixtureFactor>(factor)) {
|
||||
if (auto mf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
|
||||
if (factor == nullptr) {
|
||||
std::cout << "nullptr"
|
||||
<< "\n";
|
||||
|
|
@ -70,7 +70,7 @@ void HybridNonlinearFactorGraph::printErrors(
|
|||
std::cout << std::endl;
|
||||
}
|
||||
} else if (auto gmf =
|
||||
std::dynamic_pointer_cast<GaussianMixtureFactor>(factor)) {
|
||||
std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
|
||||
if (factor == nullptr) {
|
||||
std::cout << "nullptr"
|
||||
<< "\n";
|
||||
|
|
@ -80,7 +80,8 @@ void HybridNonlinearFactorGraph::printErrors(
|
|||
gmf->errorTree(values.continuous()).print("", keyFormatter);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
} else if (auto gm = std::dynamic_pointer_cast<GaussianMixture>(factor)) {
|
||||
} else if (auto gm = std::dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
factor)) {
|
||||
if (factor == nullptr) {
|
||||
std::cout << "nullptr"
|
||||
<< "\n";
|
||||
|
|
@ -151,8 +152,8 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
|
|||
continue;
|
||||
}
|
||||
// Check if it is a nonlinear mixture factor
|
||||
if (auto mf = dynamic_pointer_cast<MixtureFactor>(f)) {
|
||||
const GaussianMixtureFactor::shared_ptr& gmf =
|
||||
if (auto mf = dynamic_pointer_cast<HybridNonlinearFactor>(f)) {
|
||||
const HybridGaussianFactor::shared_ptr& gmf =
|
||||
mf->linearize(continuousValues);
|
||||
linearFG->push_back(gmf);
|
||||
} else if (auto nlf = dynamic_pointer_cast<NonlinearFactor>(f)) {
|
||||
|
|
@ -161,9 +162,9 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
|
|||
} else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
|
||||
// If discrete-only: doesn't need linearization.
|
||||
linearFG->push_back(f);
|
||||
} else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
|
||||
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
|
||||
linearFG->push_back(gmf);
|
||||
} else if (auto gm = dynamic_pointer_cast<GaussianMixture>(f)) {
|
||||
} else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
|
||||
linearFG->push_back(gm);
|
||||
} else if (dynamic_pointer_cast<GaussianFactor>(f)) {
|
||||
linearFG->push_back(f);
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
|
||||
#include <optional>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
|
|||
|
|
@ -138,7 +138,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianMixture::shared_ptr HybridSmoother::gaussianMixture(
|
||||
HybridGaussianConditional::shared_ptr HybridSmoother::gaussianMixture(
|
||||
size_t index) const {
|
||||
return hybridBayesNet_.at(index)->asMixture();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ class GTSAM_EXPORT HybridSmoother {
|
|||
const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const;
|
||||
|
||||
/// Get the Gaussian Mixture from the Bayes Net posterior at `index`.
|
||||
GaussianMixture::shared_ptr gaussianMixture(size_t index) const;
|
||||
HybridGaussianConditional::shared_ptr gaussianMixture(size_t index) const;
|
||||
|
||||
/// Return the Bayes Net posterior.
|
||||
const HybridBayesNet& hybridBayesNet() const;
|
||||
|
|
|
|||
|
|
@ -65,39 +65,40 @@ virtual class HybridConditional {
|
|||
double logProbability(const gtsam::HybridValues& values) const;
|
||||
double evaluate(const gtsam::HybridValues& values) const;
|
||||
double operator()(const gtsam::HybridValues& values) const;
|
||||
gtsam::GaussianMixture* asMixture() const;
|
||||
gtsam::HybridGaussianConditional* asMixture() const;
|
||||
gtsam::GaussianConditional* asGaussian() const;
|
||||
gtsam::DiscreteConditional* asDiscrete() const;
|
||||
gtsam::Factor* inner();
|
||||
double error(const gtsam::HybridValues& values) const;
|
||||
};
|
||||
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
class GaussianMixtureFactor : gtsam::HybridFactor {
|
||||
GaussianMixtureFactor(
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
class HybridGaussianFactor : gtsam::HybridFactor {
|
||||
HybridGaussianFactor(
|
||||
const gtsam::KeyVector& continuousKeys,
|
||||
const gtsam::DiscreteKeys& discreteKeys,
|
||||
const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
|
||||
|
||||
void print(string s = "GaussianMixtureFactor\n",
|
||||
void print(string s = "HybridGaussianFactor\n",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
class GaussianMixture : gtsam::HybridFactor {
|
||||
GaussianMixture(const gtsam::KeyVector& continuousFrontals,
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
class HybridGaussianConditional : gtsam::HybridFactor {
|
||||
HybridGaussianConditional(
|
||||
const gtsam::KeyVector& continuousFrontals,
|
||||
const gtsam::KeyVector& continuousParents,
|
||||
const gtsam::DiscreteKeys& discreteParents,
|
||||
const std::vector<gtsam::GaussianConditional::shared_ptr>&
|
||||
conditionalsList);
|
||||
|
||||
gtsam::GaussianMixtureFactor* likelihood(
|
||||
gtsam::HybridGaussianFactor* likelihood(
|
||||
const gtsam::VectorValues& frontals) const;
|
||||
double logProbability(const gtsam::HybridValues& values) const;
|
||||
double evaluate(const gtsam::HybridValues& values) const;
|
||||
|
||||
void print(string s = "GaussianMixture\n",
|
||||
void print(string s = "HybridGaussianConditional\n",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
|
@ -131,7 +132,7 @@ class HybridBayesTree {
|
|||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
class HybridBayesNet {
|
||||
HybridBayesNet();
|
||||
void push_back(const gtsam::GaussianMixture* s);
|
||||
void push_back(const gtsam::HybridGaussianConditional* s);
|
||||
void push_back(const gtsam::GaussianConditional* s);
|
||||
void push_back(const gtsam::DiscreteConditional* s);
|
||||
|
||||
|
|
@ -177,7 +178,7 @@ class HybridGaussianFactorGraph {
|
|||
void push_back(const gtsam::HybridGaussianFactorGraph& graph);
|
||||
void push_back(const gtsam::HybridBayesNet& bayesNet);
|
||||
void push_back(const gtsam::HybridBayesTree& bayesTree);
|
||||
void push_back(const gtsam::GaussianMixtureFactor* gmm);
|
||||
void push_back(const gtsam::HybridGaussianFactor* gmm);
|
||||
void push_back(gtsam::DecisionTreeFactor* factor);
|
||||
void push_back(gtsam::TableFactor* factor);
|
||||
void push_back(gtsam::JacobianFactor* factor);
|
||||
|
|
@ -189,7 +190,8 @@ class HybridGaussianFactorGraph {
|
|||
const gtsam::HybridFactor* at(size_t i) const;
|
||||
|
||||
void print(string s = "") const;
|
||||
bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const;
|
||||
bool equals(const gtsam::HybridGaussianFactorGraph& fg,
|
||||
double tol = 1e-9) const;
|
||||
|
||||
// evaluation
|
||||
double error(const gtsam::HybridValues& values) const;
|
||||
|
|
@ -222,7 +224,8 @@ class HybridNonlinearFactorGraph {
|
|||
void push_back(gtsam::HybridFactor* factor);
|
||||
void push_back(gtsam::NonlinearFactor* factor);
|
||||
void push_back(gtsam::DiscreteFactor* factor);
|
||||
gtsam::HybridGaussianFactorGraph linearize(const gtsam::Values& continuousValues) const;
|
||||
gtsam::HybridGaussianFactorGraph linearize(
|
||||
const gtsam::Values& continuousValues) const;
|
||||
|
||||
bool empty() const;
|
||||
void remove(size_t i);
|
||||
|
|
@ -235,28 +238,28 @@ class HybridNonlinearFactorGraph {
|
|||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
class MixtureFactor : gtsam::HybridFactor {
|
||||
MixtureFactor(
|
||||
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||
class HybridNonlinearFactor : gtsam::HybridFactor {
|
||||
HybridNonlinearFactor(
|
||||
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
|
||||
const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors,
|
||||
bool normalized = false);
|
||||
|
||||
template <FACTOR = {gtsam::NonlinearFactor}>
|
||||
MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
|
||||
HybridNonlinearFactor(const gtsam::KeyVector& keys,
|
||||
const gtsam::DiscreteKeys& discreteKeys,
|
||||
const std::vector<FACTOR*>& factors,
|
||||
bool normalized = false);
|
||||
|
||||
double error(const gtsam::Values& continuousValues,
|
||||
const gtsam::DiscreteValues& discreteValues) const;
|
||||
|
||||
double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor,
|
||||
const gtsam::Values& values) const;
|
||||
double nonlinearFactorLogNormalizingConstant(
|
||||
const gtsam::NonlinearFactor* factor, const gtsam::Values& values) const;
|
||||
|
||||
GaussianMixtureFactor* linearize(
|
||||
const gtsam::Values& continuousValues) const;
|
||||
HybridGaussianFactor* linearize(const gtsam::Values& continuousValues) const;
|
||||
|
||||
void print(string s = "MixtureFactor\n",
|
||||
void print(string s = "HybridNonlinearFactor\n",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -19,10 +19,10 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteDistribution.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
|
|
@ -57,7 +57,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
|
|||
|
||||
// keyFunc(1) to keyFunc(n+1)
|
||||
for (size_t t = 1; t < n; t++) {
|
||||
hfg.add(GaussianMixtureFactor(
|
||||
hfg.add(HybridGaussianFactor(
|
||||
{keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}},
|
||||
{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
|
||||
I_3x3, Z_3x1),
|
||||
|
|
@ -163,7 +163,7 @@ struct Switching {
|
|||
for (auto &&f : motion_models) {
|
||||
components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f));
|
||||
}
|
||||
nonlinearFactorGraph.emplace_shared<MixtureFactor>(
|
||||
nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
|
||||
keys, DiscreteKeys{modes[k]}, components);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
|
|||
// Create Gaussian mixture z_i = x0 + noise for each measurement.
|
||||
for (size_t i = 0; i < num_measurements; i++) {
|
||||
const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
|
||||
bayesNet.emplace_shared<GaussianMixture>(
|
||||
bayesNet.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{Z(i)}, KeyVector{X(0)}, DiscreteKeys{mode_i},
|
||||
std::vector{GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0),
|
||||
Z_1x1, 0.5),
|
||||
|
|
|
|||
|
|
@ -107,7 +107,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
|
|||
// Create hybrid Bayes net.
|
||||
HybridBayesNet bayesNet;
|
||||
bayesNet.push_back(continuousConditional);
|
||||
bayesNet.emplace_shared<GaussianMixture>(
|
||||
bayesNet.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia},
|
||||
std::vector{conditional0, conditional1});
|
||||
bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1");
|
||||
|
|
@ -168,7 +168,7 @@ TEST(HybridBayesNet, Error) {
|
|||
conditional1 = std::make_shared<GaussianConditional>(
|
||||
X(1), Vector1::Constant(2), I_1x1, model1);
|
||||
|
||||
auto gm = std::make_shared<GaussianMixture>(
|
||||
auto gm = std::make_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia},
|
||||
std::vector{conditional0, conditional1});
|
||||
// Create hybrid Bayes net.
|
||||
|
|
@ -389,7 +389,7 @@ TEST(HybridBayesNet, Sampling) {
|
|||
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
|
||||
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
|
||||
nfg.emplace_shared<MixtureFactor>(
|
||||
nfg.emplace_shared<HybridNonlinearFactor>(
|
||||
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
|
||||
|
||||
DiscreteKey mode(M(0), 2);
|
||||
|
|
|
|||
|
|
@ -43,9 +43,9 @@ TEST(HybridConditional, Invariants) {
|
|||
auto hc0 = bn.at(0);
|
||||
CHECK(hc0->isHybrid());
|
||||
|
||||
// Check invariants as a GaussianMixture.
|
||||
// Check invariants as a HybridGaussianConditional.
|
||||
const auto mixture = hc0->asMixture();
|
||||
EXPECT(GaussianMixture::CheckInvariants(*mixture, values));
|
||||
EXPECT(HybridGaussianConditional::CheckInvariants(*mixture, values));
|
||||
|
||||
// Check invariants as a HybridConditional.
|
||||
EXPECT(HybridConditional::CheckInvariants(*hc0, values));
|
||||
|
|
|
|||
|
|
@ -19,10 +19,10 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearISAM.h>
|
||||
#include <gtsam/hybrid/HybridSmoother.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
|
|
@ -435,7 +435,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
|
|||
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
|
||||
const auto one_motion =
|
||||
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
|
||||
nfg.emplace_shared<MixtureFactor>(
|
||||
nfg.emplace_shared<HybridNonlinearFactor>(
|
||||
KeyVector{X(0), X(1)}, DiscreteKeys{m},
|
||||
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
|
||||
|
||||
|
|
@ -531,10 +531,10 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
|
|||
* Helper function to add the constant term corresponding to
|
||||
* the difference in noise models.
|
||||
*/
|
||||
std::shared_ptr<GaussianMixtureFactor> mixedVarianceFactor(
|
||||
const MixtureFactor& mf, const Values& initial, const Key& mode,
|
||||
std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
|
||||
const HybridNonlinearFactor& mf, const Values& initial, const Key& mode,
|
||||
double noise_tight, double noise_loose, size_t d, size_t tight_index) {
|
||||
GaussianMixtureFactor::shared_ptr gmf = mf.linearize(initial);
|
||||
HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial);
|
||||
|
||||
constexpr double log2pi = 1.8378770664093454835606594728112;
|
||||
// logConstant will be of the tighter model
|
||||
|
|
@ -560,7 +560,7 @@ std::shared_ptr<GaussianMixtureFactor> mixedVarianceFactor(
|
|||
}
|
||||
};
|
||||
auto updated_components = gmf->factors().apply(func);
|
||||
auto updated_gmf = std::make_shared<GaussianMixtureFactor>(
|
||||
auto updated_gmf = std::make_shared<HybridGaussianFactor>(
|
||||
gmf->continuousKeys(), gmf->discreteKeys(), updated_components);
|
||||
|
||||
return updated_gmf;
|
||||
|
|
@ -592,7 +592,7 @@ TEST(HybridEstimation, ModeSelection) {
|
|||
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
|
||||
|
||||
KeyVector keys = {X(0), X(1)};
|
||||
MixtureFactor mf(keys, modes, components);
|
||||
HybridNonlinearFactor mf(keys, modes, components);
|
||||
|
||||
initial.insert(X(0), 0.0);
|
||||
initial.insert(X(1), 0.0);
|
||||
|
|
@ -616,7 +616,7 @@ TEST(HybridEstimation, ModeSelection) {
|
|||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1));
|
||||
bn.push_back(
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1));
|
||||
bn.emplace_shared<GaussianMixture>(
|
||||
bn.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
|
||||
std::vector{GaussianConditional::sharedMeanAndStddev(
|
||||
Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_loose),
|
||||
|
|
@ -647,7 +647,7 @@ TEST(HybridEstimation, ModeSelection2) {
|
|||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1));
|
||||
bn.push_back(
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1));
|
||||
bn.emplace_shared<GaussianMixture>(
|
||||
bn.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
|
||||
std::vector{GaussianConditional::sharedMeanAndStddev(
|
||||
Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_loose),
|
||||
|
|
@ -683,7 +683,7 @@ TEST(HybridEstimation, ModeSelection2) {
|
|||
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
|
||||
|
||||
KeyVector keys = {X(0), X(1)};
|
||||
MixtureFactor mf(keys, modes, components);
|
||||
HybridNonlinearFactor mf(keys, modes, components);
|
||||
|
||||
initial.insert<Vector3>(X(0), Z_3x1);
|
||||
initial.insert<Vector3>(X(1), Z_3x1);
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ TEST(HybridFactorGraph, Keys) {
|
|||
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
|
||||
M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
|
||||
hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt));
|
||||
|
||||
KeySet expected_continuous{X(0), X(1)};
|
||||
EXPECT(
|
||||
|
|
|
|||
|
|
@ -10,8 +10,8 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testGaussianMixture.cpp
|
||||
* @brief Unit tests for GaussianMixture class
|
||||
* @file testHybridGaussianConditional.cpp
|
||||
* @brief Unit tests for HybridGaussianConditional class
|
||||
* @author Varun Agrawal
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -19,8 +19,8 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
|
@ -45,19 +45,19 @@ static const HybridValues hv1{vv, assignment1};
|
|||
|
||||
/* ************************************************************************* */
|
||||
namespace equal_constants {
|
||||
// Create a simple GaussianMixture
|
||||
// Create a simple HybridGaussianConditional
|
||||
const double commonSigma = 2.0;
|
||||
const std::vector<GaussianConditional::shared_ptr> conditionals{
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
|
||||
commonSigma),
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
|
||||
commonSigma)};
|
||||
const GaussianMixture mixture({Z(0)}, {X(0)}, {mode}, conditionals);
|
||||
const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals);
|
||||
} // namespace equal_constants
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Check that invariants hold
|
||||
TEST(GaussianMixture, Invariants) {
|
||||
TEST(HybridGaussianConditional, Invariants) {
|
||||
using namespace equal_constants;
|
||||
|
||||
// Check that the mixture normalization constant is the max of all constants
|
||||
|
|
@ -66,13 +66,13 @@ TEST(GaussianMixture, Invariants) {
|
|||
EXPECT_DOUBLES_EQUAL(K, conditionals[0]->logNormalizationConstant(), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(K, conditionals[1]->logNormalizationConstant(), 1e-8);
|
||||
|
||||
EXPECT(GaussianMixture::CheckInvariants(mixture, hv0));
|
||||
EXPECT(GaussianMixture::CheckInvariants(mixture, hv1));
|
||||
EXPECT(HybridGaussianConditional::CheckInvariants(mixture, hv0));
|
||||
EXPECT(HybridGaussianConditional::CheckInvariants(mixture, hv1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Check LogProbability.
|
||||
TEST(GaussianMixture, LogProbability) {
|
||||
TEST(HybridGaussianConditional, LogProbability) {
|
||||
using namespace equal_constants;
|
||||
auto actual = mixture.logProbability(vv);
|
||||
|
||||
|
|
@ -94,7 +94,7 @@ TEST(GaussianMixture, LogProbability) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
/// Check error.
|
||||
TEST(GaussianMixture, Error) {
|
||||
TEST(HybridGaussianConditional, Error) {
|
||||
using namespace equal_constants;
|
||||
auto actual = mixture.errorTree(vv);
|
||||
|
||||
|
|
@ -117,7 +117,7 @@ TEST(GaussianMixture, Error) {
|
|||
/* ************************************************************************* */
|
||||
/// Check that the likelihood is proportional to the conditional density given
|
||||
/// the measurements.
|
||||
TEST(GaussianMixture, Likelihood) {
|
||||
TEST(HybridGaussianConditional, Likelihood) {
|
||||
using namespace equal_constants;
|
||||
|
||||
// Compute likelihood
|
||||
|
|
@ -146,19 +146,19 @@ TEST(GaussianMixture, Likelihood) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
namespace mode_dependent_constants {
|
||||
// Create a GaussianMixture with mode-dependent noise models.
|
||||
// Create a HybridGaussianConditional with mode-dependent noise models.
|
||||
// 0 is low-noise, 1 is high-noise.
|
||||
const std::vector<GaussianConditional::shared_ptr> conditionals{
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
|
||||
0.5),
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
|
||||
3.0)};
|
||||
const GaussianMixture mixture({Z(0)}, {X(0)}, {mode}, conditionals);
|
||||
const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals);
|
||||
} // namespace mode_dependent_constants
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create a test for continuousParents.
|
||||
TEST(GaussianMixture, ContinuousParents) {
|
||||
TEST(HybridGaussianConditional, ContinuousParents) {
|
||||
using namespace mode_dependent_constants;
|
||||
const KeyVector continuousParentKeys = mixture.continuousParents();
|
||||
// Check that the continuous parent keys are correct:
|
||||
|
|
@ -169,7 +169,7 @@ TEST(GaussianMixture, ContinuousParents) {
|
|||
/* ************************************************************************* */
|
||||
/// Check that the likelihood is proportional to the conditional density given
|
||||
/// the measurements.
|
||||
TEST(GaussianMixture, Likelihood2) {
|
||||
TEST(HybridGaussianConditional, Likelihood2) {
|
||||
using namespace mode_dependent_constants;
|
||||
|
||||
// Compute likelihood
|
||||
|
|
@ -10,8 +10,8 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testGaussianMixtureFactor.cpp
|
||||
* @brief Unit tests for GaussianMixtureFactor
|
||||
* @file testHybridGaussianFactor.cpp
|
||||
* @brief Unit tests for HybridGaussianFactor
|
||||
* @author Varun Agrawal
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -22,9 +22,9 @@
|
|||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
|
@ -46,17 +46,17 @@ using symbol_shorthand::Z;
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Check iterators of empty mixture.
|
||||
TEST(GaussianMixtureFactor, Constructor) {
|
||||
GaussianMixtureFactor factor;
|
||||
GaussianMixtureFactor::const_iterator const_it = factor.begin();
|
||||
TEST(HybridGaussianFactor, Constructor) {
|
||||
HybridGaussianFactor factor;
|
||||
HybridGaussianFactor::const_iterator const_it = factor.begin();
|
||||
CHECK(const_it == factor.end());
|
||||
GaussianMixtureFactor::iterator it = factor.begin();
|
||||
HybridGaussianFactor::iterator it = factor.begin();
|
||||
CHECK(it == factor.end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// "Add" two mixture factors together.
|
||||
TEST(GaussianMixtureFactor, Sum) {
|
||||
TEST(HybridGaussianFactor, Sum) {
|
||||
DiscreteKey m1(1, 2), m2(2, 3);
|
||||
|
||||
auto A1 = Matrix::Zero(2, 1);
|
||||
|
|
@ -77,8 +77,8 @@ TEST(GaussianMixtureFactor, 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!
|
||||
GaussianMixtureFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA);
|
||||
GaussianMixtureFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB);
|
||||
HybridGaussianFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA);
|
||||
HybridGaussianFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB);
|
||||
|
||||
// Check that number of keys is 3
|
||||
EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size());
|
||||
|
|
@ -102,7 +102,7 @@ TEST(GaussianMixtureFactor, Sum) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianMixtureFactor, Printing) {
|
||||
TEST(HybridGaussianFactor, Printing) {
|
||||
DiscreteKey m1(1, 2);
|
||||
auto A1 = Matrix::Zero(2, 1);
|
||||
auto A2 = Matrix::Zero(2, 2);
|
||||
|
|
@ -111,10 +111,10 @@ TEST(GaussianMixtureFactor, Printing) {
|
|||
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
std::vector<GaussianFactor::shared_ptr> factors{f10, f11};
|
||||
|
||||
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
|
||||
std::string expected =
|
||||
R"(GaussianMixtureFactor
|
||||
R"(HybridGaussianFactor
|
||||
Hybrid [x1 x2; 1]{
|
||||
Choice(1)
|
||||
0 Leaf :
|
||||
|
|
@ -147,7 +147,7 @@ Hybrid [x1 x2; 1]{
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianMixtureFactor, GaussianMixture) {
|
||||
TEST(HybridGaussianFactor, HybridGaussianConditional) {
|
||||
KeyVector keys;
|
||||
keys.push_back(X(0));
|
||||
keys.push_back(X(1));
|
||||
|
|
@ -157,15 +157,15 @@ TEST(GaussianMixtureFactor, GaussianMixture) {
|
|||
dKeys.emplace_back(M(1), 2);
|
||||
|
||||
auto gaussians = std::make_shared<GaussianConditional>();
|
||||
GaussianMixture::Conditionals conditionals(gaussians);
|
||||
GaussianMixture gm({}, keys, dKeys, conditionals);
|
||||
HybridGaussianConditional::Conditionals conditionals(gaussians);
|
||||
HybridGaussianConditional gm({}, keys, dKeys, conditionals);
|
||||
|
||||
EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test the error of the GaussianMixtureFactor
|
||||
TEST(GaussianMixtureFactor, Error) {
|
||||
// Test the error of the HybridGaussianFactor
|
||||
TEST(HybridGaussianFactor, Error) {
|
||||
DiscreteKey m1(1, 2);
|
||||
|
||||
auto A01 = Matrix2::Identity();
|
||||
|
|
@ -180,7 +180,7 @@ TEST(GaussianMixtureFactor, Error) {
|
|||
auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
|
||||
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
|
||||
VectorValues continuousValues;
|
||||
continuousValues.insert(X(1), Vector2(0, 0));
|
||||
|
|
@ -232,8 +232,8 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1,
|
|||
c1 = make_shared<GaussianConditional>(z, Vector1(mu1), I_1x1, model1);
|
||||
|
||||
HybridBayesNet hbn;
|
||||
hbn.emplace_shared<GaussianMixture>(KeyVector{z}, KeyVector{},
|
||||
DiscreteKeys{m}, std::vector{c0, c1});
|
||||
hbn.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1});
|
||||
|
||||
auto mixing = make_shared<DiscreteConditional>(m, "50/50");
|
||||
hbn.push_back(mixing);
|
||||
|
|
@ -253,7 +253,7 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1,
|
|||
* The resulting factor graph should eliminate to a Bayes net
|
||||
* which represents a sigmoid function.
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, GaussianMixtureModel) {
|
||||
TEST(HybridGaussianFactor, GaussianMixtureModel) {
|
||||
using namespace test_gmm;
|
||||
|
||||
double mu0 = 1.0, mu1 = 3.0;
|
||||
|
|
@ -325,7 +325,7 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) {
|
|||
* which represents a Gaussian-like function
|
||||
* where m1>m0 close to 3.1333.
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, GaussianMixtureModel2) {
|
||||
TEST(HybridGaussianFactor, GaussianMixtureModel2) {
|
||||
using namespace test_gmm;
|
||||
|
||||
double mu0 = 1.0, mu1 = 3.0;
|
||||
|
|
@ -399,23 +399,21 @@ void addMeasurement(HybridBayesNet& hbn, Key z_key, Key x_key, double sigma) {
|
|||
}
|
||||
|
||||
/// Create hybrid motion model p(x1 | x0, m1)
|
||||
static GaussianMixture::shared_ptr CreateHybridMotionModel(double mu0,
|
||||
double mu1,
|
||||
double sigma0,
|
||||
double sigma1) {
|
||||
static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
|
||||
double mu0, double mu1, double sigma0, double sigma1) {
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
|
||||
auto c0 = make_shared<GaussianConditional>(X(1), Vector1(mu0), I_1x1, X(0),
|
||||
-I_1x1, model0),
|
||||
c1 = make_shared<GaussianConditional>(X(1), Vector1(mu1), I_1x1, X(0),
|
||||
-I_1x1, model1);
|
||||
return std::make_shared<GaussianMixture>(
|
||||
return std::make_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(1)}, KeyVector{X(0)}, DiscreteKeys{m1}, std::vector{c0, c1});
|
||||
}
|
||||
|
||||
/// Create two state Bayes network with 1 or two measurement models
|
||||
HybridBayesNet CreateBayesNet(
|
||||
const GaussianMixture::shared_ptr& hybridMotionModel,
|
||||
const HybridGaussianConditional::shared_ptr& hybridMotionModel,
|
||||
bool add_second_measurement = false) {
|
||||
HybridBayesNet hbn;
|
||||
|
||||
|
|
@ -439,7 +437,7 @@ HybridBayesNet CreateBayesNet(
|
|||
/// Approximate the discrete marginal P(m1) using importance sampling
|
||||
std::pair<double, double> approximateDiscreteMarginal(
|
||||
const HybridBayesNet& hbn,
|
||||
const GaussianMixture::shared_ptr& hybridMotionModel,
|
||||
const HybridGaussianConditional::shared_ptr& hybridMotionModel,
|
||||
const VectorValues& given, size_t N = 100000) {
|
||||
/// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1),
|
||||
/// using q(x0) = N(z0, sigmaQ) to sample x0.
|
||||
|
|
@ -478,7 +476,7 @@ std::pair<double, double> approximateDiscreteMarginal(
|
|||
* the posterior probability of m1 should be 0.5/0.5.
|
||||
* Getting a measurement on z1 gives use more information.
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, TwoStateModel) {
|
||||
TEST(HybridGaussianFactor, TwoStateModel) {
|
||||
using namespace test_two_state_estimation;
|
||||
|
||||
double mu0 = 1.0, mu1 = 3.0;
|
||||
|
|
@ -534,7 +532,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) {
|
|||
* the P(m1) should be 0.5/0.5.
|
||||
* Getting a measurement on z1 gives use more information.
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, TwoStateModel2) {
|
||||
TEST(HybridGaussianFactor, TwoStateModel2) {
|
||||
using namespace test_two_state_estimation;
|
||||
|
||||
double mu0 = 1.0, mu1 = 3.0;
|
||||
|
|
@ -621,7 +619,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) {
|
|||
* measurements and vastly different motion model: either stand still or move
|
||||
* far. This yields a very informative posterior.
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, TwoStateModel3) {
|
||||
TEST(HybridGaussianFactor, TwoStateModel3) {
|
||||
using namespace test_two_state_estimation;
|
||||
|
||||
double mu0 = 0.0, mu1 = 10.0;
|
||||
|
|
@ -21,12 +21,12 @@
|
|||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridGaussianISAM.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
|
|
@ -71,13 +71,14 @@ TEST(HybridGaussianFactorGraph, Creation) {
|
|||
|
||||
// Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
|
||||
// graph
|
||||
GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}),
|
||||
GaussianMixture::Conditionals(
|
||||
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)));
|
||||
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());
|
||||
|
|
@ -129,7 +130,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
|
|||
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
|
||||
M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
|
||||
hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt));
|
||||
|
||||
auto result = hfg.eliminateSequential();
|
||||
|
||||
|
|
@ -155,7 +156,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
|
|||
std::vector<GaussianFactor::shared_ptr> factors = {
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())};
|
||||
hfg.add(GaussianMixtureFactor({X(1)}, {m1}, factors));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors));
|
||||
|
||||
// Discrete probability table for c1
|
||||
hfg.add(DecisionTreeFactor(m1, {2, 8}));
|
||||
|
|
@ -177,7 +178,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(GaussianMixtureFactor(
|
||||
hfg.add(HybridGaussianFactor(
|
||||
{X(1)}, {{M(1), 2}},
|
||||
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
|
||||
|
|
@ -212,7 +213,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
|
|||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
|
||||
|
||||
// Hybrid factor P(x1|c1)
|
||||
hfg.add(GaussianMixtureFactor({X(1)}, {m}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {m}, dt));
|
||||
// Prior factor on c1
|
||||
hfg.add(DecisionTreeFactor(m, {2, 8}));
|
||||
|
||||
|
|
@ -237,7 +238,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
|
||||
|
||||
{
|
||||
hfg.add(GaussianMixtureFactor(
|
||||
hfg.add(HybridGaussianFactor(
|
||||
{X(0)}, {{M(0), 2}},
|
||||
{std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));
|
||||
|
|
@ -246,7 +247,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
M(1), std::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(GaussianMixtureFactor({X(2)}, {{M(1), 2}}, dt1));
|
||||
hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1));
|
||||
}
|
||||
|
||||
hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
|
||||
|
|
@ -259,13 +260,13 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
|
|||
M(3), std::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(GaussianMixtureFactor({X(3)}, {{M(3), 2}}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(3)}, {{M(3), 2}}, dt));
|
||||
|
||||
DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
|
||||
M(2), std::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(GaussianMixtureFactor({X(5)}, {{M(2), 2}}, dt1));
|
||||
hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1));
|
||||
}
|
||||
|
||||
auto ordering_full =
|
||||
|
|
@ -555,7 +556,7 @@ TEST(HybridGaussianFactorGraph, optimize) {
|
|||
C(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
|
||||
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
|
||||
|
||||
hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
|
||||
hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt));
|
||||
|
||||
auto result = hfg.eliminateSequential();
|
||||
|
||||
|
|
@ -681,8 +682,8 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) {
|
|||
x0, -I_1x1, model0),
|
||||
c1 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1,
|
||||
x0, -I_1x1, model1);
|
||||
hbn.emplace_shared<GaussianMixture>(KeyVector{f01}, KeyVector{x0, x1},
|
||||
DiscreteKeys{m1}, std::vector{c0, c1});
|
||||
hbn.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{f01}, KeyVector{x0, x1}, DiscreteKeys{m1}, std::vector{c0, c1});
|
||||
|
||||
// Discrete uniform prior.
|
||||
hbn.emplace_shared<DiscreteConditional>(m1, "0.5/0.5");
|
||||
|
|
@ -717,7 +718,7 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
|
|||
// Create expected decision tree with two factor graphs:
|
||||
|
||||
// Get mixture factor:
|
||||
auto mixture = fg.at<GaussianMixtureFactor>(0);
|
||||
auto mixture = fg.at<HybridGaussianFactor>(0);
|
||||
CHECK(mixture);
|
||||
|
||||
// Get prior factor:
|
||||
|
|
@ -805,7 +806,7 @@ 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);
|
||||
expectedBayesNet.emplace_shared<GaussianMixture>(
|
||||
expectedBayesNet.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
|
||||
std::vector{conditional0, conditional1});
|
||||
|
||||
|
|
@ -830,7 +831,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
|
|||
HybridBayesNet bn;
|
||||
|
||||
// Create Gaussian mixture z_0 = x0 + noise for each measurement.
|
||||
auto gm = std::make_shared<GaussianMixture>(
|
||||
auto gm = std::make_shared<HybridGaussianConditional>(
|
||||
KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode},
|
||||
std::vector{
|
||||
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3),
|
||||
|
|
@ -862,7 +863,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
|
|||
X(0), Vector1(10.1379), I_1x1 * 2.02759),
|
||||
conditional1 = std::make_shared<GaussianConditional>(
|
||||
X(0), Vector1(14.1421), I_1x1 * 2.82843);
|
||||
expectedBayesNet.emplace_shared<GaussianMixture>(
|
||||
expectedBayesNet.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
|
||||
std::vector{conditional0, conditional1});
|
||||
|
||||
|
|
@ -899,7 +900,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
|
|||
X(0), Vector1(17.3205), I_1x1 * 3.4641),
|
||||
conditional1 = std::make_shared<GaussianConditional>(
|
||||
X(0), Vector1(10.274), I_1x1 * 2.0548);
|
||||
expectedBayesNet.emplace_shared<GaussianMixture>(
|
||||
expectedBayesNet.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
|
||||
std::vector{conditional0, conditional1});
|
||||
|
||||
|
|
@ -946,7 +947,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
|
|||
for (size_t t : {0, 1, 2}) {
|
||||
// Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t):
|
||||
const auto noise_mode_t = DiscreteKey{N(t), 2};
|
||||
bn.emplace_shared<GaussianMixture>(
|
||||
bn.emplace_shared<HybridGaussianConditional>(
|
||||
KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t},
|
||||
std::vector{GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t),
|
||||
Z_1x1, 0.5),
|
||||
|
|
@ -961,7 +962,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
|
|||
for (size_t t : {2, 1}) {
|
||||
// Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1):
|
||||
const auto motion_model_t = DiscreteKey{M(t), 2};
|
||||
auto gm = std::make_shared<GaussianMixture>(
|
||||
auto gm = std::make_shared<HybridGaussianConditional>(
|
||||
KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t},
|
||||
std::vector{GaussianConditional::sharedMeanAndStddev(
|
||||
X(t), I_1x1, X(t - 1), Z_1x1, 0.2),
|
||||
|
|
|
|||
|
|
@ -133,23 +133,26 @@ TEST(HybridGaussianElimination, IncrementalInference) {
|
|||
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
|
||||
|
||||
// The densities on X(0) should be the same
|
||||
auto x0_conditional =
|
||||
dynamic_pointer_cast<GaussianMixture>(isam[X(0)]->conditional()->inner());
|
||||
auto expected_x0_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
isam[X(0)]->conditional()->inner());
|
||||
auto expected_x0_conditional =
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
(*expectedHybridBayesTree)[X(0)]->conditional()->inner());
|
||||
EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional));
|
||||
|
||||
// The densities on X(1) should be the same
|
||||
auto x1_conditional =
|
||||
dynamic_pointer_cast<GaussianMixture>(isam[X(1)]->conditional()->inner());
|
||||
auto expected_x1_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
isam[X(1)]->conditional()->inner());
|
||||
auto expected_x1_conditional =
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
(*expectedHybridBayesTree)[X(1)]->conditional()->inner());
|
||||
EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional));
|
||||
|
||||
// The densities on X(2) should be the same
|
||||
auto x2_conditional =
|
||||
dynamic_pointer_cast<GaussianMixture>(isam[X(2)]->conditional()->inner());
|
||||
auto expected_x2_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
isam[X(2)]->conditional()->inner());
|
||||
auto expected_x2_conditional =
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
(*expectedHybridBayesTree)[X(2)]->conditional()->inner());
|
||||
EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));
|
||||
|
||||
|
|
@ -279,9 +282,9 @@ TEST(HybridGaussianElimination, Approx_inference) {
|
|||
|
||||
// Check that the hybrid nodes of the bayes net match those of the pre-pruning
|
||||
// bayes net, at the same positions.
|
||||
auto &unprunedLastDensity = *dynamic_pointer_cast<GaussianMixture>(
|
||||
auto &unprunedLastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
unprunedHybridBayesTree->clique(X(3))->conditional()->inner());
|
||||
auto &lastDensity = *dynamic_pointer_cast<GaussianMixture>(
|
||||
auto &lastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
incrementalHybrid[X(3)]->conditional()->inner());
|
||||
|
||||
std::vector<std::pair<DiscreteValues, double>> assignments =
|
||||
|
|
@ -418,7 +421,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
|
|||
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
|
||||
noise_model);
|
||||
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
|
||||
auto mixtureFactor = std::make_shared<MixtureFactor>(
|
||||
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
|
||||
fg.push_back(mixtureFactor);
|
||||
|
||||
|
|
@ -458,7 +461,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
|
|||
moving =
|
||||
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
|
||||
components = {moving, still};
|
||||
mixtureFactor = std::make_shared<MixtureFactor>(
|
||||
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
|
||||
fg.push_back(mixtureFactor);
|
||||
|
||||
|
|
@ -501,7 +504,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
|
|||
moving =
|
||||
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
|
||||
components = {moving, still};
|
||||
mixtureFactor = std::make_shared<MixtureFactor>(
|
||||
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
|
||||
fg.push_back(mixtureFactor);
|
||||
|
||||
|
|
|
|||
|
|
@ -10,8 +10,8 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testMixtureFactor.cpp
|
||||
* @brief Unit tests for MixtureFactor
|
||||
* @file testHybridNonlinearFactor.cpp
|
||||
* @brief Unit tests for HybridNonlinearFactor
|
||||
* @author Varun Agrawal
|
||||
* @date October 2022
|
||||
*/
|
||||
|
|
@ -20,8 +20,8 @@
|
|||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
|
|
@ -36,17 +36,17 @@ using symbol_shorthand::X;
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Check iterators of empty mixture.
|
||||
TEST(MixtureFactor, Constructor) {
|
||||
MixtureFactor factor;
|
||||
MixtureFactor::const_iterator const_it = factor.begin();
|
||||
TEST(HybridNonlinearFactor, Constructor) {
|
||||
HybridNonlinearFactor factor;
|
||||
HybridNonlinearFactor::const_iterator const_it = factor.begin();
|
||||
CHECK(const_it == factor.end());
|
||||
MixtureFactor::iterator it = factor.begin();
|
||||
HybridNonlinearFactor::iterator it = factor.begin();
|
||||
CHECK(it == factor.end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test .print() output.
|
||||
TEST(MixtureFactor, Printing) {
|
||||
TEST(HybridNonlinearFactor, Printing) {
|
||||
DiscreteKey m1(1, 2);
|
||||
double between0 = 0.0;
|
||||
double between1 = 1.0;
|
||||
|
|
@ -60,11 +60,11 @@ TEST(MixtureFactor, Printing) {
|
|||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
|
||||
std::string expected =
|
||||
R"(Hybrid [x1 x2; 1]
|
||||
MixtureFactor
|
||||
HybridNonlinearFactor
|
||||
Choice(1)
|
||||
0 Leaf Nonlinear factor on 2 keys
|
||||
1 Leaf Nonlinear factor on 2 keys
|
||||
|
|
@ -73,7 +73,7 @@ MixtureFactor
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static MixtureFactor getMixtureFactor() {
|
||||
static HybridNonlinearFactor getHybridNonlinearFactor() {
|
||||
DiscreteKey m1(1, 2);
|
||||
|
||||
double between0 = 0.0;
|
||||
|
|
@ -88,13 +88,13 @@ static MixtureFactor getMixtureFactor() {
|
|||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
return MixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test the error of the MixtureFactor
|
||||
TEST(MixtureFactor, Error) {
|
||||
auto mixtureFactor = getMixtureFactor();
|
||||
// Test the error of the HybridNonlinearFactor
|
||||
TEST(HybridNonlinearFactor, Error) {
|
||||
auto mixtureFactor = getHybridNonlinearFactor();
|
||||
|
||||
Values continuousValues;
|
||||
continuousValues.insert<double>(X(1), 0);
|
||||
|
|
@ -112,9 +112,9 @@ TEST(MixtureFactor, Error) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test dim of the MixtureFactor
|
||||
TEST(MixtureFactor, Dim) {
|
||||
auto mixtureFactor = getMixtureFactor();
|
||||
// Test dim of the HybridNonlinearFactor
|
||||
TEST(HybridNonlinearFactor, Dim) {
|
||||
auto mixtureFactor = getHybridNonlinearFactor();
|
||||
EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
|
||||
}
|
||||
|
||||
|
|
@ -23,8 +23,8 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/hybrid/HybridEliminationTree.h>
|
||||
#include <gtsam/hybrid/HybridFactor.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
|
@ -105,7 +105,7 @@ TEST(HybridNonlinearFactorGraph, Resize) {
|
|||
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
|
||||
fg.push_back(discreteFactor);
|
||||
|
||||
auto dcFactor = std::make_shared<MixtureFactor>();
|
||||
auto dcFactor = std::make_shared<HybridNonlinearFactor>();
|
||||
fg.push_back(dcFactor);
|
||||
|
||||
EXPECT_LONGS_EQUAL(fg.size(), 3);
|
||||
|
|
@ -132,7 +132,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
|
|||
moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
|
||||
|
||||
std::vector<MotionModel::shared_ptr> components = {still, moving};
|
||||
auto dcFactor = std::make_shared<MixtureFactor>(
|
||||
auto dcFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
|
||||
nhfg.push_back(dcFactor);
|
||||
|
||||
|
|
@ -150,10 +150,10 @@ TEST(HybridGaussianFactorGraph, Resize) {
|
|||
}
|
||||
|
||||
/***************************************************************************
|
||||
* Test that the MixtureFactor reports correctly if the number of continuous
|
||||
* keys provided do not match the keys in the factors.
|
||||
* Test that the HybridNonlinearFactor reports correctly if the number of
|
||||
* continuous keys provided do not match the keys in the factors.
|
||||
*/
|
||||
TEST(HybridGaussianFactorGraph, MixtureFactor) {
|
||||
TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
|
||||
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
|
||||
X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
|
||||
auto discreteFactor = std::make_shared<DecisionTreeFactor>();
|
||||
|
|
@ -166,12 +166,12 @@ TEST(HybridGaussianFactorGraph, MixtureFactor) {
|
|||
|
||||
// Check for exception when number of continuous keys are under-specified.
|
||||
KeyVector contKeys = {X(0)};
|
||||
THROWS_EXCEPTION(std::make_shared<MixtureFactor>(
|
||||
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
|
||||
|
||||
// Check for exception when number of continuous keys are too many.
|
||||
contKeys = {X(0), X(1), X(2)};
|
||||
THROWS_EXCEPTION(std::make_shared<MixtureFactor>(
|
||||
THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
|
||||
}
|
||||
|
||||
|
|
@ -195,7 +195,7 @@ TEST(HybridFactorGraph, PushBack) {
|
|||
|
||||
fg = HybridNonlinearFactorGraph();
|
||||
|
||||
auto dcFactor = std::make_shared<MixtureFactor>();
|
||||
auto dcFactor = std::make_shared<HybridNonlinearFactor>();
|
||||
fg.push_back(dcFactor);
|
||||
|
||||
EXPECT_LONGS_EQUAL(fg.size(), 1);
|
||||
|
|
@ -350,7 +350,8 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
|
|||
EliminateHybrid(factors, ordering);
|
||||
|
||||
auto gaussianConditionalMixture =
|
||||
dynamic_pointer_cast<GaussianMixture>(hybridConditionalMixture->inner());
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
hybridConditionalMixture->inner());
|
||||
|
||||
CHECK(gaussianConditionalMixture);
|
||||
// Frontals = [x0, x1]
|
||||
|
|
@ -413,7 +414,8 @@ TEST(HybridFactorGraph, PrintErrors) {
|
|||
// fg.print();
|
||||
// std::cout << "\n\n\n" << std::endl;
|
||||
// fg.printErrors(
|
||||
// HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint));
|
||||
// HybridValues(hv.continuous(), DiscreteValues(),
|
||||
// self.linearizationPoint));
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
|
|
@ -510,7 +512,7 @@ factor 0:
|
|||
b = [ -10 ]
|
||||
No noise model
|
||||
factor 1:
|
||||
GaussianMixtureFactor
|
||||
HybridGaussianFactor
|
||||
Hybrid [x0 x1; m0]{
|
||||
Choice(m0)
|
||||
0 Leaf :
|
||||
|
|
@ -535,7 +537,7 @@ Hybrid [x0 x1; m0]{
|
|||
|
||||
}
|
||||
factor 2:
|
||||
GaussianMixtureFactor
|
||||
HybridGaussianFactor
|
||||
Hybrid [x1 x2; m1]{
|
||||
Choice(m1)
|
||||
0 Leaf :
|
||||
|
|
@ -800,7 +802,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
|
|||
moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
|
||||
noise_model);
|
||||
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
|
||||
fg.emplace_shared<MixtureFactor>(
|
||||
fg.emplace_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
|
||||
|
||||
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
|
||||
|
|
|
|||
|
|
@ -151,23 +151,26 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
|
|||
.BaseEliminateable::eliminatePartialMultifrontal(ordering);
|
||||
|
||||
// The densities on X(1) should be the same
|
||||
auto x0_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
bayesTree[X(0)]->conditional()->inner());
|
||||
auto expected_x0_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto expected_x0_conditional =
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
(*expectedHybridBayesTree)[X(0)]->conditional()->inner());
|
||||
EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional));
|
||||
|
||||
// The densities on X(1) should be the same
|
||||
auto x1_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
bayesTree[X(1)]->conditional()->inner());
|
||||
auto expected_x1_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto expected_x1_conditional =
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
(*expectedHybridBayesTree)[X(1)]->conditional()->inner());
|
||||
EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional));
|
||||
|
||||
// The densities on X(2) should be the same
|
||||
auto x2_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
bayesTree[X(2)]->conditional()->inner());
|
||||
auto expected_x2_conditional = dynamic_pointer_cast<GaussianMixture>(
|
||||
auto expected_x2_conditional =
|
||||
dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
(*expectedHybridBayesTree)[X(2)]->conditional()->inner());
|
||||
EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));
|
||||
|
||||
|
|
@ -300,9 +303,9 @@ TEST(HybridNonlinearISAM, Approx_inference) {
|
|||
|
||||
// Check that the hybrid nodes of the bayes net match those of the pre-pruning
|
||||
// bayes net, at the same positions.
|
||||
auto &unprunedLastDensity = *dynamic_pointer_cast<GaussianMixture>(
|
||||
auto &unprunedLastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
unprunedHybridBayesTree->clique(X(3))->conditional()->inner());
|
||||
auto &lastDensity = *dynamic_pointer_cast<GaussianMixture>(
|
||||
auto &lastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
|
||||
bayesTree[X(3)]->conditional()->inner());
|
||||
|
||||
std::vector<std::pair<DiscreteValues, double>> assignments =
|
||||
|
|
@ -437,7 +440,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
|
|||
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
|
||||
noise_model);
|
||||
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
|
||||
auto mixtureFactor = std::make_shared<MixtureFactor>(
|
||||
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
|
||||
fg.push_back(mixtureFactor);
|
||||
|
||||
|
|
@ -477,7 +480,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
|
|||
moving =
|
||||
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
|
||||
components = {moving, still};
|
||||
mixtureFactor = std::make_shared<MixtureFactor>(
|
||||
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
|
||||
fg.push_back(mixtureFactor);
|
||||
|
||||
|
|
@ -520,7 +523,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
|
|||
moving =
|
||||
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
|
||||
components = {moving, still};
|
||||
mixtureFactor = std::make_shared<MixtureFactor>(
|
||||
mixtureFactor = std::make_shared<HybridNonlinearFactor>(
|
||||
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
|
||||
fg.push_back(mixtureFactor);
|
||||
|
||||
|
|
|
|||
|
|
@ -18,11 +18,11 @@
|
|||
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridBayesTree.h>
|
||||
#include <gtsam/hybrid/HybridConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianConditional.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
||||
|
|
@ -51,29 +51,30 @@ BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
|
|||
BOOST_CLASS_EXPORT_GUID(ADT::Leaf, "gtsam_AlgebraicDecisionTree_Leaf");
|
||||
BOOST_CLASS_EXPORT_GUID(ADT::Choice, "gtsam_AlgebraicDecisionTree_Choice")
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixtureFactor, "gtsam_GaussianMixtureFactor");
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixtureFactor::Factors,
|
||||
"gtsam_GaussianMixtureFactor_Factors");
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixtureFactor::Factors::Leaf,
|
||||
"gtsam_GaussianMixtureFactor_Factors_Leaf");
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixtureFactor::Factors::Choice,
|
||||
"gtsam_GaussianMixtureFactor_Factors_Choice");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor, "gtsam_HybridGaussianFactor");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors,
|
||||
"gtsam_HybridGaussianFactor_Factors");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf,
|
||||
"gtsam_HybridGaussianFactor_Factors_Leaf");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice,
|
||||
"gtsam_HybridGaussianFactor_Factors_Choice");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixture, "gtsam_GaussianMixture");
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixture::Conditionals,
|
||||
"gtsam_GaussianMixture_Conditionals");
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixture::Conditionals::Leaf,
|
||||
"gtsam_GaussianMixture_Conditionals_Leaf");
|
||||
BOOST_CLASS_EXPORT_GUID(GaussianMixture::Conditionals::Choice,
|
||||
"gtsam_GaussianMixture_Conditionals_Choice");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional,
|
||||
"gtsam_HybridGaussianConditional");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals,
|
||||
"gtsam_HybridGaussianConditional_Conditionals");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf,
|
||||
"gtsam_HybridGaussianConditional_Conditionals_Leaf");
|
||||
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Choice,
|
||||
"gtsam_HybridGaussianConditional_Conditionals_Choice");
|
||||
// Needed since GaussianConditional::FromMeanAndStddev uses it
|
||||
BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
|
||||
|
||||
/* ****************************************************************************/
|
||||
// Test GaussianMixtureFactor serialization.
|
||||
TEST(HybridSerialization, GaussianMixtureFactor) {
|
||||
// Test HybridGaussianFactor serialization.
|
||||
TEST(HybridSerialization, HybridGaussianFactor) {
|
||||
KeyVector continuousKeys{X(0)};
|
||||
DiscreteKeys discreteKeys{{M(0), 2}};
|
||||
|
||||
|
|
@ -84,11 +85,11 @@ TEST(HybridSerialization, GaussianMixtureFactor) {
|
|||
auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
|
||||
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
const GaussianMixtureFactor factor(continuousKeys, discreteKeys, factors);
|
||||
const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors);
|
||||
|
||||
EXPECT(equalsObj<GaussianMixtureFactor>(factor));
|
||||
EXPECT(equalsXML<GaussianMixtureFactor>(factor));
|
||||
EXPECT(equalsBinary<GaussianMixtureFactor>(factor));
|
||||
EXPECT(equalsObj<HybridGaussianFactor>(factor));
|
||||
EXPECT(equalsXML<HybridGaussianFactor>(factor));
|
||||
EXPECT(equalsBinary<HybridGaussianFactor>(factor));
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
|
|
@ -106,20 +107,20 @@ TEST(HybridSerialization, HybridConditional) {
|
|||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
// Test GaussianMixture serialization.
|
||||
TEST(HybridSerialization, GaussianMixture) {
|
||||
// Test HybridGaussianConditional serialization.
|
||||
TEST(HybridSerialization, HybridGaussianConditional) {
|
||||
const DiscreteKey mode(M(0), 2);
|
||||
Matrix1 I = Matrix1::Identity();
|
||||
const auto conditional0 = std::make_shared<GaussianConditional>(
|
||||
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 GaussianMixture gm({Z(0)}, {X(0)}, {mode},
|
||||
const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode},
|
||||
{conditional0, conditional1});
|
||||
|
||||
EXPECT(equalsObj<GaussianMixture>(gm));
|
||||
EXPECT(equalsXML<GaussianMixture>(gm));
|
||||
EXPECT(equalsBinary<GaussianMixture>(gm));
|
||||
EXPECT(equalsObj<HybridGaussianConditional>(gm));
|
||||
EXPECT(equalsXML<HybridGaussianConditional>(gm));
|
||||
EXPECT(equalsBinary<HybridGaussianConditional>(gm));
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ namespace gtsam {
|
|||
* Gaussian density over a set of continuous variables.
|
||||
* - \b Discrete conditionals, implemented in \class DiscreteConditional, which
|
||||
* represent a discrete conditional distribution over discrete variables.
|
||||
* - \b Hybrid conditional densities, such as \class GaussianMixture, which is
|
||||
* - \b Hybrid conditional densities, such as \class HybridGaussianConditional, which is
|
||||
* a density over continuous variables given discrete/continuous parents.
|
||||
* - \b Symbolic factors, used to represent a graph structure, implemented in
|
||||
* \class SymbolicConditional. Only used for symbolic elimination etc.
|
||||
|
|
|
|||
|
|
@ -18,8 +18,9 @@ from gtsam.symbol_shorthand import A, X
|
|||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues,
|
||||
GaussianConditional, GaussianMixture, HybridBayesNet,
|
||||
HybridValues, VectorValues, noiseModel)
|
||||
GaussianConditional, HybridBayesNet,
|
||||
HybridGaussianConditional, HybridValues, VectorValues,
|
||||
noiseModel)
|
||||
|
||||
|
||||
class TestHybridBayesNet(GtsamTestCase):
|
||||
|
|
@ -49,7 +50,7 @@ class TestHybridBayesNet(GtsamTestCase):
|
|||
bayesNet = HybridBayesNet()
|
||||
bayesNet.push_back(conditional)
|
||||
bayesNet.push_back(
|
||||
GaussianMixture([X(1)], [], discrete_keys,
|
||||
HybridGaussianConditional([X(1)], [], discrete_keys,
|
||||
[conditional0, conditional1]))
|
||||
bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
|
||||
|
||||
|
|
|
|||
|
|
@ -18,15 +18,16 @@ from gtsam.utils.test_case import GtsamTestCase
|
|||
|
||||
import gtsam
|
||||
from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
|
||||
GaussianMixture, GaussianMixtureFactor, HybridBayesNet,
|
||||
HybridGaussianFactorGraph, HybridValues, JacobianFactor,
|
||||
Ordering, noiseModel)
|
||||
HybridBayesNet, HybridGaussianConditional,
|
||||
HybridGaussianFactor, HybridGaussianFactorGraph,
|
||||
HybridValues, JacobianFactor, Ordering, noiseModel)
|
||||
|
||||
DEBUG_MARGINALS = False
|
||||
|
||||
|
||||
class TestHybridGaussianFactorGraph(GtsamTestCase):
|
||||
"""Unit tests for HybridGaussianFactorGraph."""
|
||||
|
||||
def test_create(self):
|
||||
"""Test construction of hybrid factor graph."""
|
||||
model = noiseModel.Unit.Create(3)
|
||||
|
|
@ -36,7 +37,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 = GaussianMixtureFactor([X(0)], dk, [jf1, jf2])
|
||||
gmf = HybridGaussianFactor([X(0)], dk, [jf1, jf2])
|
||||
|
||||
hfg = HybridGaussianFactorGraph()
|
||||
hfg.push_back(jf1)
|
||||
|
|
@ -48,7 +49,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
|
|||
self.assertEqual(hbn.size(), 2)
|
||||
|
||||
mixture = hbn.at(0).inner()
|
||||
self.assertIsInstance(mixture, GaussianMixture)
|
||||
self.assertIsInstance(mixture, HybridGaussianConditional)
|
||||
self.assertEqual(len(mixture.keys()), 2)
|
||||
|
||||
discrete_conditional = hbn.at(hbn.size() - 1).inner()
|
||||
|
|
@ -63,7 +64,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 = GaussianMixtureFactor([X(0)], dk, [jf1, jf2])
|
||||
gmf = HybridGaussianFactor([X(0)], dk, [jf1, jf2])
|
||||
|
||||
hfg = HybridGaussianFactorGraph()
|
||||
hfg.push_back(jf1)
|
||||
|
|
@ -106,7 +107,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
|
|||
I_1x1,
|
||||
X(0), [0],
|
||||
sigma=3)
|
||||
bayesNet.push_back(GaussianMixture([Z(i)], [X(0)], keys,
|
||||
bayesNet.push_back(
|
||||
HybridGaussianConditional([Z(i)], [X(0)], keys,
|
||||
[conditional0, conditional1]))
|
||||
|
||||
# Create prior on X(0).
|
||||
|
|
@ -219,8 +221,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
|
|||
# Check ratio between unnormalized posterior and factor graph is the same for all modes:
|
||||
for mode in [1, 0]:
|
||||
values.insert_or_assign(M(0), mode)
|
||||
self.assertAlmostEqual(bayesNet.evaluate(values) /
|
||||
np.exp(-fg.error(values)),
|
||||
self.assertAlmostEqual(
|
||||
bayesNet.evaluate(values) / np.exp(-fg.error(values)),
|
||||
0.6366197723675815)
|
||||
self.assertAlmostEqual(bayesNet.error(values), fg.error(values))
|
||||
|
||||
|
|
|
|||
|
|
@ -17,24 +17,26 @@ import unittest
|
|||
import numpy as np
|
||||
from gtsam.symbol_shorthand import C, X
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
from gtsam import BetweenFactorPoint3, noiseModel, PriorFactorPoint3, Point3
|
||||
|
||||
import gtsam
|
||||
from gtsam import BetweenFactorPoint3, Point3, PriorFactorPoint3, noiseModel
|
||||
|
||||
|
||||
class TestHybridGaussianFactorGraph(GtsamTestCase):
|
||||
"""Unit tests for HybridGaussianFactorGraph."""
|
||||
|
||||
def test_nonlinear_hybrid(self):
|
||||
nlfg = gtsam.HybridNonlinearFactorGraph()
|
||||
dk = gtsam.DiscreteKeys()
|
||||
dk.push_back((10, 2))
|
||||
nlfg.push_back(BetweenFactorPoint3(1, 2, Point3(
|
||||
1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1])))
|
||||
nlfg.push_back(
|
||||
BetweenFactorPoint3(1, 2, Point3(1, 2, 3),
|
||||
noiseModel.Diagonal.Variances([1, 1, 1])))
|
||||
nlfg.push_back(
|
||||
PriorFactorPoint3(2, Point3(1, 2, 3),
|
||||
noiseModel.Diagonal.Variances([0.5, 0.5, 0.5])))
|
||||
nlfg.push_back(
|
||||
gtsam.MixtureFactor([1], dk, [
|
||||
gtsam.HybridNonlinearFactor([1], dk, [
|
||||
PriorFactorPoint3(1, Point3(0, 0, 0),
|
||||
noiseModel.Unit.Create(3)),
|
||||
PriorFactorPoint3(1, Point3(1, 2, 1),
|
||||
|
|
|
|||
|
|
@ -14,11 +14,12 @@ from __future__ import print_function
|
|||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import C, X
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
class TestHybridValues(GtsamTestCase):
|
||||
"""Unit tests for HybridValues."""
|
||||
|
|
|
|||
Loading…
Reference in New Issue