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

tests for verification

release/4.3a0
Varun Agrawal 2024-08-20 16:30:32 -04:00
parent ea104c4b83
commit 5e3093e3e2
2 changed files with 260 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

247
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
View File

@ -22,9 +22,13 @@
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// Include for test suite
#include <CppUnitLite/TestHarness.h>
@ -56,7 +60,6 @@ TEST(GaussianMixtureFactor, Sum) {
auto b = Matrix::Zero(2, 1);
Vector2 sigmas;
sigmas << 1, 2;
auto model = noiseModel::Diagonal::Sigmas(sigmas, true);
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);
@ -106,7 +109,8 @@ TEST(GaussianMixtureFactor, Printing) {
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
std::string expected =
R"(Hybrid [x1 x2; 1]{
R"(GaussianMixtureFactor
Hybrid [x1 x2; 1]{
Choice(1)
0 Leaf :
A[x1] = [
@ -178,7 +182,8 @@ TEST(GaussianMixtureFactor, Error) {
continuousValues.insert(X(2), Vector2(1, 1));
// error should return a tree of errors, with nodes for each discrete value.
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.errorTree(continuousValues);
AlgebraicDecisionTree<Key> error_tree =
mixtureFactor.errorTree(continuousValues);
std::vector<DiscreteKey> discrete_keys = {m1};
// Error values for regression test
@ -191,8 +196,240 @@ TEST(GaussianMixtureFactor, Error) {
DiscreteValues discreteValues;
discreteValues[m1.first] = 1;
EXPECT_DOUBLES_EQUAL(
4.0, mixtureFactor.error({continuousValues, discreteValues}),
1e-9);
4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9);
}
/* ************************************************************************* */
// Test components with differing means
TEST(GaussianMixtureFactor, DifferentMeans) {
DiscreteKey m1(M(1), 2), m2(M(2), 2);
Values values;
double x1 = 0.0, x2 = 1.75, x3 = 2.60;
values.insert(X(1), x1);
values.insert(X(2), x2);
values.insert(X(3), x3);
auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0)
->linearize(values);
auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1)
->linearize(values);
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors, true);
HybridGaussianFactorGraph hfg;
hfg.push_back(mixtureFactor);
f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0)
->linearize(values);
f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1)
->linearize(values);
std::vector<GaussianFactor::shared_ptr> factors23{f0, f1};
hfg.push_back(GaussianMixtureFactor({X(2), X(3)}, {m2}, factors23, true));
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
hfg.push_back(prior);
hfg.push_back(PriorFactor<double>(X(2), 2.0, prior_noise).linearize(values));
auto bn = hfg.eliminateSequential();
HybridValues actual = bn->optimize();
HybridValues expected(
VectorValues{
{X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
DiscreteValues{{M(1), 1}, {M(2), 0}});
EXPECT(assert_equal(expected, actual));
{
DiscreteValues dv{{M(1), 0}, {M(2), 0}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
}
{
DiscreteValues dv{{M(1), 0}, {M(2), 1}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
}
{
DiscreteValues dv{{M(1), 1}, {M(2), 0}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
}
{
DiscreteValues dv{{M(1), 1}, {M(2), 1}};
VectorValues cont = bn->optimize(dv);
double error = bn->error(HybridValues(cont, dv));
// regression
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
}
}
/* ************************************************************************* */
/**
* @brief Test components with differing covariances.
* The factor graph is
* *-X1-*-X2
* |
* M1
*/
TEST(GaussianMixtureFactor, DifferentCovariances) {
DiscreteKey m1(M(1), 2);
Values values;
double x1 = 1.0, x2 = 1.0;
values.insert(X(1), x1);
values.insert(X(2), x2);
double between = 0.0;
auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
auto f0 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
// Create via toFactorGraph
using symbol_shorthand::Z;
Matrix H0_1, H0_2, H1_1, H1_2;
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(1), H0_1 /*Sp1*/},
{X(2), H0_2 /*Tp2*/}};
Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(1), H1_1 /*Sp1*/},
{X(2), H1_2 /*Tp2*/}};
gtsam::GaussianMixtureFactor gmf(
{X(1), X(2)}, {m1},
{std::make_shared<JacobianFactor>(X(1), H0_1, X(2), H0_2, -d0, model0),
std::make_shared<JacobianFactor>(X(1), H1_1, X(2), H1_2, -d1, model1)},
true);
// Create FG with single GaussianMixtureFactor
HybridGaussianFactorGraph mixture_fg;
mixture_fg.add(gmf);
// Linearized prior factor on X1
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
mixture_fg.push_back(prior);
auto hbn = mixture_fg.eliminateSequential();
// hbn->print();
VectorValues cv;
cv.insert(X(1), Vector1(0.0));
cv.insert(X(2), Vector1(0.0));
// Check that the error values at the MLE point μ.
AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
DiscreteValues dv0{{M(1), 0}};
DiscreteValues dv1{{M(1), 1}};
// regression
EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9);
EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9);
DiscreteConditional expected_m1(m1, "0.5/0.5");
DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
EXPECT(assert_equal(expected_m1, actual_m1));
}
/* ************************************************************************* */
/**
* @brief Test components with differing covariances
* but with a Bayes net P(Z|X, M) converted to a FG.
*/
TEST(GaussianMixtureFactor, DifferentCovariances2) {
DiscreteKey m1(M(1), 2);
Values values;
double x1 = 1.0, x2 = 1.0;
values.insert(X(1), x1);
values.insert(X(2), x2);
double between = 0.0;
auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
auto f0 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
// Create via toFactorGraph
using symbol_shorthand::Z;
Matrix H0_1, H0_2, H1_1, H1_2;
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(1), H0_1 /*Sp1*/},
{X(2), H0_2 /*Tp2*/}};
Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(1), H1_1 /*Sp1*/},
{X(2), H1_2 /*Tp2*/}};
auto gm = new gtsam::GaussianMixture(
{Z(1)}, {X(1), X(2)}, {m1},
{std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
gtsam::HybridBayesNet bn;
bn.emplace_back(gm);
gtsam::VectorValues measurements;
measurements.insert(Z(1), gtsam::Z_1x1);
// Create FG with single GaussianMixtureFactor
HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
// Linearized prior factor on X1
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
mixture_fg.push_back(prior);
auto hbn = mixture_fg.eliminateSequential();
VectorValues cv;
cv.insert(X(1), Vector1(0.0));
cv.insert(X(2), Vector1(0.0));
// Check that the error values at the MLE point μ.
AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
DiscreteValues dv0{{M(1), 0}};
DiscreteValues dv1{{M(1), 1}};
// regression
EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9);
EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9);
DiscreteConditional expected_m1(m1, "0.5/0.5");
DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
EXPECT(assert_equal(expected_m1, actual_m1));
}
/* ************************************************************************* */

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

@ -510,6 +510,7 @@ factor 0:
b = [ -10 ]
No noise model
factor 1:
GaussianMixtureFactor
Hybrid [x0 x1; m0]{
Choice(m0)
0 Leaf :
@ -534,6 +535,7 @@ Hybrid [x0 x1; m0]{
}
factor 2:
GaussianMixtureFactor
Hybrid [x1 x2; m1]{
Choice(m1)
0 Leaf :
@ -675,33 +677,41 @@ factor 6: P( m1 | m0 ):
size: 3
conditional 0: Hybrid P( x0 | x1 m0)
Discrete Keys = (m0, 2),
logNormalizationConstant: 1.38862
Choice(m0)
0 Leaf p(x0 | x1)
R = [ 10.0499 ]
S[x1] = [ -0.0995037 ]
d = [ -9.85087 ]
logNormalizationConstant: 1.38862
No noise model
1 Leaf p(x0 | x1)
R = [ 10.0499 ]
S[x1] = [ -0.0995037 ]
d = [ -9.95037 ]
logNormalizationConstant: 1.38862
No noise model
conditional 1: Hybrid P( x1 | x2 m0 m1)
Discrete Keys = (m0, 2), (m1, 2),
logNormalizationConstant: 1.3935
Choice(m1)
0 Choice(m0)
0 0 Leaf p(x1 | x2)
R = [ 10.099 ]
S[x2] = [ -0.0990196 ]
d = [ -9.99901 ]
logNormalizationConstant: 1.3935
No noise model
0 1 Leaf p(x1 | x2)
R = [ 10.099 ]
S[x2] = [ -0.0990196 ]
d = [ -9.90098 ]
logNormalizationConstant: 1.3935
No noise model
1 Choice(m0)
@ -709,16 +719,20 @@ conditional 1: Hybrid P( x1 | x2 m0 m1)
R = [ 10.099 ]
S[x2] = [ -0.0990196 ]
d = [ -10.098 ]
logNormalizationConstant: 1.3935
No noise model
1 1 Leaf p(x1 | x2)
R = [ 10.099 ]
S[x2] = [ -0.0990196 ]
d = [ -10 ]
logNormalizationConstant: 1.3935
No noise model
conditional 2: Hybrid P( x2 | m0 m1)
Discrete Keys = (m0, 2), (m1, 2),
logNormalizationConstant: 1.38857
Choice(m1)
0 Choice(m0)
0 0 Leaf p(x2)
@ -726,6 +740,7 @@ conditional 2: Hybrid P( x2 | m0 m1)
d = [ -10.1489 ]
mean: 1 elements
x2: -1.0099
logNormalizationConstant: 1.38857
No noise model
0 1 Leaf p(x2)
@ -733,6 +748,7 @@ conditional 2: Hybrid P( x2 | m0 m1)
d = [ -10.1479 ]
mean: 1 elements
x2: -1.0098
logNormalizationConstant: 1.38857
No noise model
1 Choice(m0)
@ -741,6 +757,7 @@ conditional 2: Hybrid P( x2 | m0 m1)
d = [ -10.0504 ]
mean: 1 elements
x2: -1.0001
logNormalizationConstant: 1.38857
No noise model
1 1 Leaf p(x2)
@ -748,6 +765,7 @@ conditional 2: Hybrid P( x2 | m0 m1)
d = [ -10.0494 ]
mean: 1 elements
x2: -1
logNormalizationConstant: 1.38857
No noise model
)";