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

Merge pull request #1892 from borglab/cleanup-hybrid-tests

release/4.3a0
Varun Agrawal 2024-10-30 17:41:18 -04:00 committed by GitHub
parent ecc736667b 31f0011d5f
commit f4f54dd12d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
10 changed files with 224 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -120,21 +120,13 @@ using MotionModel = BetweenFactor<double>;
// Test fixture with switching network.
/// ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
struct Switching {
private:
HybridNonlinearFactorGraph nonlinearFactorGraph_;
public:
size_t K;
DiscreteKeys modes;
HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain;
HybridGaussianFactorGraph linearizedFactorGraph;
HybridGaussianFactorGraph linearUnaryFactors, linearBinaryFactors;
Values linearizationPoint;
// Access the flat nonlinear factor graph.
const HybridNonlinearFactorGraph &nonlinearFactorGraph() const {
return nonlinearFactorGraph_;
}
/**
* @brief Create with given number of time steps.
*
@ -164,36 +156,33 @@ struct Switching {
// Create hybrid factor graph.
// Add a prior ϕ(X(0)) on X(0).
nonlinearFactorGraph_.emplace_shared<PriorFactor<double>>(
unaryFactors.emplace_shared<PriorFactor<double>>(
X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
unaryFactors.push_back(nonlinearFactorGraph_.back());
// Add "motion models" ϕ(X(k),X(k+1),M(k)).
for (size_t k = 0; k < K - 1; k++) {
auto motion_models = motionModels(k, between_sigma);
nonlinearFactorGraph_.emplace_shared<HybridNonlinearFactor>(modes[k],
motion_models);
binaryFactors.push_back(nonlinearFactorGraph_.back());
binaryFactors.emplace_shared<HybridNonlinearFactor>(modes[k],
motion_models);
}
// Add measurement factors ϕ(X(k);z_k).
auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
for (size_t k = 1; k < K; k++) {
nonlinearFactorGraph_.emplace_shared<PriorFactor<double>>(
X(k), measurements.at(k), measurement_noise);
unaryFactors.push_back(nonlinearFactorGraph_.back());
unaryFactors.emplace_shared<PriorFactor<double>>(X(k), measurements.at(k),
measurement_noise);
}
// Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2))
modeChain = createModeChain(transitionProbabilityTable);
nonlinearFactorGraph_ += modeChain;
// Create the linearization point.
for (size_t k = 0; k < K; k++) {
linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
}
linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint);
linearUnaryFactors = *unaryFactors.linearize(linearizationPoint);
linearBinaryFactors = *binaryFactors.linearize(linearizationPoint);
}
// Create motion models for a given time step
@ -224,6 +213,24 @@ struct Switching {
}
return chain;
}
/// Get the full linear factor graph.
HybridGaussianFactorGraph linearizedFactorGraph() const {
HybridGaussianFactorGraph graph;
graph.push_back(linearUnaryFactors);
graph.push_back(linearBinaryFactors);
graph.push_back(modeChain);
return graph;
}
/// Get all the nonlinear factors.
HybridNonlinearFactorGraph nonlinearFactorGraph() const {
HybridNonlinearFactorGraph graph;
graph.push_back(unaryFactors);
graph.push_back(binaryFactors);
graph.push_back(modeChain);
return graph;
}
};
} // namespace gtsam

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

@ -31,6 +31,7 @@
// Include for test suite
#include <CppUnitLite/TestHarness.h>
#include <memory>
using namespace std;
@ -263,7 +264,7 @@ TEST(HybridBayesNet, Choose) {
const Ordering ordering(s.linearizationPoint.keys());
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
s.linearizedFactorGraph().eliminatePartialSequential(ordering);
DiscreteValues assignment;
assignment[M(0)] = 1;
@ -292,7 +293,7 @@ TEST(HybridBayesNet, OptimizeAssignment) {
const Ordering ordering(s.linearizationPoint.keys());
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
s.linearizedFactorGraph().eliminatePartialSequential(ordering);
DiscreteValues assignment;
assignment[M(0)] = 1;
@ -319,7 +320,7 @@ TEST(HybridBayesNet, Optimize) {
Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1");
HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential();
s.linearizedFactorGraph().eliminateSequential();
HybridValues delta = hybridBayesNet->optimize();
@ -347,7 +348,7 @@ TEST(HybridBayesNet, Pruning) {
Switching s(3);
HybridBayesNet::shared_ptr posterior =
s.linearizedFactorGraph.eliminateSequential();
s.linearizedFactorGraph().eliminateSequential();
EXPECT_LONGS_EQUAL(5, posterior->size());
// Optimize
@ -400,7 +401,7 @@ TEST(HybridBayesNet, Prune) {
Switching s(4);
HybridBayesNet::shared_ptr posterior =
s.linearizedFactorGraph.eliminateSequential();
s.linearizedFactorGraph().eliminateSequential();
EXPECT_LONGS_EQUAL(7, posterior->size());
HybridValues delta = posterior->optimize();
@ -418,7 +419,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
Switching s(4);
HybridBayesNet::shared_ptr posterior =
s.linearizedFactorGraph.eliminateSequential();
s.linearizedFactorGraph().eliminateSequential();
EXPECT_LONGS_EQUAL(7, posterior->size());
DiscreteConditional joint;

112
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -352,7 +352,7 @@ TEST(HybridBayesTree, OptimizeMultifrontal) {
Switching s(4);
HybridBayesTree::shared_ptr hybridBayesTree =
s.linearizedFactorGraph.eliminateMultifrontal();
s.linearizedFactorGraph().eliminateMultifrontal();
HybridValues delta = hybridBayesTree->optimize();
VectorValues expectedValues;
@ -364,30 +364,40 @@ TEST(HybridBayesTree, OptimizeMultifrontal) {
EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5));
}
namespace optimize_fixture {
HybridGaussianFactorGraph GetGaussianFactorGraph(size_t N) {
Switching s(N);
HybridGaussianFactorGraph graph;
for (size_t i = 0; i < N - 1; i++) {
graph.push_back(s.linearBinaryFactors.at(i));
}
for (size_t i = 0; i < N; i++) {
graph.push_back(s.linearUnaryFactors.at(i));
}
for (size_t i = 0; i < N - 1; i++) {
graph.push_back(s.modeChain.at(i));
}
return graph;
}
} // namespace optimize_fixture
/* ****************************************************************************/
// Test for optimizing a HybridBayesTree with a given assignment.
TEST(HybridBayesTree, OptimizeAssignment) {
Switching s(4);
using namespace optimize_fixture;
size_t N = 4;
HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1;
// Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4
for (size_t i = 1; i < 4; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the Gaussian factors, 1 prior on X(1),
// 3 measurements on X(2), X(3), X(4)
graph1.push_back(s.linearizedFactorGraph.at(0));
for (size_t i = 4; i <= 7; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the discrete factors
for (size_t i = 7; i <= 9; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
// Then add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
// Finally add the discrete factors
// m0, m1-m0, m2-m1
HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N);
isam.update(graph1);
@ -409,12 +419,13 @@ TEST(HybridBayesTree, OptimizeAssignment) {
EXPECT(assert_equal(expected_delta, delta));
Switching s(N);
// Create ordering.
Ordering ordering;
for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
s.linearizedFactorGraph().eliminatePartialSequential(ordering);
GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
VectorValues expected = gbn.optimize();
@ -425,38 +436,29 @@ TEST(HybridBayesTree, OptimizeAssignment) {
/* ****************************************************************************/
// Test for optimizing a HybridBayesTree.
TEST(HybridBayesTree, Optimize) {
Switching s(4);
using namespace optimize_fixture;
size_t N = 4;
HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1;
// Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4
for (size_t i = 1; i < 4; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(2), X(3), X(4)
graph1.push_back(s.linearizedFactorGraph.at(0));
for (size_t i = 4; i <= 6; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the discrete factors
for (size_t i = 7; i <= 9; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
// Then add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
// Finally add the discrete factors
// m0, m1-m0, m2-m1
HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N);
isam.update(graph1);
HybridValues delta = isam.optimize();
Switching s(N);
// Create ordering.
Ordering ordering;
for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
s.linearizedFactorGraph().eliminatePartialSequential(ordering);
DiscreteFactorGraph dfg;
for (auto&& f : *remainingFactorGraph) {
@ -481,27 +483,18 @@ TEST(HybridBayesTree, Optimize) {
/* ****************************************************************************/
// Test for choosing a GaussianBayesTree from a HybridBayesTree.
TEST(HybridBayesTree, Choose) {
Switching s(4);
using namespace optimize_fixture;
size_t N = 4;
HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1;
// Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4
for (size_t i = 1; i < 4; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(2), X(3), X(4)
graph1.push_back(s.linearizedFactorGraph.at(0));
for (size_t i = 4; i <= 6; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the discrete factors
for (size_t i = 7; i <= 9; i++) {
graph1.push_back(s.linearizedFactorGraph.at(i));
}
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
// Then add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
// Finally add the discrete factors
// m0, m1-m0, m2-m1
HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N);
isam.update(graph1);
@ -513,8 +506,9 @@ TEST(HybridBayesTree, Choose) {
GaussianBayesTree gbt = isam.choose(assignment);
// Specify ordering so it matches that of HybridGaussianISAM.
Switching s(N);
Ordering ordering(KeyVector{X(0), X(1), X(2), X(3), M(0), M(1), M(2)});
auto bayesTree = s.linearizedFactorGraph.eliminateMultifrontal(ordering);
auto bayesTree = s.linearizedFactorGraph().eliminateMultifrontal(ordering);
auto expected_gbt = bayesTree->choose(assignment);

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

@ -82,7 +82,7 @@ TEST(HybridEstimation, Full) {
// Switching example of robot moving in 1D
// with given measurements and equal mode priors.
Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1");
HybridGaussianFactorGraph graph = switching.linearizedFactorGraph;
HybridGaussianFactorGraph graph = switching.linearizedFactorGraph();
Ordering hybridOrdering;
for (size_t k = 0; k < K; k++) {
@ -325,7 +325,7 @@ TEST(HybridEstimation, Probability) {
// given measurements and equal mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements,
"1/1 1/1");
auto graph = switching.linearizedFactorGraph;
auto graph = switching.linearizedFactorGraph();
// Continuous elimination
Ordering continuous_ordering(graph.continuousKeySet());
@ -365,7 +365,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
// mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements,
"1/1 1/1");
auto graph = switching.linearizedFactorGraph;
auto graph = switching.linearizedFactorGraph();
// Get the tree of unnormalized probabilities for each mode sequence.
AlgebraicDecisionTree<Key> expected_probPrimeTree = GetProbPrimeTree(graph);

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

@ -175,7 +175,7 @@ TEST(HybridBayesNet, Switching) {
Switching s(2, betweenSigma, priorSigma);
// Check size of linearized factor graph
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph();
EXPECT_LONGS_EQUAL(4, graph.size());
// Create some continuous and discrete values
@ -184,7 +184,7 @@ TEST(HybridBayesNet, Switching) {
const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}};
// Get the hybrid gaussian factor and check it is as expected
auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(graph.at(1));
auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(graph.at(2));
CHECK(hgf);
// Get factors and scalars for both modes
@ -298,7 +298,7 @@ TEST(HybridBayesNet, Switching) {
factors_x1.push_back(
factor); // Use the remaining factor from previous elimination
factors_x1.push_back(
graph.at(2)); // Add the factor for x1 from the original graph
graph.at(1)); // Add the factor for x1 from the original graph
// Test collectProductFactor for x1 clique
auto productFactor_x1 = factors_x1.collectProductFactor();
@ -356,7 +356,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
Switching s(3);
// Check size of linearized factor graph
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph();
EXPECT_LONGS_EQUAL(7, graph.size());
// Eliminate the graph
@ -383,16 +383,16 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
TEST(HybridGaussianFactorGraph, DiscreteSelection) {
Switching s(3);
HybridGaussianFactorGraph graph = s.linearizedFactorGraph;
HybridGaussianFactorGraph graph = s.linearizedFactorGraph();
DiscreteValues dv00{{M(0), 0}, {M(1), 0}};
GaussianFactorGraph continuous_00 = graph(dv00);
GaussianFactorGraph expected_00;
expected_00.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1)));
expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1)));
expected_00.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
expected_00.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1)));
expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1)));
EXPECT(assert_equal(expected_00, continuous_00));
@ -400,10 +400,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) {
GaussianFactorGraph continuous_01 = graph(dv01);
GaussianFactorGraph expected_01;
expected_01.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1)));
expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0)));
expected_01.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
expected_01.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1)));
expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0)));
EXPECT(assert_equal(expected_01, continuous_01));
@ -411,10 +411,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) {
GaussianFactorGraph continuous_10 = graph(dv10);
GaussianFactorGraph expected_10;
expected_10.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0)));
expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1)));
expected_10.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
expected_10.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0)));
expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1)));
EXPECT(assert_equal(expected_10, continuous_10));
@ -422,16 +422,16 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) {
GaussianFactorGraph continuous_11 = graph(dv11);
GaussianFactorGraph expected_11;
expected_11.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10)));
expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0)));
expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0)));
expected_11.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10)));
expected_11.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10)));
expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0)));
expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0)));
EXPECT(assert_equal(expected_11, continuous_11));
}
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, optimize) {
TEST(HybridGaussianFactorGraph, Optimize) {
HybridGaussianFactorGraph hfg;
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
@ -451,16 +451,16 @@ TEST(HybridGaussianFactorGraph, Conditionals) {
Switching switching(4);
HybridGaussianFactorGraph hfg;
hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X0)
hfg.push_back(switching.linearUnaryFactors.at(0)); // P(X0)
Ordering ordering;
ordering.push_back(X(0));
HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering);
HybridGaussianFactorGraph hfg2;
hfg2.push_back(*bayes_net); // P(X0)
hfg2.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0)
hfg2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1)
hfg2.push_back(switching.linearizedFactorGraph.at(5)); // P(M1)
hfg2.push_back(*bayes_net); // P(X0)
hfg2.push_back(switching.linearBinaryFactors.at(0)); // P(X0, X1 | M0)
hfg2.push_back(switching.linearBinaryFactors.at(1)); // P(X1, X2 | M1)
hfg2.push_back(switching.linearUnaryFactors.at(2)); // P(X2)
ordering += X(1), X(2), M(0), M(1);
// Created product of first two factors and check eliminate:
@ -510,13 +510,13 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
Switching s(4);
HybridGaussianFactorGraph graph;
graph.push_back(s.linearizedFactorGraph.at(0)); // f(X0)
graph.push_back(s.linearizedFactorGraph.at(1)); // f(X0, X1, M0)
graph.push_back(s.linearizedFactorGraph.at(2)); // f(X1, X2, M1)
graph.push_back(s.linearizedFactorGraph.at(4)); // f(X1)
graph.push_back(s.linearizedFactorGraph.at(5)); // f(X2)
graph.push_back(s.linearizedFactorGraph.at(7)); // f(M0)
graph.push_back(s.linearizedFactorGraph.at(8)); // f(M0, M1)
graph.push_back(s.linearUnaryFactors.at(0)); // f(X0)
graph.push_back(s.linearBinaryFactors.at(0)); // f(X0, X1, M0)
graph.push_back(s.linearBinaryFactors.at(1)); // f(X1, X2, M1)
graph.push_back(s.linearUnaryFactors.at(1)); // f(X1)
graph.push_back(s.linearUnaryFactors.at(2)); // f(X2)
graph.push_back(s.modeChain.at(0)); // f(M0)
graph.push_back(s.modeChain.at(1)); // f(M0, M1)
HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
@ -531,8 +531,8 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
graph = HybridGaussianFactorGraph();
graph.push_back(*hybridBayesNet);
graph.push_back(s.linearizedFactorGraph.at(3)); // f(X2, X3, M2)
graph.push_back(s.linearizedFactorGraph.at(6)); // f(X3)
graph.push_back(s.linearBinaryFactors.at(2)); // f(X2, X3, M2)
graph.push_back(s.linearUnaryFactors.at(3)); // f(X3)
hybridBayesNet = graph.eliminateSequential();
EXPECT_LONGS_EQUAL(7, hybridBayesNet->size());

42
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -42,15 +42,15 @@ using symbol_shorthand::Z;
/* ****************************************************************************/
namespace switching3 {
// ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1)
// ϕ(x0) ϕ(x1;z1) ϕ(x2;z2) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(m0) ϕ(m0,m1)
const Switching switching(3);
const HybridGaussianFactorGraph &lfg = switching.linearizedFactorGraph;
const HybridGaussianFactorGraph &lfg = switching.linearizedFactorGraph();
// First update graph: ϕ(x0) ϕ(x0,x1,m0) ϕ(m0)
const HybridGaussianFactorGraph graph1{lfg.at(0), lfg.at(1), lfg.at(5)};
const HybridGaussianFactorGraph graph1{lfg.at(0), lfg.at(3), lfg.at(5)};
// Second update graph: ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0,m1)
const HybridGaussianFactorGraph graph2{lfg.at(2), lfg.at(3), lfg.at(4),
const HybridGaussianFactorGraph graph2{lfg.at(4), lfg.at(1), lfg.at(2),
lfg.at(6)};
} // namespace switching3
@ -108,7 +108,7 @@ TEST(HybridGaussianElimination, IncrementalInference) {
// Now we calculate the expected factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
switching.linearizedFactorGraph().eliminatePartialMultifrontal(ordering);
// The densities on X(0) should be the same
auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
@ -162,15 +162,14 @@ TEST(HybridGaussianElimination, Approx_inference) {
HybridGaussianFactorGraph graph1;
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
for (size_t i = 1; i < 4; i++) {
graph1.push_back(switching.linearizedFactorGraph.at(i));
for (size_t i = 0; i < 3; i++) {
graph1.push_back(switching.linearBinaryFactors.at(i));
}
// Add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
graph1.push_back(switching.linearizedFactorGraph.at(0));
for (size_t i = 4; i <= 7; i++) {
graph1.push_back(switching.linearizedFactorGraph.at(i));
// Add the Gaussian factors, 1 prior on x0,
// 3 measurements on x1, x2, x3
for (size_t i = 0; i <= 3; i++) {
graph1.push_back(switching.linearUnaryFactors.at(i));
}
// Create ordering.
@ -181,7 +180,7 @@ TEST(HybridGaussianElimination, Approx_inference) {
// Now we calculate the actual factors using full elimination
const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] =
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
switching.linearizedFactorGraph().eliminatePartialMultifrontal(ordering);
size_t maxNrLeaves = 5;
incrementalHybrid.update(graph1);
@ -266,15 +265,14 @@ TEST(HybridGaussianElimination, IncrementalApproximate) {
/***** Run Round 1 *****/
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
for (size_t i = 1; i < 4; i++) {
graph1.push_back(switching.linearizedFactorGraph.at(i));
for (size_t i = 0; i < 3; i++) {
graph1.push_back(switching.linearBinaryFactors.at(i));
}
// Add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
graph1.push_back(switching.linearizedFactorGraph.at(0));
for (size_t i = 5; i <= 7; i++) {
graph1.push_back(switching.linearizedFactorGraph.at(i));
// Add the Gaussian factors, 1 prior on x0,
// 3 measurements on x1, x2, x3
for (size_t i = 0; i <= 3; i++) {
graph1.push_back(switching.linearUnaryFactors.at(i));
}
// Run update with pruning
@ -296,8 +294,8 @@ TEST(HybridGaussianElimination, IncrementalApproximate) {
/***** Run Round 2 *****/
HybridGaussianFactorGraph graph2;
graph2.push_back(switching.linearizedFactorGraph.at(4));
graph2.push_back(switching.linearizedFactorGraph.at(8));
graph2.push_back(switching.linearBinaryFactors.at(3)); // x3-x4
graph2.push_back(switching.linearUnaryFactors.at(4)); // x4
// Run update with pruning a second time.
incrementalHybrid.update(graph2);

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

@ -189,8 +189,10 @@ TEST(HybridGaussianProductFactor, AddPruned) {
product += prunedFactorB;
EXPECT_LONGS_EQUAL(6, product.nrLeaves());
#ifdef GTSAM_DT_MERGING
auto pruned = product.removeEmpty();
EXPECT_LONGS_EQUAL(5, pruned.nrLeaves());
#endif
}
/* ************************************************************************* */

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

@ -249,7 +249,7 @@ TEST(HybridNonlinearFactorGraph, Switching) {
Switching self(3);
EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph().size());
EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph.size());
EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph().size());
}
/****************************************************************************
@ -276,7 +276,7 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) {
for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
// Create elimination tree.
HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
HybridEliminationTree etree(self.linearizedFactorGraph(), ordering);
EXPECT_LONGS_EQUAL(1, etree.roots().size())
}
@ -286,12 +286,12 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) {
TEST(GaussianElimination, Eliminate_x0) {
Switching self(3);
// Gather factors on x1, has a simple Gaussian and a hybrid factor.
// Gather factors on x0, has a simple Gaussian and a hybrid factor.
HybridGaussianFactorGraph factors;
// Add gaussian prior
factors.push_back(self.linearizedFactorGraph[0]);
factors.push_back(self.linearUnaryFactors[0]);
// Add first hybrid factor
factors.push_back(self.linearizedFactorGraph[1]);
factors.push_back(self.linearBinaryFactors[0]);
// Eliminate x0
const Ordering ordering{X(0)};
@ -313,8 +313,8 @@ TEST(HybridsGaussianElimination, Eliminate_x1) {
// Gather factors on x1, will be two hybrid factors (with x0 and x2, resp.).
HybridGaussianFactorGraph factors;
factors.push_back(self.linearizedFactorGraph[1]); // involves m0
factors.push_back(self.linearizedFactorGraph[2]); // involves m1
factors.push_back(self.linearBinaryFactors[0]); // involves m0
factors.push_back(self.linearBinaryFactors[1]); // involves m1
// Eliminate x1
const Ordering ordering{X(1)};
@ -349,7 +349,7 @@ GaussianFactorGraph::shared_ptr batchGFG(double between,
TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
Switching self(2, 1.0, 0.1);
auto factors = self.linearizedFactorGraph;
auto factors = self.linearizedFactorGraph();
// Eliminate x0
const Ordering ordering{X(0), X(1)};
@ -380,15 +380,13 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
TEST(HybridNonlinearFactorGraph, Partial_Elimination) {
Switching self(3);
auto linearizedFactorGraph = self.linearizedFactorGraph;
// Create ordering of only continuous variables.
Ordering ordering;
for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
// Eliminate partially i.e. only continuous part.
const auto [hybridBayesNet, remainingFactorGraph] =
linearizedFactorGraph.eliminatePartialSequential(ordering);
self.linearizedFactorGraph().eliminatePartialSequential(ordering);
CHECK(hybridBayesNet);
EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
@ -444,11 +442,11 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) {
// Get nonlinear factor graph and add linear factors to be holistic.
// TODO(Frank): ???
HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph();
fg.add(self.linearizedFactorGraph);
fg.add(self.linearizedFactorGraph());
// Optimize to get HybridValues
HybridBayesNet::shared_ptr bn =
self.linearizedFactorGraph.eliminateSequential();
self.linearizedFactorGraph().eliminateSequential();
HybridValues hv = bn->optimize();
// Print and verify
@ -465,8 +463,6 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) {
TEST(HybridNonlinearFactorGraph, Full_Elimination) {
Switching self(3);
auto linearizedFactorGraph = self.linearizedFactorGraph;
// We first do a partial elimination
DiscreteBayesNet discreteBayesNet;
@ -477,7 +473,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
// Eliminate partially.
const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
linearizedFactorGraph.eliminatePartialSequential(ordering);
self.linearizedFactorGraph().eliminatePartialSequential(ordering);
DiscreteFactorGraph discrete_fg;
// TODO(Varun) Make this a function of HybridGaussianFactorGraph?
@ -500,7 +496,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
// Eliminate partially.
HybridBayesNet::shared_ptr hybridBayesNet =
linearizedFactorGraph.eliminateSequential(ordering);
self.linearizedFactorGraph().eliminateSequential(ordering);
CHECK(hybridBayesNet);
EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
@ -533,7 +529,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
TEST(HybridNonlinearFactorGraph, Printing) {
Switching self(3);
auto linearizedFactorGraph = self.linearizedFactorGraph;
auto linearizedFactorGraph = self.linearizedFactorGraph();
// Create ordering.
Ordering ordering;
@ -556,6 +552,24 @@ GaussianFactor:
No noise model
Factor 1
GaussianFactor:
A[x1] = [
10
]
b = [ -10 ]
No noise model
Factor 2
GaussianFactor:
A[x2] = [
10
]
b = [ -10 ]
No noise model
Factor 3
HybridGaussianFactor:
Hybrid [x0 x1; m0]{
Choice(m0)
@ -583,7 +597,7 @@ scalar: 0.918939
}
Factor 2
Factor 4
HybridGaussianFactor:
Hybrid [x1 x2; m1]{
Choice(m1)
@ -611,24 +625,6 @@ scalar: 0.918939
}
Factor 3
GaussianFactor:
A[x1] = [
10
]
b = [ -10 ]
No noise model
Factor 4
GaussianFactor:
A[x2] = [
10
]
b = [ -10 ]
No noise model
Factor 5
DiscreteFactor:
P( m0 ):
@ -651,16 +647,38 @@ DiscreteFactor:
#else
string expected_hybridFactorGraph = R"(
size: 7
factor 0:
Factor 0
GaussianFactor:
A[x0] = [
10
]
b = [ -10 ]
No noise model
factor 1:
Factor 1
GaussianFactor:
A[x1] = [
10
]
b = [ -10 ]
No noise model
Factor 2
GaussianFactor:
A[x2] = [
10
]
b = [ -10 ]
No noise model
Factor 3
HybridGaussianFactor:
Hybrid [x0 x1; m0]{
Choice(m0)
0 Leaf:
0 Leaf :
A[x0] = [
-1
]
@ -669,8 +687,9 @@ Hybrid [x0 x1; m0]{
]
b = [ -1 ]
No noise model
scalar: 0.918939
1 Leaf:
1 Leaf :
A[x0] = [
-1
]
@ -679,12 +698,15 @@ Hybrid [x0 x1; m0]{
]
b = [ -0 ]
No noise model
scalar: 0.918939
}
factor 2:
Factor 4
HybridGaussianFactor:
Hybrid [x1 x2; m1]{
Choice(m1)
0 Leaf:
0 Leaf :
A[x1] = [
-1
]
@ -693,8 +715,9 @@ Hybrid [x1 x2; m1]{
]
b = [ -1 ]
No noise model
scalar: 0.918939
1 Leaf:
1 Leaf :
A[x1] = [
-1
]
@ -703,26 +726,21 @@ Hybrid [x1 x2; m1]{
]
b = [ -0 ]
No noise model
scalar: 0.918939
}
factor 3:
A[x1] = [
10
]
b = [ -10 ]
No noise model
factor 4:
A[x2] = [
10
]
b = [ -10 ]
No noise model
factor 5: P( m0 ):
Choice(m0)
0 Leaf 0.5
1 Leaf 0.5
factor 6: P( m1 | m0 ):
Factor 5
DiscreteFactor:
P( m0 ):
Choice(m0)
0 Leaf 0.5
1 Leaf 0.5
Factor 6
DiscreteFactor:
P( m1 | m0 ):
Choice(m1)
0 Choice(m0)
0 0 Leaf 0.33333333
@ -731,6 +749,7 @@ factor 6: P( m1 | m0 ):
1 0 Leaf 0.66666667
1 1 Leaf 0.4
)";
#endif

9
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -147,7 +147,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
// Now we calculate the actual factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph
switching.linearizedFactorGraph()
.BaseEliminateable::eliminatePartialMultifrontal(ordering);
// The densities on X(1) should be the same
@ -214,8 +214,6 @@ TEST(HybridNonlinearISAM, Approx_inference) {
initial.insert<double>(X(i), i + 1);
}
// TODO(Frank): no mode chain?
// Create ordering.
Ordering ordering;
for (size_t j = 0; j < 4; j++) {
@ -224,7 +222,7 @@ TEST(HybridNonlinearISAM, Approx_inference) {
// Now we calculate the actual factors using full elimination
const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] =
switching.linearizedFactorGraph
switching.linearizedFactorGraph()
.BaseEliminateable::eliminatePartialMultifrontal(ordering);
size_t maxNrLeaves = 5;
@ -325,7 +323,6 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
// TODO(Frank): no mode chain?
// Run update with pruning
size_t maxComponents = 5;
incrementalHybrid.update(graph1, initial);
@ -347,7 +344,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
/***** Run Round 2 *****/
HybridGaussianFactorGraph graph2;
graph2.push_back(switching.binaryFactors.at(3)); // x3-x4
graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement
graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement
initial = Values();
initial.insert<double>(X(4), 5);

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

@ -130,7 +130,7 @@ TEST(HybridSerialization, HybridGaussianConditional) {
// Test HybridBayesNet serialization.
TEST(HybridSerialization, HybridBayesNet) {
Switching s(2);
HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential());
HybridBayesNet hbn = *(s.linearizedFactorGraph().eliminateSequential());
EXPECT(equalsObj<HybridBayesNet>(hbn));
EXPECT(equalsXML<HybridBayesNet>(hbn));
@ -141,7 +141,7 @@ TEST(HybridSerialization, HybridBayesNet) {
// Test HybridBayesTree serialization.
TEST(HybridSerialization, HybridBayesTree) {
Switching s(2);
HybridBayesTree hbt = *(s.linearizedFactorGraph.eliminateMultifrontal());
HybridBayesTree hbt = *(s.linearizedFactorGraph().eliminateMultifrontal());
EXPECT(equalsObj<HybridBayesTree>(hbt));
EXPECT(equalsXML<HybridBayesTree>(hbt));