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