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);

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

28
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,7 +60,8 @@ 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,
discreteParents,
Conditionals(discreteParents, conditionals)) {} Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/ /* *******************************************************************************/
@ -67,12 +69,13 @@ 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,
discreteParents,
Conditionals(discreteParents, conditionals)) {} 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;
@ -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());
} }

8
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>
@ -114,7 +114,8 @@ 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(
KeyVector &&continuousFrontals, KeyVector &&continuousParents,
DiscreteKeys &&discreteParents, DiscreteKeys &&discreteParents,
std::vector<GaussianConditional::shared_ptr> &&conditionals); std::vector<GaussianConditional::shared_ptr> &&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

3
gtsam/hybrid/HybridGaussianFactor.h
View File

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

4
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>

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

1
gtsam/hybrid/HybridJunctionTree.h
View File

@ -51,7 +51,6 @@ 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

11
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -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 {

25
gtsam/hybrid/hybrid.i
View File

@ -10,7 +10,7 @@ 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;
@ -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,7 +86,8 @@ 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& continuousFrontals,
const gtsam::KeyVector& continuousParents, const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents, const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>& const std::vector<gtsam::GaussianConditional::shared_ptr>&
@ -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);
@ -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 gtsam::DiscreteKeys& discreteKeys,
const std::vector<FACTOR*>& factors, const std::vector<FACTOR*>& factors,
bool normalized = false); 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 =

2
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>

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);

19
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(
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}),
HybridGaussianConditional::Conditionals( HybridGaussianConditional::Conditionals(
M(0), M(0),
std::make_shared<GaussianConditional>( std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
X(0), Z_3x1, I_3x3, X(1), I_3x3), I_3x3),
std::make_shared<GaussianConditional>( std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
X(0), Vector3::Ones(), I_3x3, X(1), 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");

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

@ -126,30 +126,33 @@ 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 =
dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(0)]->conditional()->inner()); (*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 =
dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(1)]->conditional()->inner()); (*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 =
dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(2)]->conditional()->inner()); (*expectedHybridBayesTree)[X(2)]->conditional()->inner());
EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));

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));
} }
/**************************************************************************** /****************************************************************************

11
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,21 +153,24 @@ 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 =
dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(0)]->conditional()->inner()); (*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 =
dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(1)]->conditional()->inner()); (*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 =
dynamic_pointer_cast<HybridGaussianConditional>(
(*expectedHybridBayesTree)[X(2)]->conditional()->inner()); (*expectedHybridBayesTree)[X(2)]->conditional()->inner());
EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional));

7
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,

8
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,7 +107,8 @@ 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(
HybridGaussianConditional([Z(i)], [X(0)], keys,
[conditional0, conditional1])) [conditional0, conditional1]))
# Create prior on X(0). # Create prior on X(0).
@ -219,8 +221,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
# Check ratio between unnormalized posterior and factor graph is the same for all modes: # 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))

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