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
// HybridGaussianFactor, no?
// GaussianMixtureFactor, no?
GaussianFactorGraphTree GaussianMixture::add(
const GaussianFactorGraphTree &sum) const {
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 {
if (!allFrontalsGiven(given)) {
throw std::runtime_error(
@ -212,7 +212,7 @@ std::shared_ptr<HybridGaussianFactor> GaussianMixture::likelihood(
const DiscreteKeys discreteParentKeys = discreteKeys();
const KeyVector continuousParentKeys = continuousParents();
const HybridGaussianFactor::Factors likelihoods(
const GaussianMixtureFactor::Factors likelihoods(
conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm =
@ -231,7 +231,7 @@ std::shared_ptr<HybridGaussianFactor> GaussianMixture::likelihood(
return std::make_shared<JacobianFactor>(gfg);
}
});
return std::make_shared<HybridGaussianFactor>(
return std::make_shared<GaussianMixtureFactor>(
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
const Factors &factors() const { return factors_; }
/// Add MixtureFactor to a Sum, syntactic sugar.
/// Add HybridNonlinearFactor to a Sum, syntactic sugar.
friend GaussianFactorGraphTree &operator+=(
GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) {
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
*
* Examples:
* - MixtureFactor
* - HybridNonlinearFactor
* - HybridGaussianFactor
* - GaussianMixture
*

6
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -59,7 +59,7 @@ void HybridNonlinearFactorGraph::printErrors(
// Clear the stringstream
ss.str(std::string());
if (auto mf = std::dynamic_pointer_cast<MixtureFactor>(factor)) {
if (auto mf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
if (factor == nullptr) {
std::cout << "nullptr"
<< "\n";
@ -151,7 +151,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
continue;
}
// 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 =
mf->linearize(continuousValues);
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.
* @author Kevin Doherty, kdoherty@mit.edu
* @author Varun Agrawal
@ -44,11 +44,11 @@ namespace gtsam {
* one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
* the correct operation.
*/
class MixtureFactor : public HybridFactor {
class HybridNonlinearFactor : public HybridFactor {
public:
using Base = HybridFactor;
using This = MixtureFactor;
using shared_ptr = std::shared_ptr<MixtureFactor>;
using This = HybridNonlinearFactor;
using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
using sharedFactor = std::shared_ptr<NonlinearFactor>;
/**
@ -63,7 +63,7 @@ class MixtureFactor : public HybridFactor {
bool normalized_;
public:
MixtureFactor() = default;
HybridNonlinearFactor() = default;
/**
* @brief Construct from Decision tree.
@ -74,7 +74,7 @@ class MixtureFactor : public HybridFactor {
* @param normalized Flag indicating if the factor error is already
* normalized.
*/
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const Factors& factors, bool normalized = false)
: Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}
@ -95,7 +95,7 @@ class MixtureFactor : public HybridFactor {
* normalized.
*/
template <typename FACTOR>
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<std::shared_ptr<FACTOR>>& factors,
bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) {
@ -111,7 +111,7 @@ class MixtureFactor : public HybridFactor {
nonlinear_factors.push_back(nf);
} else {
throw std::runtime_error(
"Factors passed into MixtureFactor need to be nonlinear!");
"Factors passed into HybridNonlinearFactor need to be nonlinear!");
}
}
factors_ = Factors(discreteKeys, nonlinear_factors);
@ -124,7 +124,7 @@ class MixtureFactor : public HybridFactor {
}
/**
* @brief Compute error of the MixtureFactor as a tree.
* @brief Compute error of the HybridNonlinearFactor as a tree.
*
* @param continuousValues The continuous values for which to compute the
* error.
@ -201,15 +201,15 @@ class MixtureFactor : public HybridFactor {
/// Check equality
bool equals(const HybridFactor& other, double tol = 1e-9) const override {
// We attempt a dynamic cast from HybridFactor to MixtureFactor. If it
// We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If it
// 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`
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) {
return traits<NonlinearFactor>::Equals(*a, *b, tol);
};

10
gtsam/hybrid/hybrid.i
View File

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

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

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

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::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_shared<MixtureFactor>(
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
DiscreteKey mode(M(0), 2);

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

@ -22,7 +22,7 @@
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearISAM.h>
#include <gtsam/hybrid/HybridSmoother.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h>
@ -435,7 +435,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_shared<MixtureFactor>(
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{m},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
@ -532,7 +532,7 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
* the difference in noise models.
*/
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) {
HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial);
@ -592,7 +592,7 @@ TEST(HybridEstimation, ModeSelection) {
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
MixtureFactor mf(keys, modes, components);
HybridNonlinearFactor mf(keys, modes, components);
initial.insert(X(0), 0.0);
initial.insert(X(1), 0.0);
@ -683,7 +683,7 @@ TEST(HybridEstimation, ModeSelection2) {
std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
KeyVector keys = {X(0), X(1)};
MixtureFactor mf(keys, modes, components);
HybridNonlinearFactor mf(keys, modes, components);
initial.insert<Vector3>(X(0), Z_3x1);
initial.insert<Vector3>(X(1), Z_3x1);

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

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

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

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

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

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