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)
for (size_t t = 1; t < n; t++) {
std::vector<GaussianFactorValuePair> components = {
{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
I_3x3, Z_3x1),
0.0},
{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
I_3x3, Vector3::Ones()),
0.0}};
hfg.add(HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)},
{{dKeyFunc(t), 2}}, components));
DiscreteKeys dKeys{{dKeyFunc(t), 2}};
HybridGaussianFactor::FactorValuePairs components(
dKeys, {{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3,
keyFunc(t + 1), I_3x3, Z_3x1),
0.0},
{std::make_shared<JacobianFactor>(
keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()),
0.0}});
hfg.add(
HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components));
if (t > 1) {
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;
auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0));
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
auto zero_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactorValuePair> factors = {{zero_motion, 0.0},
{one_motion, 0.0}};
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)};
HybridNonlinearFactor::Factors factors(
discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}});
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, discreteKeys,
factors);
DiscreteKey mode(M(0), 2);
nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");