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

rename MixtureFactor to HybridNonlinearFactor

release/4.3a0
Varun Agrawal 2024-09-13 05:40:00 -04:00
parent 6a92db62c3
commit 187935407c
14 changed files with 68 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/hybrid/GaussianMixture.cpp
View File

@ -72,7 +72,7 @@ GaussianMixture::GaussianMixture(
/* *******************************************************************************/ /* *******************************************************************************/
// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from // TODO(dellaert): This is copy/paste: GaussianMixture should be derived from
// HybridGaussianFactor, no? // GaussianMixtureFactor, no?
GaussianFactorGraphTree GaussianMixture::add( GaussianFactorGraphTree GaussianMixture::add(
const GaussianFactorGraphTree &sum) const { const GaussianFactorGraphTree &sum) const {
using Y = GaussianFactorGraph; using Y = GaussianFactorGraph;
@ -203,7 +203,7 @@ bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::shared_ptr<HybridGaussianFactor> GaussianMixture::likelihood( std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
const VectorValues &given) const { const VectorValues &given) const {
if (!allFrontalsGiven(given)) { if (!allFrontalsGiven(given)) {
throw std::runtime_error( throw std::runtime_error(
@ -212,7 +212,7 @@ std::shared_ptr<HybridGaussianFactor> GaussianMixture::likelihood(
const DiscreteKeys discreteParentKeys = discreteKeys(); const DiscreteKeys discreteParentKeys = discreteKeys();
const KeyVector continuousParentKeys = continuousParents(); const KeyVector continuousParentKeys = continuousParents();
const HybridGaussianFactor::Factors likelihoods( const GaussianMixtureFactor::Factors likelihoods(
conditionals_, [&](const GaussianConditional::shared_ptr &conditional) { conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
const auto likelihood_m = conditional->likelihood(given); const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm = const double Cgm_Kgcm =
@ -231,7 +231,7 @@ std::shared_ptr<HybridGaussianFactor> GaussianMixture::likelihood(
return std::make_shared<JacobianFactor>(gfg); return std::make_shared<JacobianFactor>(gfg);
} }
}); });
return std::make_shared<HybridGaussianFactor>( return std::make_shared<GaussianMixtureFactor>(
continuousParentKeys, discreteParentKeys, likelihoods); continuousParentKeys, discreteParentKeys, likelihoods);
} }

2
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -146,7 +146,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
/// Getter for GaussianFactor decision tree /// Getter for GaussianFactor decision tree
const Factors &factors() const { return factors_; } const Factors &factors() const { return factors_; }
/// Add MixtureFactor to a Sum, syntactic sugar. /// Add HybridNonlinearFactor to a Sum, syntactic sugar.
friend GaussianFactorGraphTree &operator+=( friend GaussianFactorGraphTree &operator+=(
GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) { GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) {
sum = factor.add(sum); sum = factor.add(sum);

2
gtsam/hybrid/HybridFactor.h
View File

@ -45,7 +45,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
* Base class for *truly* hybrid probabilistic factors * Base class for *truly* hybrid probabilistic factors
* *
* Examples: * Examples:
* - MixtureFactor * - HybridNonlinearFactor
* - HybridGaussianFactor * - HybridGaussianFactor
* - GaussianMixture * - GaussianMixture
* *

6
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/hybrid/GaussianMixture.h> #include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h> #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -59,7 +59,7 @@ void HybridNonlinearFactorGraph::printErrors(
// Clear the stringstream // Clear the stringstream
ss.str(std::string()); ss.str(std::string());
if (auto mf = std::dynamic_pointer_cast<MixtureFactor>(factor)) { if (auto mf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
if (factor == nullptr) { if (factor == nullptr) {
std::cout << "nullptr" std::cout << "nullptr"
<< "\n"; << "\n";
@ -151,7 +151,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
continue; continue;
} }
// Check if it is a nonlinear mixture factor // Check if it is a nonlinear mixture factor
if (auto mf = dynamic_pointer_cast<MixtureFactor>(f)) { if (auto mf = dynamic_pointer_cast<HybridNonlinearFactor>(f)) {
const HybridGaussianFactor::shared_ptr& gmf = const HybridGaussianFactor::shared_ptr& gmf =
mf->linearize(continuousValues); mf->linearize(continuousValues);
linearFG->push_back(gmf); linearFG->push_back(gmf);

28
gtsam/hybrid/MixtureFactor.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file MixtureFactor.h * @file HybridNonlinearFactor.h
* @brief Nonlinear Mixture factor of continuous and discrete. * @brief Nonlinear Mixture factor of continuous and discrete.
* @author Kevin Doherty, kdoherty@mit.edu * @author Kevin Doherty, kdoherty@mit.edu
* @author Varun Agrawal * @author Varun Agrawal
@ -44,11 +44,11 @@ namespace gtsam {
* one of (NonlinearFactor, GaussianFactor) which can then be checked to perform * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
* the correct operation. * the correct operation.
*/ */
class MixtureFactor : public HybridFactor { class HybridNonlinearFactor : public HybridFactor {
public: public:
using Base = HybridFactor; using Base = HybridFactor;
using This = MixtureFactor; using This = HybridNonlinearFactor;
using shared_ptr = std::shared_ptr<MixtureFactor>; using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
using sharedFactor = std::shared_ptr<NonlinearFactor>; using sharedFactor = std::shared_ptr<NonlinearFactor>;
/** /**
@ -63,7 +63,7 @@ class MixtureFactor : public HybridFactor {
bool normalized_; bool normalized_;
public: public:
MixtureFactor() = default; HybridNonlinearFactor() = default;
/** /**
* @brief Construct from Decision tree. * @brief Construct from Decision tree.
@ -74,7 +74,7 @@ class MixtureFactor : public HybridFactor {
* @param normalized Flag indicating if the factor error is already * @param normalized Flag indicating if the factor error is already
* normalized. * normalized.
*/ */
MixtureFactor(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) {}
@ -95,7 +95,7 @@ class MixtureFactor : public HybridFactor {
* normalized. * normalized.
*/ */
template <typename FACTOR> template <typename FACTOR>
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<std::shared_ptr<FACTOR>>& factors, const std::vector<std::shared_ptr<FACTOR>>& factors,
bool normalized = false) bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) { : Base(keys, discreteKeys), normalized_(normalized) {
@ -111,7 +111,7 @@ class MixtureFactor : public HybridFactor {
nonlinear_factors.push_back(nf); nonlinear_factors.push_back(nf);
} else { } else {
throw std::runtime_error( throw std::runtime_error(
"Factors passed into MixtureFactor need to be nonlinear!"); "Factors passed into HybridNonlinearFactor need to be nonlinear!");
} }
} }
factors_ = Factors(discreteKeys, nonlinear_factors); factors_ = Factors(discreteKeys, nonlinear_factors);
@ -124,7 +124,7 @@ class MixtureFactor : public HybridFactor {
} }
/** /**
* @brief Compute error of the MixtureFactor as a tree. * @brief Compute error of the HybridNonlinearFactor as a tree.
* *
* @param continuousValues The continuous values for which to compute the * @param continuousValues The continuous values for which to compute the
* error. * error.
@ -201,15 +201,15 @@ class MixtureFactor : 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 MixtureFactor. If it // We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If it
// fails, return false. // fails, return false.
if (!dynamic_cast<const MixtureFactor*>(&other)) return false; if (!dynamic_cast<const HybridNonlinearFactor*>(&other)) return false;
// If the cast is successful, we'll properly construct a MixtureFactor // If the cast is successful, we'll properly construct a HybridNonlinearFactor
// object from `other` // object from `other`
const MixtureFactor& f(static_cast<const MixtureFactor&>(other)); const HybridNonlinearFactor& f(static_cast<const HybridNonlinearFactor&>(other));
// Ensure that this MixtureFactor and `f` have the same `factors_`. // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
auto compare = [tol](const sharedFactor& a, const sharedFactor& b) { auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
return traits<NonlinearFactor>::Equals(*a, *b, tol); return traits<NonlinearFactor>::Equals(*a, *b, tol);
}; };

10
gtsam/hybrid/hybrid.i
View File

@ -235,15 +235,15 @@ class HybridNonlinearFactorGraph {
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
}; };
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
class MixtureFactor : gtsam::HybridFactor { class HybridNonlinearFactor : gtsam::HybridFactor {
MixtureFactor( HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors, const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors,
bool normalized = false); bool normalized = false);
template <FACTOR = {gtsam::NonlinearFactor}> template <FACTOR = {gtsam::NonlinearFactor}>
MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, HybridNonlinearFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const std::vector<FACTOR*>& factors, const std::vector<FACTOR*>& factors,
bool normalized = false); bool normalized = false);
@ -256,7 +256,7 @@ class MixtureFactor : gtsam::HybridFactor {
HybridGaussianFactor* linearize( HybridGaussianFactor* linearize(
const gtsam::Values& continuousValues) const; const gtsam::Values& continuousValues) const;
void print(string s = "MixtureFactor\n", void print(string s = "HybridNonlinearFactor\n",
const gtsam::KeyFormatter& keyFormatter = const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
}; };

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

@ -22,7 +22,7 @@
#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/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.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>
@ -163,7 +163,7 @@ struct Switching {
for (auto &&f : motion_models) { for (auto &&f : motion_models) {
components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f)); components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f));
} }
nonlinearFactorGraph.emplace_shared<MixtureFactor>( nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
keys, DiscreteKeys{modes[k]}, components); keys, DiscreteKeys{modes[k]}, components);
} }

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

@ -389,7 +389,7 @@ TEST(HybridBayesNet, Sampling) {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion}; std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_shared<MixtureFactor>( nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
DiscreteKey mode(M(0), 2); DiscreteKey mode(M(0), 2);

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

@ -22,7 +22,7 @@
#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/MixtureFactor.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>
@ -435,7 +435,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion = const auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_shared<MixtureFactor>( nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{m}, KeyVector{X(0), X(1)}, DiscreteKeys{m},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion}); std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
@ -532,7 +532,7 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
* the difference in noise models. * the difference in noise models.
*/ */
std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor( std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
const MixtureFactor& mf, const Values& initial, const Key& mode, const HybridNonlinearFactor& mf, const Values& initial, const Key& mode,
double noise_tight, double noise_loose, size_t d, size_t tight_index) { double noise_tight, double noise_loose, size_t d, size_t tight_index) {
HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial); HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial);
@ -592,7 +592,7 @@ TEST(HybridEstimation, ModeSelection) {
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1}; std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)}; KeyVector keys = {X(0), X(1)};
MixtureFactor mf(keys, modes, components); HybridNonlinearFactor mf(keys, modes, components);
initial.insert(X(0), 0.0); initial.insert(X(0), 0.0);
initial.insert(X(1), 0.0); initial.insert(X(1), 0.0);
@ -683,7 +683,7 @@ TEST(HybridEstimation, ModeSelection2) {
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1}; std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)}; KeyVector keys = {X(0), X(1)};
MixtureFactor mf(keys, modes, components); HybridNonlinearFactor mf(keys, modes, components);
initial.insert<Vector3>(X(0), Z_3x1); initial.insert<Vector3>(X(0), Z_3x1);
initial.insert<Vector3>(X(1), Z_3x1); initial.insert<Vector3>(X(1), Z_3x1);

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

@ -418,7 +418,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
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<MixtureFactor>( auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
@ -458,7 +458,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still}; components = {moving, still};
mixtureFactor = std::make_shared<MixtureFactor>( mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
@ -501,7 +501,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still}; components = {moving, still};
mixtureFactor = std::make_shared<MixtureFactor>( mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);

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

@ -24,7 +24,7 @@
#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/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.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>
@ -105,7 +105,7 @@ TEST(HybridNonlinearFactorGraph, Resize) {
auto discreteFactor = std::make_shared<DecisionTreeFactor>(); auto discreteFactor = std::make_shared<DecisionTreeFactor>();
fg.push_back(discreteFactor); fg.push_back(discreteFactor);
auto dcFactor = std::make_shared<MixtureFactor>(); auto dcFactor = std::make_shared<HybridNonlinearFactor>();
fg.push_back(dcFactor); fg.push_back(dcFactor);
EXPECT_LONGS_EQUAL(fg.size(), 3); EXPECT_LONGS_EQUAL(fg.size(), 3);
@ -132,7 +132,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model); moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
std::vector<MotionModel::shared_ptr> components = {still, moving}; std::vector<MotionModel::shared_ptr> components = {still, moving};
auto dcFactor = std::make_shared<MixtureFactor>( auto dcFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
nhfg.push_back(dcFactor); nhfg.push_back(dcFactor);
@ -150,10 +150,10 @@ TEST(HybridGaussianFactorGraph, Resize) {
} }
/*************************************************************************** /***************************************************************************
* Test that the MixtureFactor reports correctly if the number of continuous * Test that the HybridNonlinearFactor reports correctly if the number of continuous
* keys provided do not match the keys in the factors. * keys provided do not match the keys in the factors.
*/ */
TEST(HybridGaussianFactorGraph, MixtureFactor) { TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
auto nonlinearFactor = std::make_shared<BetweenFactor<double>>( auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
auto discreteFactor = std::make_shared<DecisionTreeFactor>(); auto discreteFactor = std::make_shared<DecisionTreeFactor>();
@ -166,12 +166,12 @@ TEST(HybridGaussianFactorGraph, MixtureFactor) {
// Check for exception when number of continuous keys are under-specified. // Check for exception when number of continuous keys are under-specified.
KeyVector contKeys = {X(0)}; KeyVector contKeys = {X(0)};
THROWS_EXCEPTION(std::make_shared<MixtureFactor>( THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
// Check for exception when number of continuous keys are too many. // Check for exception when number of continuous keys are too many.
contKeys = {X(0), X(1), X(2)}; contKeys = {X(0), X(1), X(2)};
THROWS_EXCEPTION(std::make_shared<MixtureFactor>( THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
} }
@ -195,7 +195,7 @@ TEST(HybridFactorGraph, PushBack) {
fg = HybridNonlinearFactorGraph(); fg = HybridNonlinearFactorGraph();
auto dcFactor = std::make_shared<MixtureFactor>(); auto dcFactor = std::make_shared<HybridNonlinearFactor>();
fg.push_back(dcFactor); fg.push_back(dcFactor);
EXPECT_LONGS_EQUAL(fg.size(), 1); EXPECT_LONGS_EQUAL(fg.size(), 1);
@ -800,7 +800,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model); noise_model);
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving}; std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
fg.emplace_shared<MixtureFactor>( fg.emplace_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.

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

@ -437,7 +437,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
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<MixtureFactor>( auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
@ -477,7 +477,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
components = {moving, still}; components = {moving, still};
mixtureFactor = std::make_shared<MixtureFactor>( mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
@ -520,7 +520,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
moving = moving =
std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model); std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
components = {moving, still}; components = {moving, still};
mixtureFactor = std::make_shared<MixtureFactor>( mixtureFactor = std::make_shared<HybridNonlinearFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);

30
gtsam/hybrid/tests/testMixtureFactor.cpp
View File

@ -11,7 +11,7 @@
/** /**
* @file testMixtureFactor.cpp * @file testMixtureFactor.cpp
* @brief Unit tests for MixtureFactor * @brief Unit tests for HybridNonlinearFactor
* @author Varun Agrawal * @author Varun Agrawal
* @date October 2022 * @date October 2022
*/ */
@ -21,7 +21,7 @@
#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/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
@ -36,17 +36,17 @@ using symbol_shorthand::X;
/* ************************************************************************* */ /* ************************************************************************* */
// Check iterators of empty mixture. // Check iterators of empty mixture.
TEST(MixtureFactor, Constructor) { TEST(HybridNonlinearFactor, Constructor) {
MixtureFactor factor; HybridNonlinearFactor factor;
MixtureFactor::const_iterator const_it = factor.begin(); HybridNonlinearFactor::const_iterator const_it = factor.begin();
CHECK(const_it == factor.end()); CHECK(const_it == factor.end());
MixtureFactor::iterator it = factor.begin(); HybridNonlinearFactor::iterator it = factor.begin();
CHECK(it == factor.end()); CHECK(it == factor.end());
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test .print() output. // Test .print() output.
TEST(MixtureFactor, Printing) { TEST(HybridNonlinearFactor, Printing) {
DiscreteKey m1(1, 2); DiscreteKey m1(1, 2);
double between0 = 0.0; double between0 = 0.0;
double between1 = 1.0; double between1 = 1.0;
@ -60,11 +60,11 @@ TEST(MixtureFactor, Printing) {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1}; std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
std::string expected = std::string expected =
R"(Hybrid [x1 x2; 1] R"(Hybrid [x1 x2; 1]
MixtureFactor HybridNonlinearFactor
Choice(1) Choice(1)
0 Leaf Nonlinear factor on 2 keys 0 Leaf Nonlinear factor on 2 keys
1 Leaf Nonlinear factor on 2 keys 1 Leaf Nonlinear factor on 2 keys
@ -73,7 +73,7 @@ MixtureFactor
} }
/* ************************************************************************* */ /* ************************************************************************* */
static MixtureFactor getMixtureFactor() { static HybridNonlinearFactor getMixtureFactor() {
DiscreteKey m1(1, 2); DiscreteKey m1(1, 2);
double between0 = 0.0; double between0 = 0.0;
@ -88,12 +88,12 @@ static MixtureFactor getMixtureFactor() {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1}; std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
return MixtureFactor({X(1), X(2)}, {m1}, factors); return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test the error of the MixtureFactor // Test the error of the HybridNonlinearFactor
TEST(MixtureFactor, Error) { TEST(HybridNonlinearFactor, Error) {
auto mixtureFactor = getMixtureFactor(); auto mixtureFactor = getMixtureFactor();
Values continuousValues; Values continuousValues;
@ -112,8 +112,8 @@ TEST(MixtureFactor, Error) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test dim of the MixtureFactor // Test dim of the HybridNonlinearFactor
TEST(MixtureFactor, Dim) { TEST(HybridNonlinearFactor, Dim) {
auto mixtureFactor = getMixtureFactor(); auto mixtureFactor = getMixtureFactor();
EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
} }

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

@ -17,9 +17,9 @@ import unittest
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
from gtsam import BetweenFactorPoint3, noiseModel, PriorFactorPoint3, Point3
import gtsam import gtsam
from gtsam import BetweenFactorPoint3, Point3, PriorFactorPoint3, noiseModel
class TestHybridGaussianFactorGraph(GtsamTestCase): class TestHybridGaussianFactorGraph(GtsamTestCase):
@ -34,7 +34,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
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])))
nlfg.push_back( nlfg.push_back(
gtsam.MixtureFactor([1], dk, [ gtsam.HybridNonlinearFactor([1], dk, [
PriorFactorPoint3(1, Point3(0, 0, 0), PriorFactorPoint3(1, Point3(0, 0, 0),
noiseModel.Unit.Create(3)), noiseModel.Unit.Create(3)),
PriorFactorPoint3(1, Point3(1, 2, 1), PriorFactorPoint3(1, Point3(1, 2, 1),