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

Simplify all call sites

release/4.3a0
Frank Dellaert 2024-09-26 11:29:03 -07:00
parent 977112d004
commit b45ba003ca
10 changed files with 27 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -414,7 +414,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
HybridGaussianConditional::Conditionals conditionals( HybridGaussianConditional::Conditionals conditionals(
eliminationResults, [](const Result &pair) { return pair.first; }); eliminationResults, [](const Result &pair) { return pair.first; });
auto hybridGaussian = std::make_shared<HybridGaussianConditional>( auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
frontalKeys, continuousSeparator, discreteSeparator, conditionals); discreteSeparator, conditionals);
return {std::make_shared<HybridConditional>(hybridGaussian), newFactor}; return {std::make_shared<HybridConditional>(hybridGaussian), newFactor};
} }

3
gtsam/hybrid/tests/TinyHybridExample.h
View File

@ -46,8 +46,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
std::vector<GaussianConditional::shared_ptr> conditionals{ std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5), GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)}; GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
bayesNet.emplace_shared<HybridGaussianConditional>( bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals);
} }
// Create prior on X(0). // Create prior on X(0).

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

@ -43,9 +43,6 @@ const DiscreteValues m1Assignment{{M(0), 1}};
DiscreteConditional::shared_ptr mixing = DiscreteConditional::shared_ptr mixing =
std::make_shared<DiscreteConditional>(m, "60/40"); std::make_shared<DiscreteConditional>(m, "60/40");
// define Continuous keys
const KeyVector continuousKeys{Z(0)};
/** /**
* Create a simple Gaussian Mixture Model represented as p(z|m)P(m) * Create a simple Gaussian Mixture Model represented as p(z|m)P(m)
* where m is a discrete variable and z is a continuous variable. * where m is a discrete variable and z is a continuous variable.
@ -61,8 +58,7 @@ HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0,
model0), model0),
c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1, c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1,
model1); model1);
hbn.emplace_shared<HybridGaussianConditional>(continuousKeys, KeyVector{}, m, hbn.emplace_shared<HybridGaussianConditional>(m, std::vector{c0, c1});
std::vector{c0, c1});
hbn.push_back(mixing); hbn.push_back(mixing);
return hbn; return hbn;
} }

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

@ -108,8 +108,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
HybridBayesNet bayesNet; HybridBayesNet bayesNet;
bayesNet.push_back(continuousConditional); bayesNet.push_back(continuousConditional);
bayesNet.emplace_shared<HybridGaussianConditional>( bayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(1)}, KeyVector{}, Asia, Asia, std::vector{conditional0, conditional1});
std::vector{conditional0, conditional1});
bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1"); bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1");
// Create values at which to evaluate. // Create values at which to evaluate.
@ -169,8 +168,7 @@ TEST(HybridBayesNet, Error) {
X(1), Vector1::Constant(2), I_1x1, model1); X(1), Vector1::Constant(2), I_1x1, model1);
auto gm = std::make_shared<HybridGaussianConditional>( auto gm = std::make_shared<HybridGaussianConditional>(
KeyVector{X(1)}, KeyVector{}, Asia, Asia, std::vector{conditional0, conditional1});
std::vector{conditional0, conditional1});
// Create hybrid Bayes net. // Create hybrid Bayes net.
HybridBayesNet bayesNet; HybridBayesNet bayesNet;
bayesNet.push_back(continuousConditional); bayesNet.push_back(continuousConditional);

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

@ -39,6 +39,8 @@ TEST(HybridConditional, Invariants) {
const DiscreteValues d{{M(0), 1}}; const DiscreteValues d{{M(0), 1}};
const HybridValues values{c, d}; const HybridValues values{c, d};
GTSAM_PRINT(bn);
// Check invariants for p(z|x,m) // Check invariants for p(z|x,m)
auto hc0 = bn.at(0); auto hc0 = bn.at(0);
CHECK(hc0->isHybrid()); CHECK(hc0->isHybrid());

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

@ -623,10 +623,7 @@ TEST(HybridEstimation, ModeSelection) {
Z_1x1, noise_loose), Z_1x1, noise_loose),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
Z_1x1, noise_tight)}; Z_1x1, noise_tight)};
bn.emplace_shared<HybridGaussianConditional>( bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
conditionals));
VectorValues vv; VectorValues vv;
vv.insert(Z(0), Z_1x1); vv.insert(Z(0), Z_1x1);
@ -658,10 +655,7 @@ TEST(HybridEstimation, ModeSelection2) {
Z_3x1, noise_loose), Z_3x1, noise_loose),
GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1), GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
Z_3x1, noise_tight)}; Z_3x1, noise_tight)};
bn.emplace_shared<HybridGaussianConditional>( bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
conditionals));
VectorValues vv; VectorValues vv;
vv.insert(Z(0), Z_3x1); vv.insert(Z(0), Z_3x1);

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

@ -52,8 +52,7 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
commonSigma), commonSigma),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
commonSigma)}; commonSigma)};
const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode, const HybridGaussianConditional hybrid_conditional(mode, conditionals);
conditionals);
} // namespace equal_constants } // namespace equal_constants
/* ************************************************************************* */ /* ************************************************************************* */
@ -158,8 +157,7 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
0.5), 0.5),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
3.0)}; 3.0)};
const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode, const HybridGaussianConditional hybrid_conditional(mode, conditionals);
conditionals);
} // namespace mode_dependent_constants } // namespace mode_dependent_constants
/* ************************************************************************* */ /* ************************************************************************* */

7
gtsam/hybrid/tests/testHybridGaussianFactor.cpp
View File

@ -169,7 +169,7 @@ TEST(HybridGaussianFactor, HybridGaussianConditional) {
auto gaussians = std::make_shared<GaussianConditional>(); auto gaussians = std::make_shared<GaussianConditional>();
HybridGaussianConditional::Conditionals conditionals(gaussians); HybridGaussianConditional::Conditionals conditionals(gaussians);
HybridGaussianConditional gm({}, keys, dKeys, conditionals); HybridGaussianConditional gm(dKeys, conditionals);
EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size()); EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
} }
@ -234,9 +234,8 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
-I_1x1, model1); -I_1x1, model1);
DiscreteKeys discreteParents{m1}; DiscreteKeys discreteParents{m1};
return std::make_shared<HybridGaussianConditional>( return std::make_shared<HybridGaussianConditional>(
KeyVector{X(1)}, KeyVector{X(0)}, discreteParents, discreteParents, HybridGaussianConditional::Conditionals(
HybridGaussianConditional::Conditionals(discreteParents, discreteParents, std::vector{c0, c1}));
std::vector{c0, c1}));
} }
/// Create two state Bayes network with 1 or two measurement models /// Create two state Bayes network with 1 or two measurement models

46
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -72,13 +72,10 @@ TEST(HybridGaussianFactorGraph, Creation) {
// Define a hybrid gaussian conditional P(x0|x1, c0) // Define a hybrid gaussian conditional P(x0|x1, c0)
// and add it to the factor graph. // and add it to the factor graph.
HybridGaussianConditional gm( HybridGaussianConditional gm(
{X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), {M(0), 2},
HybridGaussianConditional::Conditionals( {std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
M(0), std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3)});
I_3x3),
std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
X(1), I_3x3)));
hfg.add(gm); hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size()); EXPECT_LONGS_EQUAL(2, hfg.size());
@ -654,11 +651,7 @@ 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);
DiscreteKeys discreteParents{m1}; hbn.emplace_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});
hbn.emplace_shared<HybridGaussianConditional>(
KeyVector{f01}, KeyVector{x0, x1}, discreteParents,
HybridGaussianConditional::Conditionals(discreteParents,
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");
@ -830,11 +823,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
X(0), Vector1(14.1421), I_1x1 * 2.82843), X(0), Vector1(14.1421), I_1x1 * 2.82843),
conditional1 = std::make_shared<GaussianConditional>( conditional1 = std::make_shared<GaussianConditional>(
X(0), Vector1(10.1379), I_1x1 * 2.02759); X(0), Vector1(10.1379), I_1x1 * 2.02759);
DiscreteKeys discreteParents{mode};
expectedBayesNet.emplace_shared<HybridGaussianConditional>( expectedBayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(0)}, KeyVector{}, discreteParents, mode, std::vector{conditional0, conditional1});
HybridGaussianConditional::Conditionals(
discreteParents, std::vector{conditional0, conditional1}));
// Add prior on mode. // Add prior on mode.
expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26"); expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
@ -860,10 +850,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
std::vector<GaussianConditional::shared_ptr> conditionals{ std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)}; GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)};
auto gm = std::make_shared<HybridGaussianConditional>( auto gm = std::make_shared<HybridGaussianConditional>(mode, conditionals);
KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode},
HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
conditionals));
bn.push_back(gm); bn.push_back(gm);
// Create prior on X(0). // Create prior on X(0).
@ -891,9 +878,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
conditional1 = std::make_shared<GaussianConditional>( conditional1 = std::make_shared<GaussianConditional>(
X(0), Vector1(14.1421), I_1x1 * 2.82843); X(0), Vector1(14.1421), I_1x1 * 2.82843);
expectedBayesNet.emplace_shared<HybridGaussianConditional>( expectedBayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, mode, std::vector{conditional0, conditional1});
HybridGaussianConditional::Conditionals(
DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
// Add prior on mode. // Add prior on mode.
expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "1/1"); expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "1/1");
@ -929,9 +914,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
conditional1 = std::make_shared<GaussianConditional>( conditional1 = std::make_shared<GaussianConditional>(
X(0), Vector1(10.274), I_1x1 * 2.0548); X(0), Vector1(10.274), I_1x1 * 2.0548);
expectedBayesNet.emplace_shared<HybridGaussianConditional>( expectedBayesNet.emplace_shared<HybridGaussianConditional>(
KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, mode, std::vector{conditional0, conditional1});
HybridGaussianConditional::Conditionals(
DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
// Add prior on mode. // Add prior on mode.
expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77"); expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
@ -980,10 +963,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5), GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5),
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1,
3.0)}; 3.0)};
bn.emplace_shared<HybridGaussianConditional>( bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, conditionals);
KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t},
HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t},
conditionals));
// Create prior on discrete mode N(t): // Create prior on discrete mode N(t):
bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80"); bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
@ -999,10 +979,8 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
0.2), 0.2),
GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1, GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1,
0.2)}; 0.2)};
auto gm = std::make_shared<HybridGaussianConditional>( auto gm = std::make_shared<HybridGaussianConditional>(motion_model_t,
KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t}, conditionals);
HybridGaussianConditional::Conditionals(DiscreteKeys{motion_model_t},
conditionals));
bn.push_back(gm); bn.push_back(gm);
// Create prior on motion model M(t): // Create prior on motion model M(t):

4
gtsam/hybrid/tests/testSerializationHybrid.cpp
View File

@ -115,9 +115,7 @@ TEST(HybridSerialization, HybridGaussianConditional) {
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5));
const auto conditional1 = std::make_shared<GaussianConditional>( const auto conditional1 = std::make_shared<GaussianConditional>(
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, const HybridGaussianConditional gm(mode, {conditional0, conditional1});
HybridGaussianConditional::Conditionals(
{mode}, {conditional0, conditional1}));
EXPECT(equalsObj<HybridGaussianConditional>(gm)); EXPECT(equalsObj<HybridGaussianConditional>(gm));
EXPECT(equalsXML<HybridGaussianConditional>(gm)); EXPECT(equalsXML<HybridGaussianConditional>(gm));