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) { for (auto &&conditional : *this) {
if (auto gm = conditional->asMixture()) { if (auto gm = conditional->asMixture()) {
// Make a copy of the Gaussian mixture and prune it! // Make a copy of the Gaussian mixture and prune it!
auto prunedHybridGaussianConditional = std::make_shared<HybridGaussianConditional>(*gm); auto prunedHybridGaussianConditional =
prunedHybridGaussianConditional->prune(prunedDiscreteProbs); // imperative :-( std::make_shared<HybridGaussianConditional>(*gm);
prunedHybridGaussianConditional->prune(
prunedDiscreteProbs); // imperative :-(
// Type-erase and add to the pruned Bayes Net fragment. // Type-erase and add to the pruned Bayes Net fragment.
prunedBayesNetFragment.push_back(prunedHybridGaussianConditional); prunedBayesNetFragment.push_back(prunedHybridGaussianConditional);

4
gtsam/hybrid/HybridConditional.cpp
View File

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

11
gtsam/hybrid/HybridConditional.h
View File

@ -18,8 +18,8 @@
#pragma once #pragma once
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -127,7 +127,8 @@ class GTSAM_EXPORT HybridConditional
* @param gaussianMixture Gaussian Mixture Conditional used to create the * @param gaussianMixture Gaussian Mixture Conditional used to create the
* HybridConditional. * HybridConditional.
*/ */
HybridConditional(const std::shared_ptr<HybridGaussianConditional>& gaussianMixture); HybridConditional(
const std::shared_ptr<HybridGaussianConditional>& gaussianMixture);
/// @} /// @}
/// @name Testable /// @name Testable
@ -222,8 +223,10 @@ class GTSAM_EXPORT HybridConditional
boost::serialization::void_cast_register<GaussianConditional, Factor>( boost::serialization::void_cast_register<GaussianConditional, Factor>(
static_cast<GaussianConditional*>(NULL), static_cast<Factor*>(NULL)); static_cast<GaussianConditional*>(NULL), static_cast<Factor*>(NULL));
} else { } else {
boost::serialization::void_cast_register<HybridGaussianConditional, Factor>( boost::serialization::void_cast_register<HybridGaussianConditional,
static_cast<HybridGaussianConditional*>(NULL), static_cast<Factor*>(NULL)); Factor>(
static_cast<HybridGaussianConditional*>(NULL),
static_cast<Factor*>(NULL));
} }
} }
#endif #endif

4
gtsam/hybrid/HybridEliminationTree.h
View File

@ -35,8 +35,8 @@ class GTSAM_EXPORT HybridEliminationTree
public: public:
typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
Base; ///< Base class Base; ///< Base class
typedef HybridEliminationTree This; ///< This class typedef HybridEliminationTree This; ///< This class
typedef std::shared_ptr<This> shared_ptr; ///< Shared pointer to this class typedef std::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
/// @name Constructors /// @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_; return conditionals_;
} }
@ -59,20 +60,22 @@ HybridGaussianConditional::HybridGaussianConditional(
KeyVector &&continuousFrontals, KeyVector &&continuousParents, KeyVector &&continuousFrontals, KeyVector &&continuousParents,
DiscreteKeys &&discreteParents, DiscreteKeys &&discreteParents,
std::vector<GaussianConditional::shared_ptr> &&conditionals) std::vector<GaussianConditional::shared_ptr> &&conditionals)
: HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents, : HybridGaussianConditional(continuousFrontals, continuousParents,
Conditionals(discreteParents, conditionals)) {} discreteParents,
Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/ /* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents, const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents, const DiscreteKeys &discreteParents,
const std::vector<GaussianConditional::shared_ptr> &conditionals) const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents, : HybridGaussianConditional(continuousFrontals, continuousParents,
Conditionals(discreteParents, conditionals)) {} discreteParents,
Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/ /* *******************************************************************************/
// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be derived from // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
// HybridGaussianFactor, no? // derived from HybridGaussianFactor, no?
GaussianFactorGraphTree HybridGaussianConditional::add( GaussianFactorGraphTree HybridGaussianConditional::add(
const GaussianFactorGraphTree &sum) const { const GaussianFactorGraphTree &sum) const {
using Y = GaussianFactorGraph; 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) { auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
// First check if conditional has not been pruned // First check if conditional has not been pruned
if (gc) { 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); const This *e = dynamic_cast<const This *>(&lf);
if (e == nullptr) return false; 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, void HybridGaussianConditional::print(const std::string &s,
const KeyFormatter &formatter) const { const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n"); std::cout << (s.empty() ? "" : s + "\n");
if (isContinuous()) std::cout << "Continuous "; if (isContinuous()) std::cout << "Continuous ";
if (isDiscrete()) std::cout << "Discrete "; 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) { for (auto &&kv : given) {
if (given.find(kv.first) == given.end()) { if (given.find(kv.first) == given.end()) {
return false; return false;
@ -207,7 +213,8 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const VectorValues &given) const { const VectorValues &given) const {
if (!allFrontalsGiven(given)) { if (!allFrontalsGiven(given)) {
throw std::runtime_error( throw std::runtime_error(
"HybridGaussianConditional::likelihood: given values are missing some frontals."); "HybridGaussianConditional::likelihood: given values are missing some "
"frontals.");
} }
const DiscreteKeys discreteParentKeys = discreteKeys(); 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()); auto conditional = conditionals_(values.discrete());
return conditional->logProbability(values.continuous()); return conditional->logProbability(values.continuous());
} }

18
gtsam/hybrid/HybridGaussianConditional.h
View File

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

6
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -30,8 +30,8 @@ namespace gtsam {
/* *******************************************************************************/ /* *******************************************************************************/
HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys, HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys, const DiscreteKeys &discreteKeys,
const Factors &factors) const Factors &factors)
: Base(continuousKeys, discreteKeys), 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, void HybridGaussianFactor::print(const std::string &s,
const KeyFormatter &formatter) const { const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n"); std::cout << (s.empty() ? "" : s + "\n");
std::cout << "HybridGaussianFactor" << std::endl; std::cout << "HybridGaussianFactor" << std::endl;
HybridFactor::print("", formatter); HybridFactor::print("", formatter);

13
gtsam/hybrid/HybridGaussianFactor.h
View File

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

8
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

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

3
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -18,9 +18,9 @@
#pragma once #pragma once
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/EliminateableFactorGraph.h> #include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
@ -221,7 +221,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// Get the GaussianFactorGraph at a given discrete assignment. /// Get the GaussianFactorGraph at a given discrete assignment.
GaussianFactorGraph operator()(const DiscreteValues& assignment) const; GaussianFactorGraph operator()(const DiscreteValues& assignment) const;
}; };
} // namespace gtsam } // namespace gtsam

5
gtsam/hybrid/HybridJunctionTree.h
View File

@ -51,11 +51,10 @@ class HybridEliminationTree;
*/ */
class GTSAM_EXPORT HybridJunctionTree class GTSAM_EXPORT HybridJunctionTree
: public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> { : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
public: public:
typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
Base; ///< Base class Base; ///< Base class
typedef HybridJunctionTree This; ///< This class typedef HybridJunctionTree This; ///< This class
typedef std::shared_ptr<This> shared_ptr; ///< Shared pointer to 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. * normalized.
*/ */
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, 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) {} : Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}
/** /**
@ -96,8 +96,8 @@ class HybridNonlinearFactor : public HybridFactor {
*/ */
template <typename FACTOR> template <typename FACTOR>
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<std::shared_ptr<FACTOR>>& factors, const std::vector<std::shared_ptr<FACTOR>>& factors,
bool normalized = false) bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) { : Base(keys, discreteKeys), normalized_(normalized) {
std::vector<NonlinearFactor::shared_ptr> nonlinear_factors; std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet continuous_keys_set(keys.begin(), keys.end());
@ -201,13 +201,14 @@ class HybridNonlinearFactor : public HybridFactor {
/// Check equality /// Check equality
bool equals(const HybridFactor& other, double tol = 1e-9) const override { bool equals(const HybridFactor& other, double tol = 1e-9) const override {
// We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If it // We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If
// fails, return false. // it fails, return false.
if (!dynamic_cast<const HybridNonlinearFactor*>(&other)) return false; if (!dynamic_cast<const HybridNonlinearFactor*>(&other)) return false;
// If the cast is successful, we'll properly construct a HybridNonlinearFactor // If the cast is successful, we'll properly construct a
// object from `other` // HybridNonlinearFactor object from `other`
const HybridNonlinearFactor& f(static_cast<const HybridNonlinearFactor&>(other)); const HybridNonlinearFactor& f(
static_cast<const HybridNonlinearFactor&>(other));
// Ensure that this HybridNonlinearFactor 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) { 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/discrete/TableFactor.h>
#include <gtsam/hybrid/HybridGaussianConditional.h> #include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -80,7 +80,8 @@ void HybridNonlinearFactorGraph::printErrors(
gmf->errorTree(values.continuous()).print("", keyFormatter); gmf->errorTree(values.continuous()).print("", keyFormatter);
std::cout << std::endl; 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) { if (factor == nullptr) {
std::cout << "nullptr" std::cout << "nullptr"
<< "\n"; << "\n";

1
gtsam/hybrid/HybridNonlinearISAM.h
View File

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

51
gtsam/hybrid/hybrid.i
View File

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

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

@ -21,8 +21,8 @@
#include <gtsam/discrete/DiscreteDistribution.h> #include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
@ -60,9 +60,9 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
hfg.add(HybridGaussianFactor( hfg.add(HybridGaussianFactor(
{keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}},
{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1), {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), std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
I_3x3, Vector3::Ones())})); I_3x3, Vector3::Ones())}));
if (t > 1) { if (t > 1) {
hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, 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/Pose2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h> #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearISAM.h> #include <gtsam/hybrid/HybridNonlinearISAM.h>
#include <gtsam/hybrid/HybridSmoother.h> #include <gtsam/hybrid/HybridSmoother.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.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/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianConditional.h> #include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridValues.h> #include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.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); c1 = make_shared<GaussianConditional>(z, Vector1(mu1), I_1x1, model1);
HybridBayesNet hbn; HybridBayesNet hbn;
hbn.emplace_shared<HybridGaussianConditional>(KeyVector{z}, KeyVector{}, hbn.emplace_shared<HybridGaussianConditional>(
DiscreteKeys{m}, std::vector{c0, c1}); KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1});
auto mixing = make_shared<DiscreteConditional>(m, "0.5/0.5"); auto mixing = make_shared<DiscreteConditional>(m, "0.5/0.5");
hbn.push_back(mixing); hbn.push_back(mixing);

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

@ -21,12 +21,12 @@
#include <gtsam/discrete/DecisionTreeFactor.h> #include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianISAM.h> #include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridValues.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 // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
// graph // graph
HybridGaussianConditional gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), HybridGaussianConditional gm(
HybridGaussianConditional::Conditionals( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}),
M(0), HybridGaussianConditional::Conditionals(
std::make_shared<GaussianConditional>( M(0),
X(0), Z_3x1, I_3x3, X(1), I_3x3), std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
std::make_shared<GaussianConditional>( I_3x3),
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3))); std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
X(1), I_3x3)));
hfg.add(gm); hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size()); EXPECT_LONGS_EQUAL(2, hfg.size());
@ -681,8 +682,8 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) {
x0, -I_1x1, model0), x0, -I_1x1, model0),
c1 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1, c1 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1,
x0, -I_1x1, model1); x0, -I_1x1, model1);
hbn.emplace_shared<HybridGaussianConditional>(KeyVector{f01}, KeyVector{x0, x1}, hbn.emplace_shared<HybridGaussianConditional>(
DiscreteKeys{m1}, std::vector{c0, c1}); KeyVector{f01}, KeyVector{x0, x1}, DiscreteKeys{m1}, std::vector{c0, c1});
// Discrete uniform prior. // Discrete uniform prior.
hbn.emplace_shared<DiscreteConditional>(m1, "0.5/0.5"); 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. // 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 // Now we calculate the expected factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] = const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
// The densities on X(0) should be the same // The densities on X(0) should be the same
auto x0_conditional = auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
dynamic_pointer_cast<HybridGaussianConditional>(isam[X(0)]->conditional()->inner()); isam[X(0)]->conditional()->inner());
auto expected_x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto expected_x0_conditional =
(*expectedHybridBayesTree)[X(0)]->conditional()->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(0)]->conditional()->inner());
EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional)); EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional));
// The densities on X(1) should be the same // The densities on X(1) should be the same
auto x1_conditional = auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
dynamic_pointer_cast<HybridGaussianConditional>(isam[X(1)]->conditional()->inner()); isam[X(1)]->conditional()->inner());
auto expected_x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto expected_x1_conditional =
(*expectedHybridBayesTree)[X(1)]->conditional()->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(1)]->conditional()->inner());
EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional)); EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional));
// The densities on X(2) should be the same // The densities on X(2) should be the same
auto x2_conditional = auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
dynamic_pointer_cast<HybridGaussianConditional>(isam[X(2)]->conditional()->inner()); isam[X(2)]->conditional()->inner());
auto expected_x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto expected_x2_conditional =
(*expectedHybridBayesTree)[X(2)]->conditional()->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(2)]->conditional()->inner());
EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));
// We only perform manual continuous elimination for 0,0. // We only perform manual continuous elimination for 0,0.
@ -381,11 +384,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), 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), 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), fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
// Create initial estimate // Create initial estimate
Values initial; Values initial;
@ -414,9 +417,9 @@ TEST(HybridGaussianISAM, NonTrivial) {
KeyVector contKeys = {W(0), W(1)}; KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 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, moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model); noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still}; std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>( auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
@ -424,14 +427,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), 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), 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), 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(X(1), Pose2(1.0, 0.0, 0.0));
initial.insert(Y(1), Pose2(1.0, 1.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. // Add odometry factor with discrete modes.
contKeys = {W(1), W(2)}; contKeys = {W(1), W(2)};
still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0), still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model); noise_model);
moving = moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still}; components = {moving, still};
@ -464,14 +467,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), 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), 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), 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(X(2), Pose2(2.0, 0.0, 0.0));
initial.insert(Y(2), Pose2(2.0, 1.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. // Add odometry factor with discrete modes.
contKeys = {W(2), W(3)}; contKeys = {W(2), W(3)};
still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0), still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model); noise_model);
moving = moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still}; components = {moving, still};
@ -507,14 +510,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=3 // PoseFactors-like at k=3
fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), 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), 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), 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(X(3), Pose2(3.0, 0.0, 0.0));
initial.insert(Y(3), Pose2(3.0, 1.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/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.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/geometry/Pose2.h>
#include <gtsam/hybrid/HybridEliminationTree.h> #include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -150,8 +150,8 @@ TEST(HybridGaussianFactorGraph, Resize) {
} }
/*************************************************************************** /***************************************************************************
* Test that the HybridNonlinearFactor reports correctly if the number of continuous * Test that the HybridNonlinearFactor reports correctly if the number of
* keys provided do not match the keys in the factors. * continuous keys provided do not match the keys in the factors.
*/ */
TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>( auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
@ -350,7 +350,8 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
EliminateHybrid(factors, ordering); EliminateHybrid(factors, ordering);
auto gaussianConditionalMixture = auto gaussianConditionalMixture =
dynamic_pointer_cast<HybridGaussianConditional>(hybridConditionalMixture->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
hybridConditionalMixture->inner());
CHECK(gaussianConditionalMixture); CHECK(gaussianConditionalMixture);
// Frontals = [x0, x1] // Frontals = [x0, x1]
@ -413,7 +414,8 @@ TEST(HybridFactorGraph, PrintErrors) {
// fg.print(); // fg.print();
// std::cout << "\n\n\n" << std::endl; // std::cout << "\n\n\n" << std::endl;
// fg.printErrors( // 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. // 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 // Now we calculate the actual factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] = const auto [expectedHybridBayesTree, expectedRemainingGraph] =
@ -153,22 +153,25 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
// The densities on X(1) should be the same // The densities on X(1) should be the same
auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
bayesTree[X(0)]->conditional()->inner()); bayesTree[X(0)]->conditional()->inner());
auto expected_x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto expected_x0_conditional =
(*expectedHybridBayesTree)[X(0)]->conditional()->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(0)]->conditional()->inner());
EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional)); EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional));
// The densities on X(1) should be the same // The densities on X(1) should be the same
auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
bayesTree[X(1)]->conditional()->inner()); bayesTree[X(1)]->conditional()->inner());
auto expected_x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto expected_x1_conditional =
(*expectedHybridBayesTree)[X(1)]->conditional()->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(1)]->conditional()->inner());
EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional)); EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional));
// The densities on X(2) should be the same // The densities on X(2) should be the same
auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
bayesTree[X(2)]->conditional()->inner()); bayesTree[X(2)]->conditional()->inner());
auto expected_x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>( auto expected_x2_conditional =
(*expectedHybridBayesTree)[X(2)]->conditional()->inner()); dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(2)]->conditional()->inner());
EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));
// We only perform manual continuous elimination for 0,0. // We only perform manual continuous elimination for 0,0.
@ -410,11 +413,11 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), 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), 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), fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
// Create initial estimate // Create initial estimate
Values initial; Values initial;
@ -433,9 +436,9 @@ TEST(HybridNonlinearISAM, NonTrivial) {
KeyVector contKeys = {W(0), W(1)}; KeyVector contKeys = {W(0), W(1)};
auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 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, moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
noise_model); noise_model);
std::vector<PlanarMotionModel::shared_ptr> components = {moving, still}; std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
auto mixtureFactor = std::make_shared<HybridNonlinearFactor>( auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
@ -443,14 +446,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), 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), 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), 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(X(1), Pose2(1.0, 0.0, 0.0));
initial.insert(Y(1), Pose2(1.0, 1.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. // Add odometry factor with discrete modes.
contKeys = {W(1), W(2)}; contKeys = {W(1), W(2)};
still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0), still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
noise_model); noise_model);
moving = moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still}; components = {moving, still};
@ -483,14 +486,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), 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), 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), 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(X(2), Pose2(2.0, 0.0, 0.0));
initial.insert(Y(2), Pose2(2.0, 1.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. // Add odometry factor with discrete modes.
contKeys = {W(2), W(3)}; contKeys = {W(2), W(3)};
still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0), still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
noise_model); noise_model);
moving = moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still}; components = {moving, still};
@ -526,14 +529,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=3 // PoseFactors-like at k=3
fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), 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), 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), 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(X(3), Pose2(3.0, 0.0, 0.0));
initial.insert(Y(3), Pose2(3.0, 1.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/base/serializationTestHelpers.h>
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h> #include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
@ -59,7 +59,8 @@ BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf,
BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice, BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice,
"gtsam_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, BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals,
"gtsam_HybridGaussianConditional_Conditionals"); "gtsam_HybridGaussianConditional_Conditionals");
BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf, BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf,
@ -115,7 +116,7 @@ TEST(HybridSerialization, HybridGaussianConditional) {
const auto conditional1 = std::make_shared<GaussianConditional>( const auto conditional1 = std::make_shared<GaussianConditional>(
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode},
{conditional0, conditional1}); {conditional0, conditional1});
EXPECT(equalsObj<HybridGaussianConditional>(gm)); EXPECT(equalsObj<HybridGaussianConditional>(gm));
EXPECT(equalsXML<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(conditional)
bayesNet.push_back( bayesNet.push_back(
HybridGaussianConditional([X(1)], [], discrete_keys, HybridGaussianConditional([X(1)], [], discrete_keys,
[conditional0, conditional1])) [conditional0, conditional1]))
bayesNet.push_back(DiscreteConditional(Asia, "99/1")) bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
# Create values at which to evaluate. # 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): class TestHybridGaussianFactorGraph(GtsamTestCase):
"""Unit tests for HybridGaussianFactorGraph.""" """Unit tests for HybridGaussianFactorGraph."""
def test_create(self): def test_create(self):
"""Test construction of hybrid factor graph.""" """Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3) model = noiseModel.Unit.Create(3)
@ -106,8 +107,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
I_1x1, I_1x1,
X(0), [0], X(0), [0],
sigma=3) sigma=3)
bayesNet.push_back(HybridGaussianConditional([Z(i)], [X(0)], keys, bayesNet.push_back(
[conditional0, conditional1])) HybridGaussianConditional([Z(i)], [X(0)], keys,
[conditional0, conditional1]))
# Create prior on X(0). # Create prior on X(0).
prior_on_x0 = GaussianConditional.FromMeanAndStddev( 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: # Check ratio between unnormalized posterior and factor graph is the same for all modes:
for mode in [1, 0]: for mode in [1, 0]:
values.insert_or_assign(M(0), mode) values.insert_or_assign(M(0), mode)
self.assertAlmostEqual(bayesNet.evaluate(values) / self.assertAlmostEqual(
np.exp(-fg.error(values)), bayesNet.evaluate(values) / np.exp(-fg.error(values)),
0.6366197723675815) 0.6366197723675815)
self.assertAlmostEqual(bayesNet.error(values), fg.error(values)) self.assertAlmostEqual(bayesNet.error(values), fg.error(values))
# Test elimination. # 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): class TestHybridGaussianFactorGraph(GtsamTestCase):
"""Unit tests for HybridGaussianFactorGraph.""" """Unit tests for HybridGaussianFactorGraph."""
def test_nonlinear_hybrid(self): def test_nonlinear_hybrid(self):
nlfg = gtsam.HybridNonlinearFactorGraph() nlfg = gtsam.HybridNonlinearFactorGraph()
dk = gtsam.DiscreteKeys() dk = gtsam.DiscreteKeys()
dk.push_back((10, 2)) dk.push_back((10, 2))
nlfg.push_back(BetweenFactorPoint3(1, 2, Point3( nlfg.push_back(
1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1]))) BetweenFactorPoint3(1, 2, Point3(1, 2, 3),
noiseModel.Diagonal.Variances([1, 1, 1])))
nlfg.push_back( nlfg.push_back(
PriorFactorPoint3(2, Point3(1, 2, 3), PriorFactorPoint3(2, Point3(1, 2, 3),
noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) 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 unittest
import gtsam
import numpy as np import numpy as np
from gtsam.symbol_shorthand import C, X from gtsam.symbol_shorthand import C, X
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
import gtsam
class TestHybridValues(GtsamTestCase): class TestHybridValues(GtsamTestCase):
"""Unit tests for HybridValues.""" """Unit tests for HybridValues."""