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

remove duplicate test and focus only on direct specification

release/4.3a0
Varun Agrawal 2024-09-19 10:27:43 -04:00
parent 987ecd4a07
commit 80d9a5a65f
1 changed files with 7 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -741,152 +741,6 @@ TEST(HybridGaussianFactor, TwoStateModel4) {
EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
} }
/**
* @brief Helper function to specify a Hybrid Bayes Net
* P(X1)P(Z1 | X1, X2, M1) and convert it to a Hybrid Factor Graph
* ϕ(X1)ϕ(X1, X2, M1; Z1) by converting to likelihoods given Z1.
*
* We can specify either different means or different sigmas,
* or both for each hybrid factor component.
*
* @param values Initial values for linearization.
* @param means The mean values for the conditional components.
* @param sigmas Noise model sigma values (standard deviation).
* @param m1 The discrete mode key.
* @param z1 The measurement value.
* @return HybridGaussianFactorGraph
*/
static HybridGaussianFactorGraph GetFactorGraphFromBayesNet(
const gtsam::Values &values, const std::vector<double> &means,
const std::vector<double> &sigmas, DiscreteKey &m1, double z1 = 0.0) {
// Noise models
auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]);
auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
// HybridGaussianFactor component factors
auto f0 =
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0);
auto f1 =
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1);
/// Get terms for each p^m(z1 | x1, x2)
Matrix H0_1, H0_2, H1_1, H1_2;
double x1 = values.at<double>(X(0)), x2 = values.at<double>(X(1));
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
//
{X(0), H0_1 /*Sp1*/},
{X(1), 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(0), H1_1 /*Sp1*/},
{X(1), H1_2 /*Tp2*/}};
// Create conditional P(Z1 | X1, X2, M1)
auto conditionals = std::vector{
std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)};
gtsam::HybridBayesNet bn;
bn.emplace_shared<HybridGaussianConditional>(
KeyVector{Z(1)}, KeyVector{X(0), X(1)}, DiscreteKeys{m1}, conditionals);
// Create FG via toFactorGraph
gtsam::VectorValues measurements;
measurements.insert(Z(1), gtsam::I_1x1 * z1); // Set Z1
HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
// Linearized prior factor on X1
auto prior = PriorFactor<double>(X(0), x1, prior_noise).linearize(values);
mixture_fg.push_back(prior);
return mixture_fg;
}
/* ************************************************************************* */
/**
* @brief Test Hybrid Factor Graph.
*
* We specify a hybrid Bayes network P(Z | X, M) = P(X1)P(Z1 | X1, X2, M1),
* which is then converted to a factor graph by specifying Z1.
* This is different from the TwoStateModel version since
* we use a factor with 2 continuous variables ϕ(x1, x2, m1)
* directly instead of a conditional.
* This serves as a good sanity check.
*
* P(Z1 | X1, X2, M1) has 2 conditionals each for the binary
* mode m1.
*/
TEST(HybridGaussianFactor, FactorGraphFromBayesNet) {
DiscreteKey m1(M(1), 2);
Values values;
double x1 = 0.0, x2 = 1.75;
values.insert(X(0), x1);
values.insert(X(1), x2);
// Different means, same sigma
std::vector<double> means{0.0, 2.0}, sigmas{1e-0, 1e-0};
HybridGaussianFactorGraph hfg =
GetFactorGraphFromBayesNet(values, means, sigmas, m1, 0.0);
{
// With no measurement on X2, each mode should be equally likely
auto bn = hfg.eliminateSequential();
HybridValues actual = bn->optimize();
HybridValues expected(
VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}},
DiscreteValues{{M(1), 0}});
EXPECT(assert_equal(expected, actual));
DiscreteValues dv0{{M(1), 0}};
VectorValues cont0 = bn->optimize(dv0);
double error0 = bn->error(HybridValues(cont0, dv0));
// regression
EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9);
DiscreteValues dv1{{M(1), 1}};
VectorValues cont1 = bn->optimize(dv1);
double error1 = bn->error(HybridValues(cont1, dv1));
EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9);
}
{
// If we add a measurement on X2, we have more information to work with.
// Add a measurement on X2
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
GaussianConditional meas_z2(Z(2), Vector1(2.0), I_1x1, X(1), I_1x1,
prior_noise);
auto prior_x2 = meas_z2.likelihood(Vector1(x2));
hfg.push_back(prior_x2);
auto bn = hfg.eliminateSequential();
HybridValues actual = bn->optimize();
// regression
HybridValues expected(
VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}},
DiscreteValues{{M(1), 1}});
EXPECT(assert_equal(expected, actual));
DiscreteValues dv0{{M(1), 0}};
VectorValues cont0 = bn->optimize(dv0);
// regression
EXPECT_DOUBLES_EQUAL(2.12692448787, bn->error(HybridValues(cont0, dv0)),
1e-9);
DiscreteValues dv1{{M(1), 1}};
VectorValues cont1 = bn->optimize(dv1);
// regression
EXPECT_DOUBLES_EQUAL(0.126928487854, bn->error(HybridValues(cont1, dv1)),
1e-9);
}
}
namespace test_direct_factor_graph { namespace test_direct_factor_graph {
/** /**
* @brief Create a Factor Graph by directly specifying all * @brief Create a Factor Graph by directly specifying all
@ -902,10 +756,11 @@ namespace test_direct_factor_graph {
*/ */
static HybridGaussianFactorGraph CreateFactorGraph( static HybridGaussianFactorGraph CreateFactorGraph(
const gtsam::Values &values, const std::vector<double> &means, const gtsam::Values &values, const std::vector<double> &means,
const std::vector<double> &sigmas, DiscreteKey &m1) { const std::vector<double> &sigmas, DiscreteKey &m1,
double measurement_noise = 1e-3) {
auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]);
auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]);
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
auto f0 = auto f0 =
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0) std::make_shared<BetweenFactor<double>>(X(0), X(1), means[0], model0)
@ -917,10 +772,10 @@ static HybridGaussianFactorGraph CreateFactorGraph(
// Create HybridGaussianFactor // Create HybridGaussianFactor
std::vector<GaussianFactorValuePair> factors{ std::vector<GaussianFactorValuePair> factors{
{f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}};
HybridGaussianFactor mixtureFactor({X(0), X(1)}, {m1}, factors); HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;
hfg.push_back(mixtureFactor); hfg.push_back(motionFactor);
hfg.push_back(PriorFactor<double>(X(0), values.at<double>(X(0)), prior_noise) hfg.push_back(PriorFactor<double>(X(0), values.at<double>(X(0)), prior_noise)
.linearize(values)); .linearize(values));
@ -1025,10 +880,8 @@ TEST(HybridGaussianFactor, DifferentCovariancesFG) {
std::vector<double> means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; std::vector<double> means = {0.0, 0.0}, sigmas = {1e2, 1e-2};
// Create FG with HybridGaussianFactor and prior on X1 // Create FG with HybridGaussianFactor and prior on X1
HybridGaussianFactorGraph mixture_fg = HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1);
CreateFactorGraph(values, means, sigmas, m1); auto hbn = fg.eliminateSequential();
auto hbn = mixture_fg.eliminateSequential();
VectorValues cv; VectorValues cv;
cv.insert(X(0), Vector1(0.0)); cv.insert(X(0), Vector1(0.0));