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

update tests

release/4.3a0
Varun Agrawal 2024-09-14 13:52:56 -04:00
parent 0bbf16bf4b
commit f3b920257d
8 changed files with 61 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -542,8 +542,10 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
double logNormalizationConstant = log(1.0 / noise_tight);
double logConstant = -0.5 * d * log2pi + logNormalizationConstant;
auto func = [&](const Assignment<Key>& assignment,
const GaussianFactor::shared_ptr& gf) {
auto func =
[&](const Assignment<Key>& assignment,
const GaussianFactorValuePair& gfv) -> GaussianFactorValuePair {
auto [gf, val] = gfv;
if (assignment.at(mode) != tight_index) {
double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose);
@ -555,9 +557,9 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
}
_gfg.emplace_shared<JacobianFactor>(c);
return std::make_shared<JacobianFactor>(_gfg);
return {std::make_shared<JacobianFactor>(_gfg), 0.0};
} else {
return dynamic_pointer_cast<JacobianFactor>(gf);
return {dynamic_pointer_cast<JacobianFactor>(gf), 0.0};
}
};
auto updated_components = gmf->factors().apply(func);

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

@ -52,9 +52,9 @@ TEST(HybridFactorGraph, Keys) {
// Add a gaussian mixture factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt(
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt));
KeySet expected_continuous{X(0), X(1)};

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

@ -183,7 +183,7 @@ TEST(HybridGaussianConditional, Likelihood2) {
// Check the detailed JacobianFactor calculation for mode==1.
{
// We have a JacobianFactor
const auto gf1 = (*likelihood)(assignment1);
const auto gf1 = (*likelihood)(assignment1).first;
const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1);
CHECK(jf1);

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

@ -71,9 +71,8 @@ TEST(HybridGaussianFactor, Sum) {
auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factorsA{
{f10, 0.0}, {f11, 0.0}};
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factorsB{
std::vector<GaussianFactorValuePair> factorsA{{f10, 0.0}, {f11, 0.0}};
std::vector<GaussianFactorValuePair> factorsB{
{f20, 0.0}, {f21, 0.0}, {f22, 0.0}};
// TODO(Frank): why specify keys at all? And: keys in factor should be *all*
@ -111,8 +110,7 @@ TEST(HybridGaussianFactor, Printing) {
auto b = Matrix::Zero(2, 1);
auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factors{
{f10, 0.0}, {f11, 0.0}};
std::vector<GaussianFactorValuePair> factors{{f10, 0.0}, {f11, 0.0}};
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
@ -183,8 +181,7 @@ TEST(HybridGaussianFactor, Error) {
auto f0 = std::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factors{{f0, 0.0},
{f1, 0.0}};
std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);

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

@ -127,9 +127,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
// Add a gaussian mixture factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt(
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt));
auto result = hfg.eliminateSequential();
@ -153,9 +153,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
std::vector<GaussianFactor::shared_ptr> factors = {
std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())};
std::vector<GaussianFactorValuePair> factors = {
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}};
hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors));
// Discrete probability table for c1
@ -178,10 +178,10 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
hfg.add(HybridGaussianFactor(
{X(1)}, {{M(1), 2}},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
std::vector<GaussianFactorValuePair> factors = {
{std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}};
hfg.add(HybridGaussianFactor({X(1)}, {{M(1), 2}}, factors));
hfg.add(DecisionTreeFactor(m1, {2, 8}));
// TODO(Varun) Adding extra discrete variable not connected to continuous
@ -208,9 +208,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Decision tree with different modes on x1
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt(
M(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
// Hybrid factor P(x1|c1)
hfg.add(HybridGaussianFactor({X(1)}, {m}, dt));
@ -238,14 +238,14 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
{
hfg.add(HybridGaussianFactor(
{X(0)}, {{M(0), 2}},
{std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));
std::vector<GaussianFactorValuePair> factors = {
{std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()), 0.0}};
hfg.add(HybridGaussianFactor({X(0)}, {{M(0), 2}}, factors));
DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
M(1), std::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt1(
M(1), {std::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()), 0.0});
hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1));
}
@ -256,15 +256,15 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
{
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
M(3), std::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt(
M(3), {std::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()), 0.0});
hfg.add(HybridGaussianFactor({X(3)}, {{M(3), 2}}, dt));
DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
M(2), std::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt1(
M(2), {std::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()), 0.0});
hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1));
}
@ -552,9 +552,9 @@ TEST(HybridGaussianFactorGraph, optimize) {
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
C(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
DecisionTree<Key, GaussianFactorValuePair> dt(
C(1), {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
{std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0});
hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt));
@ -734,8 +734,8 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
// Expected decision tree with two factor graphs:
// f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0)
GaussianFactorGraphTree expected{
M(0), GaussianFactorGraph(std::vector<GF>{(*mixture)(d0), prior}),
GaussianFactorGraph(std::vector<GF>{(*mixture)(d1), prior})};
M(0), GaussianFactorGraph(std::vector<GF>{(*mixture)(d0).first, prior}),
GaussianFactorGraph(std::vector<GF>{(*mixture)(d1).first, prior})};
EXPECT(assert_equal(expected(d0), actual(d0), 1e-5));
EXPECT(assert_equal(expected(d1), actual(d1), 1e-5));

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

@ -526,6 +526,7 @@ Hybrid [x0 x1; m0]{
]
b = [ -1 ]
No noise model
value: 0
1 Leaf :
A[x0] = [
@ -536,6 +537,7 @@ Hybrid [x0 x1; m0]{
]
b = [ -0 ]
No noise model
value: 0
}
factor 2:
@ -551,6 +553,7 @@ Hybrid [x1 x2; m1]{
]
b = [ -1 ]
No noise model
value: 0
1 Leaf :
A[x1] = [
@ -561,6 +564,7 @@ Hybrid [x1 x2; m1]{
]
b = [ -0 ]
No noise model
value: 0
}
factor 3:
@ -609,6 +613,7 @@ Hybrid [x0 x1; m0]{
]
b = [ -1 ]
No noise model
value: 0
1 Leaf:
A[x0] = [
@ -619,6 +624,7 @@ Hybrid [x0 x1; m0]{
]
b = [ -0 ]
No noise model
value: 0
}
factor 2:
@ -633,6 +639,7 @@ Hybrid [x1 x2; m1]{
]
b = [ -1 ]
No noise model
value: 0
1 Leaf:
A[x1] = [
@ -643,6 +650,7 @@ Hybrid [x1 x2; m1]{
]
b = [ -0 ]
No noise model
value: 0
}
factor 3:

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

@ -83,7 +83,7 @@ TEST(HybridSerialization, HybridGaussianFactor) {
auto b1 = Matrix::Ones(2, 1);
auto f0 = std::make_shared<JacobianFactor>(X(0), A, b0);
auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors);