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

use single graph in tests

release/4.3a0
Varun Agrawal 2024-10-28 18:50:39 -04:00
parent 1f68a3d9b1
commit a7e48c0a81
1 changed files with 34 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,7 +49,7 @@ using symbol_shorthand::Z;
TEST(HybridNonlinearISAM, IncrementalElimination) {
Switching switching(3);
HybridNonlinearISAM isam;
HybridNonlinearFactorGraph graph1;
HybridNonlinearFactorGraph graph;
Values initial;
// Create initial factor graph
@ -57,17 +57,17 @@ TEST(HybridNonlinearISAM, IncrementalElimination) {
// | | |
// X0 -*- X1 -*- X2
// \*-M0-*/
graph1.push_back(switching.unaryFactors.at(0)); // P(X0)
graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0)
graph1.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1)
graph1.push_back(switching.modeChain.at(0)); // P(M0)
graph.push_back(switching.unaryFactors.at(0)); // P(X0)
graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0)
graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1)
graph.push_back(switching.modeChain.at(0)); // P(M0)
initial.insert<double>(X(0), 1);
initial.insert<double>(X(1), 2);
initial.insert<double>(X(2), 3);
// Run update step
isam.update(graph1, initial);
isam.update(graph, initial);
// Check that after update we have 3 hybrid Bayes net nodes:
// P(X0 | X1, M0) and P(X1, X2 | M0, M1), P(M0, M1)
@ -80,14 +80,14 @@ TEST(HybridNonlinearISAM, IncrementalElimination) {
/********************************************************/
// New factor graph for incremental update.
HybridNonlinearFactorGraph graph2;
graph = HybridNonlinearFactorGraph();
initial = Values();
graph2.push_back(switching.unaryFactors.at(1)); // P(X1)
graph2.push_back(switching.unaryFactors.at(2)); // P(X2)
graph2.push_back(switching.modeChain.at(1)); // P(M0, M1)
graph.push_back(switching.unaryFactors.at(1)); // P(X1)
graph.push_back(switching.unaryFactors.at(2)); // P(X2)
graph.push_back(switching.modeChain.at(1)); // P(M0, M1)
isam.update(graph2, initial);
isam.update(graph, initial);
bayesTree = isam.bayesTree();
// Check that after the second update we have
@ -103,7 +103,7 @@ TEST(HybridNonlinearISAM, IncrementalElimination) {
TEST(HybridNonlinearISAM, IncrementalInference) {
Switching switching(3);
HybridNonlinearISAM isam;
HybridNonlinearFactorGraph graph1;
HybridNonlinearFactorGraph graph;
Values initial;
// Create initial factor graph
@ -112,16 +112,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
// X0 -*- X1 -*- X2
// | |
// *-M0 - * - M1
graph1.push_back(switching.unaryFactors.at(0)); // P(X0)
graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0)
graph1.push_back(switching.unaryFactors.at(1)); // P(X1)
graph1.push_back(switching.modeChain.at(0)); // P(M0)
graph.push_back(switching.unaryFactors.at(0)); // P(X0)
graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0)
graph.push_back(switching.unaryFactors.at(1)); // P(X1)
graph.push_back(switching.modeChain.at(0)); // P(M0)
initial.insert<double>(X(0), 1);
initial.insert<double>(X(1), 2);
// Run update step
isam.update(graph1, initial);
isam.update(graph, initial);
HybridGaussianISAM bayesTree = isam.bayesTree();
auto discreteConditional_m0 = bayesTree[M(0)]->conditional()->asDiscrete();
@ -129,16 +129,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
/********************************************************/
// New factor graph for incremental update.
HybridNonlinearFactorGraph graph2;
graph = HybridNonlinearFactorGraph();
initial = Values();
initial.insert<double>(X(2), 3);
graph2.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1)
graph2.push_back(switching.unaryFactors.at(2)); // P(X2)
graph2.push_back(switching.modeChain.at(1)); // P(M0, M1)
graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1)
graph.push_back(switching.unaryFactors.at(2)); // P(X2)
graph.push_back(switching.modeChain.at(1)); // P(M0, M1)
isam.update(graph2, initial);
isam.update(graph, initial);
bayesTree = isam.bayesTree();
/********************************************************/
@ -199,18 +199,18 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
TEST(HybridNonlinearISAM, ApproxInference) {
Switching switching(4);
HybridNonlinearISAM incrementalHybrid;
HybridNonlinearFactorGraph graph1;
HybridNonlinearFactorGraph graph;
Values initial;
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
for (size_t i = 0; i < 3; i++) {
graph1.push_back(switching.binaryFactors.at(i));
graph.push_back(switching.binaryFactors.at(i));
}
// Add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
for (size_t i = 0; i < 4; i++) {
graph1.push_back(switching.unaryFactors.at(i));
graph.push_back(switching.unaryFactors.at(i));
initial.insert<double>(X(i), i + 1);
}
@ -228,7 +228,7 @@ TEST(HybridNonlinearISAM, ApproxInference) {
.BaseEliminateable::eliminatePartialMultifrontal(ordering);
size_t maxNrLeaves = 5;
incrementalHybrid.update(graph1, initial);
incrementalHybrid.update(graph, initial);
HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree();
bayesTree.prune(maxNrLeaves);
@ -307,19 +307,19 @@ TEST(HybridNonlinearISAM, ApproxInference) {
TEST(HybridNonlinearISAM, IncrementalApproximate) {
Switching switching(5);
HybridNonlinearISAM incrementalHybrid;
HybridNonlinearFactorGraph graph1;
HybridNonlinearFactorGraph graph;
Values initial;
/***** Run Round 1 *****/
// Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
for (size_t i = 0; i < 3; i++) {
graph1.push_back(switching.binaryFactors.at(i));
graph.push_back(switching.binaryFactors.at(i));
}
// Add the Gaussian factors, 1 prior on X(0),
// 3 measurements on X(1), X(2), X(3)
for (size_t i = 0; i < 4; i++) {
graph1.push_back(switching.unaryFactors.at(i));
graph.push_back(switching.unaryFactors.at(i));
initial.insert<double>(X(i), i + 1);
}
@ -328,7 +328,7 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) {
// Run update with pruning
size_t maxComponents = 5;
incrementalHybrid.update(graph1, initial);
incrementalHybrid.update(graph, initial);
incrementalHybrid.prune(maxComponents);
HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree();
@ -345,14 +345,14 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) {
5, bayesTree[X(3)]->conditional()->asHybrid()->nrComponents());
/***** Run Round 2 *****/
HybridGaussianFactorGraph graph2;
graph2.push_back(switching.binaryFactors.at(3)); // x3-x4
graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement
graph = HybridGaussianFactorGraph();
graph.push_back(switching.binaryFactors.at(3)); // x3-x4
graph.push_back(switching.unaryFactors.at(4)); // x4 measurement
initial = Values();
initial.insert<double>(X(4), 5);
// Run update with pruning a second time.
incrementalHybrid.update(graph2, initial);
incrementalHybrid.update(graph, initial);
incrementalHybrid.prune(maxComponents);
bayesTree = incrementalHybrid.bayesTree();