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

test for differing covariances

release/4.3a0
Varun Agrawal 2023-12-12 19:15:24 -05:00
parent af490e9ffc
commit c004bd8df0
1 changed files with 70 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
gtsam/hybrid/tests/testMixtureFactor.cpp
View File

@ -18,6 +18,9 @@
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
@ -114,6 +117,73 @@ TEST(MixtureFactor, Dim) {
EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
}
/* ************************************************************************* */
// Test components with differing covariances
TEST(MixtureFactor, 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*/}};
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 bn2;
bn2.emplace_back(gm);
gtsam::VectorValues measurements;
measurements.insert(Z(1), gtsam::Z_1x1);
// Create FG with single GaussianMixtureFactor
auto mixture_fg = bn2.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();
// hbn->print("\n\nfinal bayes net");
HybridValues actual_values = hbn->optimize();
VectorValues cv;
cv.insert(X(1), Vector1(0.0));
cv.insert(X(2), Vector1(0.0));
DiscreteValues dv;
dv.insert({M(1), 1});
HybridValues expected_values(cv, dv);
EXPECT(assert_equal(expected_values, actual_values));
}
/* ************************************************************************* */
int main() {
TestResult tr;