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

formatting

release/4.3a0
Varun Agrawal 2024-09-13 06:20:46 -04:00
parent 035c92a38f
commit 629989f9ee
28 changed files with 212 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -182,8 +182,10 @@ 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 prunedHybridGaussianConditional = std::make_shared<HybridGaussianConditional>(*gm);
prunedHybridGaussianConditional->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(prunedHybridGaussianConditional);

4
gtsam/hybrid/HybridConditional.cpp
View File

@ -157,10 +157,10 @@ double HybridConditional::logNormalizationConstant() const {
return gc->logNormalizationConstant();
}
if (auto gm = asMixture()) {
return gm->logNormalizationConstant(); // 0.0!
return gm->logNormalizationConstant(); // 0.0!
}
if (auto dc = asDiscrete()) {
return dc->logNormalizationConstant(); // 0.0!
return dc->logNormalizationConstant(); // 0.0!
}
throw std::runtime_error(
"HybridConditional::logProbability: conditional type not handled");

11
gtsam/hybrid/HybridConditional.h
View File

@ -18,8 +18,8 @@
#pragma once
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/HybridGaussianConditional.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>
@ -127,7 +127,8 @@ class GTSAM_EXPORT HybridConditional
* @param gaussianMixture Gaussian Mixture Conditional used to create the
* HybridConditional.
*/
HybridConditional(const std::shared_ptr<HybridGaussianConditional>& gaussianMixture);
HybridConditional(
const std::shared_ptr<HybridGaussianConditional>& gaussianMixture);
/// @}
/// @name Testable
@ -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<HybridGaussianConditional, Factor>(
static_cast<HybridGaussianConditional*>(NULL), static_cast<Factor*>(NULL));
boost::serialization::void_cast_register<HybridGaussianConditional,
Factor>(
static_cast<HybridGaussianConditional*>(NULL),
static_cast<Factor*>(NULL));
}
}
#endif

4
gtsam/hybrid/HybridEliminationTree.h
View File

@ -35,8 +35,8 @@ class GTSAM_EXPORT HybridEliminationTree
public:
typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
Base; ///< Base class
typedef HybridEliminationTree This; ///< This class
Base; ///< Base class
typedef HybridEliminationTree This; ///< This class
typedef std::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
/// @name Constructors

34
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -50,7 +50,8 @@ HybridGaussianConditional::HybridGaussianConditional(
}
/* *******************************************************************************/
const HybridGaussianConditional::Conditionals &HybridGaussianConditional::conditionals() const {
const HybridGaussianConditional::Conditionals &
HybridGaussianConditional::conditionals() const {
return conditionals_;
}
@ -59,20 +60,22 @@ HybridGaussianConditional::HybridGaussianConditional(
KeyVector &&continuousFrontals, KeyVector &&continuousParents,
DiscreteKeys &&discreteParents,
std::vector<GaussianConditional::shared_ptr> &&conditionals)
: HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents,
Conditionals(discreteParents, conditionals)) {}
: HybridGaussianConditional(continuousFrontals, continuousParents,
discreteParents,
Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents,
Conditionals(discreteParents, conditionals)) {}
: HybridGaussianConditional(continuousFrontals, continuousParents,
discreteParents,
Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/
// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be derived from
// HybridGaussianFactor, no?
// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
// derived from HybridGaussianFactor, no?
GaussianFactorGraphTree HybridGaussianConditional::add(
const GaussianFactorGraphTree &sum) const {
using Y = GaussianFactorGraph;
@ -86,7 +89,8 @@ GaussianFactorGraphTree HybridGaussianConditional::add(
}
/* *******************************************************************************/
GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() const {
GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
const {
auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
// First check if conditional has not been pruned
if (gc) {
@ -131,7 +135,8 @@ GaussianConditional::shared_ptr HybridGaussianConditional::operator()(
}
/* *******************************************************************************/
bool HybridGaussianConditional::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;
@ -150,7 +155,7 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf, double tol) const
/* *******************************************************************************/
void HybridGaussianConditional::print(const std::string &s,
const KeyFormatter &formatter) const {
const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n");
if (isContinuous()) std::cout << "Continuous ";
if (isDiscrete()) std::cout << "Discrete ";
@ -193,7 +198,8 @@ KeyVector HybridGaussianConditional::continuousParents() const {
}
/* ************************************************************************* */
bool HybridGaussianConditional::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;
@ -207,7 +213,8 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const VectorValues &given) const {
if (!allFrontalsGiven(given)) {
throw std::runtime_error(
"HybridGaussianConditional::likelihood: given values are missing some frontals.");
"HybridGaussianConditional::likelihood: given values are missing some "
"frontals.");
}
const DiscreteKeys discreteParentKeys = discreteKeys();
@ -365,7 +372,8 @@ double HybridGaussianConditional::error(const HybridValues &values) const {
}
/* *******************************************************************************/
double HybridGaussianConditional::logProbability(const HybridValues &values) const {
double HybridGaussianConditional::logProbability(
const HybridValues &values) const {
auto conditional = conditionals_(values.discrete());
return conditional->logProbability(values.continuous());
}

18
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -23,8 +23,8 @@
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/linear/GaussianConditional.h>
@ -102,9 +102,9 @@ class GTSAM_EXPORT HybridGaussianConditional
* discreteParents will be used as the labels in the decision tree.
*/
HybridGaussianConditional(const KeyVector &continuousFrontals,
const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const Conditionals &conditionals);
const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const Conditionals &conditionals);
/**
* @brief Make a Gaussian Mixture from a list of Gaussian conditionals
@ -114,9 +114,10 @@ class GTSAM_EXPORT HybridGaussianConditional
* @param discreteParents Discrete parents variables
* @param conditionals List of conditionals
*/
HybridGaussianConditional(KeyVector &&continuousFrontals, KeyVector &&continuousParents,
DiscreteKeys &&discreteParents,
std::vector<GaussianConditional::shared_ptr> &&conditionals);
HybridGaussianConditional(
KeyVector &&continuousFrontals, KeyVector &&continuousParents,
DiscreteKeys &&discreteParents,
std::vector<GaussianConditional::shared_ptr> &&conditionals);
/**
* @brief Make a Gaussian Mixture from a list of Gaussian conditionals
@ -277,6 +278,7 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
// traits
template <>
struct traits<HybridGaussianConditional> : public Testable<HybridGaussianConditional> {};
struct traits<HybridGaussianConditional>
: public Testable<HybridGaussianConditional> {};
} // namespace gtsam

6
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -30,8 +30,8 @@ namespace gtsam {
/* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const Factors &factors)
const DiscreteKeys &discreteKeys,
const Factors &factors)
: Base(continuousKeys, discreteKeys), factors_(factors) {}
/* *******************************************************************************/
@ -53,7 +53,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
/* *******************************************************************************/
void HybridGaussianFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n");
std::cout << "HybridGaussianFactor" << std::endl;
HybridFactor::print("", formatter);

13
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -84,8 +84,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* as the mixture density.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const Factors &factors);
const DiscreteKeys &discreteKeys,
const Factors &factors);
/**
* @brief Construct a new HybridGaussianFactor object using a vector of
@ -96,10 +96,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* @param factors Vector of gaussian factor shared pointers.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const std::vector<sharedFactor> &factors)
const DiscreteKeys &discreteKeys,
const std::vector<sharedFactor> &factors)
: HybridGaussianFactor(continuousKeys, discreteKeys,
Factors(discreteKeys, factors)) {}
Factors(discreteKeys, factors)) {}
/// @}
/// @name Testable
@ -168,7 +168,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
// traits
template <>
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {
};
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
} // namespace gtsam

8
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -23,11 +23,11 @@
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.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>
@ -363,7 +363,7 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
correct);
return std::make_shared<HybridGaussianFactor>(continuousSeparator,
discreteSeparator, newFactors);
discreteSeparator, newFactors);
}
static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
@ -406,7 +406,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
continuousSeparator.empty()
? createDiscreteFactor(eliminationResults, discreteSeparator)
: createHybridGaussianFactor(eliminationResults, continuousSeparator,
discreteSeparator);
discreteSeparator);
// Create the HybridGaussianConditional from the conditionals
HybridGaussianConditional::Conditionals conditionals(

3
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -18,9 +18,9 @@
#pragma once
#include <gtsam/hybrid/HybridGaussianFactor.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

5
gtsam/hybrid/HybridJunctionTree.h
View File

@ -51,11 +51,10 @@ class HybridEliminationTree;
*/
class GTSAM_EXPORT HybridJunctionTree
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
public:
typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
Base; ///< Base class
typedef HybridJunctionTree This; ///< This class
Base; ///< Base class
typedef HybridJunctionTree This; ///< This class
typedef std::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
/**

17
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -75,7 +75,7 @@ class HybridNonlinearFactor : public HybridFactor {
* normalized.
*/
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const Factors& factors, bool normalized = false)
const Factors& factors, bool normalized = false)
: Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}
/**
@ -96,8 +96,8 @@ class HybridNonlinearFactor : public HybridFactor {
*/
template <typename FACTOR>
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<std::shared_ptr<FACTOR>>& factors,
bool normalized = false)
const std::vector<std::shared_ptr<FACTOR>>& factors,
bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) {
std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end());
@ -201,13 +201,14 @@ class HybridNonlinearFactor : public HybridFactor {
/// Check equality
bool equals(const HybridFactor& other, double tol = 1e-9) const override {
// We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If it
// fails, 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 HybridNonlinearFactor
// object from `other`
const HybridNonlinearFactor& f(static_cast<const HybridNonlinearFactor&>(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 HybridNonlinearFactor and `f` have the same `factors_`.
auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {

5
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -20,8 +20,8 @@
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -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<HybridGaussianConditional>(factor)) {
} else if (auto gm = std::dynamic_pointer_cast<HybridGaussianConditional>(
factor)) {
if (factor == nullptr) {
std::cout << "nullptr"
<< "\n";

1
gtsam/hybrid/HybridNonlinearISAM.h
View File

@ -19,6 +19,7 @@
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <optional>
namespace gtsam {

51
gtsam/hybrid/hybrid.i
View File

@ -10,26 +10,26 @@ class HybridValues {
gtsam::DiscreteValues discrete() const;
HybridValues();
HybridValues(const gtsam::VectorValues &cv, const gtsam::DiscreteValues &dv);
HybridValues(const gtsam::VectorValues& cv, const gtsam::DiscreteValues& dv);
void print(string s = "HybridValues",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::HybridValues& other, double tol) const;
void insert(gtsam::Key j, int value);
void insert(gtsam::Key j, const gtsam::Vector& value);
void insert_or_assign(gtsam::Key j, const gtsam::Vector& value);
void insert_or_assign(gtsam::Key j, size_t value);
void insert(const gtsam::VectorValues& values);
void insert(const gtsam::DiscreteValues& values);
void insert(const gtsam::HybridValues& values);
void update(const gtsam::VectorValues& values);
void update(const gtsam::DiscreteValues& values);
void update(const gtsam::HybridValues& values);
size_t& atDiscrete(gtsam::Key j);
gtsam::Vector& at(gtsam::Key j);
};
@ -42,7 +42,7 @@ virtual class HybridFactor : gtsam::Factor {
bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const;
// Standard interface:
double error(const gtsam::HybridValues &values) const;
double error(const gtsam::HybridValues& values) const;
bool isDiscrete() const;
bool isContinuous() const;
bool isHybrid() const;
@ -86,11 +86,12 @@ class HybridGaussianFactor : gtsam::HybridFactor {
#include <gtsam/hybrid/HybridGaussianConditional.h>
class HybridGaussianConditional : gtsam::HybridFactor {
HybridGaussianConditional(const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
conditionalsList);
HybridGaussianConditional(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
conditionalsList);
gtsam::HybridGaussianFactor* likelihood(
const gtsam::VectorValues& frontals) const;
@ -139,7 +140,7 @@ class HybridBayesNet {
size_t size() const;
gtsam::KeySet keys() const;
const gtsam::HybridConditional* at(size_t i) const;
// Standard interface:
double logProbability(const gtsam::HybridValues& values) const;
double evaluate(const gtsam::HybridValues& values) const;
@ -149,7 +150,7 @@ class HybridBayesNet {
const gtsam::VectorValues& measurements) const;
gtsam::HybridValues optimize() const;
gtsam::HybridValues sample(const gtsam::HybridValues &given) const;
gtsam::HybridValues sample(const gtsam::HybridValues& given) const;
gtsam::HybridValues sample() const;
void print(string s = "HybridBayesNet\n",
@ -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);
@ -231,8 +234,8 @@ class HybridNonlinearFactorGraph {
const gtsam::HybridFactor* at(size_t i) const;
void print(string s = "HybridNonlinearFactorGraph\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
};
#include <gtsam/hybrid/HybridNonlinearFactor.h>
@ -243,18 +246,18 @@ class HybridNonlinearFactor : gtsam::HybridFactor {
bool normalized = false);
template <FACTOR = {gtsam::NonlinearFactor}>
HybridNonlinearFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const std::vector<FACTOR*>& factors,
bool normalized = false);
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;
HybridGaussianFactor* linearize(
const gtsam::Values& continuousValues) const;
HybridGaussianFactor* linearize(const gtsam::Values& continuousValues) const;
void print(string s = "HybridNonlinearFactor\n",
const gtsam::KeyFormatter& keyFormatter =

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

@ -21,8 +21,8 @@
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
@ -60,9 +60,9 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
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),
I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
I_3x3, Vector3::Ones())}));
I_3x3, Vector3::Ones())}));
if (t > 1) {
hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},

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

@ -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/HybridNonlinearFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h>

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

@ -20,9 +20,9 @@
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.h>
@ -229,8 +229,8 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1,
c1 = make_shared<GaussianConditional>(z, Vector1(mu1), I_1x1, model1);
HybridBayesNet hbn;
hbn.emplace_shared<HybridGaussianConditional>(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, "0.5/0.5");
hbn.push_back(mixing);

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

@ -21,12 +21,12 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.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
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)));
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)));
hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size());
@ -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<HybridGaussianConditional>(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");

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

@ -126,31 +126,34 @@ TEST(HybridGaussianElimination, IncrementalInference) {
/********************************************************/
// Run batch elimination so we can compare results.
const Ordering ordering {X(0), X(1), X(2)};
const Ordering ordering{X(0), X(1), X(2)};
// Now we calculate the expected factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
// The densities on X(0) should be the same
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());
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<HybridGaussianConditional>(isam[X(1)]->conditional()->inner());
auto expected_x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(1)]->conditional()->inner());
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<HybridGaussianConditional>(isam[X(2)]->conditional()->inner());
auto expected_x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(2)]->conditional()->inner());
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));
// We only perform manual continuous elimination for 0,0.
@ -381,11 +384,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add connecting poses similar to PoseFactors in GTD
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise);
poseNoise);
// Create initial estimate
Values initial;
@ -414,9 +417,9 @@ TEST(HybridGaussianISAM, NonTrivial) {
KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
noise_model),
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
@ -424,14 +427,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise);
poseNoise);
// PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise);
poseNoise);
initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
initial.insert(Y(1), Pose2(1.0, 1.0, 0.0));
@ -454,7 +457,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add odometry factor with discrete modes.
contKeys = {W(1), W(2)};
still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model);
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still};
@ -464,14 +467,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise);
poseNoise);
// PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
poseNoise);
poseNoise);
initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
initial.insert(Y(2), Pose2(2.0, 1.0, 0.0));
@ -497,7 +500,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add odometry factor with discrete modes.
contKeys = {W(2), W(3)};
still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model);
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still};
@ -507,14 +510,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise);
poseNoise);
// PoseFactors-like at k=3
fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
poseNoise);
poseNoise);
initial.insert(X(3), Pose2(3.0, 0.0, 0.0));
initial.insert(Y(3), Pose2(3.0, 1.0, 0.0));

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

@ -20,8 +20,8 @@
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>

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

@ -23,8 +23,8 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -150,8 +150,8 @@ TEST(HybridGaussianFactorGraph, Resize) {
}
/***************************************************************************
* Test that the HybridNonlinearFactor 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, HybridNonlinearFactor) {
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
@ -350,7 +350,8 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
EliminateHybrid(factors, ordering);
auto gaussianConditionalMixture =
dynamic_pointer_cast<HybridGaussianConditional>(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));
}
/****************************************************************************

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

@ -143,7 +143,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
/********************************************************/
// Run batch elimination so we can compare results.
const Ordering ordering {X(0), X(1), X(2)};
const Ordering ordering{X(0), X(1), X(2)};
// Now we calculate the actual factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] =
@ -153,22 +153,25 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
// The densities on X(1) should be the same
auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
bayesTree[X(0)]->conditional()->inner());
auto expected_x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[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<HybridGaussianConditional>(
bayesTree[X(1)]->conditional()->inner());
auto expected_x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[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<HybridGaussianConditional>(
bayesTree[X(2)]->conditional()->inner());
auto expected_x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[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));
// We only perform manual continuous elimination for 0,0.
@ -410,11 +413,11 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add connecting poses similar to PoseFactors in GTD
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise);
poseNoise);
// Create initial estimate
Values initial;
@ -433,9 +436,9 @@ TEST(HybridNonlinearISAM, NonTrivial) {
KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
noise_model),
noise_model),
moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model);
noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
@ -443,14 +446,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise);
poseNoise);
// PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise);
poseNoise);
initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
initial.insert(Y(1), Pose2(1.0, 1.0, 0.0));
@ -473,7 +476,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add odometry factor with discrete modes.
contKeys = {W(1), W(2)};
still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model);
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still};
@ -483,14 +486,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise);
poseNoise);
// PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
poseNoise);
poseNoise);
initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
initial.insert(Y(2), Pose2(2.0, 1.0, 0.0));
@ -516,7 +519,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add odometry factor with discrete modes.
contKeys = {W(2), W(3)};
still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model);
noise_model);
moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still};
@ -526,14 +529,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise);
poseNoise);
// PoseFactors-like at k=3
fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise);
poseNoise);
fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
poseNoise);
poseNoise);
initial.insert(X(3), Pose2(3.0, 0.0, 0.0));
initial.insert(Y(3), Pose2(3.0, 1.0, 0.0));

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

@ -18,11 +18,11 @@
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.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>
@ -59,7 +59,8 @@ BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf,
BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice,
"gtsam_HybridGaussianFactor_Factors_Choice");
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional, "gtsam_HybridGaussianConditional");
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional,
"gtsam_HybridGaussianConditional");
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals,
"gtsam_HybridGaussianConditional_Conditionals");
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf,
@ -115,7 +116,7 @@ TEST(HybridSerialization, HybridGaussianConditional) {
const auto conditional1 = std::make_shared<GaussianConditional>(
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode},
{conditional0, conditional1});
{conditional0, conditional1});
EXPECT(equalsObj<HybridGaussianConditional>(gm));
EXPECT(equalsXML<HybridGaussianConditional>(gm));

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

@ -51,7 +51,7 @@ class TestHybridBayesNet(GtsamTestCase):
bayesNet.push_back(conditional)
bayesNet.push_back(
HybridGaussianConditional([X(1)], [], discrete_keys,
[conditional0, conditional1]))
[conditional0, conditional1]))
bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
# Create values at which to evaluate.

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

@ -27,6 +27,7 @@ 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)
@ -106,8 +107,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
I_1x1,
X(0), [0],
sigma=3)
bayesNet.push_back(HybridGaussianConditional([Z(i)], [X(0)], keys,
[conditional0, conditional1]))
bayesNet.push_back(
HybridGaussianConditional([Z(i)], [X(0)], keys,
[conditional0, conditional1]))
# Create prior on X(0).
prior_on_x0 = GaussianConditional.FromMeanAndStddev(
@ -219,9 +221,9 @@ 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)),
0.6366197723675815)
self.assertAlmostEqual(
bayesNet.evaluate(values) / np.exp(-fg.error(values)),
0.6366197723675815)
self.assertAlmostEqual(bayesNet.error(values), fg.error(values))
# Test elimination.

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

@ -24,12 +24,14 @@ 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])))

3
python/gtsam/tests/test_HybridValues.py
View File

@ -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."""