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

tree based constructors

release/4.3a0
Varun Agrawal 2024-09-16 17:30:08 -04:00
parent 1c74da26f4
commit 64d9fc67bd
2 changed files with 17 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -57,15 +57,16 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
// keyFunc(1) to keyFunc(n+1) // keyFunc(1) to keyFunc(n+1)
for (size_t t = 1; t < n; t++) { for (size_t t = 1; t < n; t++) {
std::vector<GaussianFactorValuePair> components = { DiscreteKeys dKeys{{dKeyFunc(t), 2}};
{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1), HybridGaussianFactor::FactorValuePairs components(
I_3x3, Z_3x1), dKeys, {{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3,
0.0}, keyFunc(t + 1), I_3x3, Z_3x1),
{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1), 0.0},
I_3x3, Vector3::Ones()), {std::make_shared<JacobianFactor>(
0.0}}; keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()),
hfg.add(HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, 0.0}});
{{dKeyFunc(t), 2}}, components)); hfg.add(
HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components));
if (t > 1) { if (t > 1) {
hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},

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

@ -383,15 +383,17 @@ TEST(HybridBayesNet, Sampling) {
HybridNonlinearFactorGraph nfg; HybridNonlinearFactorGraph nfg;
auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0));
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
auto zero_motion = auto zero_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
auto one_motion = 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);
std::vector<NonlinearFactorValuePair> factors = {{zero_motion, 0.0}, DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)};
{one_motion, 0.0}}; HybridNonlinearFactor::Factors factors(
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model); discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}});
nfg.emplace_shared<HybridNonlinearFactor>( nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, discreteKeys,
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); factors);
DiscreteKey mode(M(0), 2); DiscreteKey mode(M(0), 2);
nfg.emplace_shared<DiscreteDistribution>(mode, "1/1"); nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");