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

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

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

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

@ -352,7 +352,7 @@ TEST(HybridBayesTree, OptimizeMultifrontal) {
Switching s(4); Switching s(4);
HybridBayesTree::shared_ptr hybridBayesTree = HybridBayesTree::shared_ptr hybridBayesTree =
s.linearizedFactorGraph.eliminateMultifrontal(); s.linearizedFactorGraph().eliminateMultifrontal();
HybridValues delta = hybridBayesTree->optimize(); HybridValues delta = hybridBayesTree->optimize();
VectorValues expectedValues; VectorValues expectedValues;
@ -364,30 +364,40 @@ TEST(HybridBayesTree, OptimizeMultifrontal) {
EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); 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 for optimizing a HybridBayesTree with a given assignment.
TEST(HybridBayesTree, OptimizeAssignment) { TEST(HybridBayesTree, OptimizeAssignment) {
Switching s(4); using namespace optimize_fixture;
size_t N = 4;
HybridGaussianISAM isam; HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1;
// Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
for (size_t i = 1; i < 4; i++) { // Then add the Gaussian factors, 1 prior on X(0),
graph1.push_back(s.linearizedFactorGraph.at(i)); // 3 measurements on X(1), X(2), X(3)
} // Finally add the discrete factors
// m0, m1-m0, m2-m1
// Add the Gaussian factors, 1 prior on X(1), HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N);
// 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));
}
isam.update(graph1); isam.update(graph1);
@ -409,12 +419,13 @@ TEST(HybridBayesTree, OptimizeAssignment) {
EXPECT(assert_equal(expected_delta, delta)); EXPECT(assert_equal(expected_delta, delta));
Switching s(N);
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering); s.linearizedFactorGraph().eliminatePartialSequential(ordering);
GaussianBayesNet gbn = hybridBayesNet->choose(assignment); GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
VectorValues expected = gbn.optimize(); VectorValues expected = gbn.optimize();
@ -425,38 +436,29 @@ TEST(HybridBayesTree, OptimizeAssignment) {
/* ****************************************************************************/ /* ****************************************************************************/
// Test for optimizing a HybridBayesTree. // Test for optimizing a HybridBayesTree.
TEST(HybridBayesTree, Optimize) { TEST(HybridBayesTree, Optimize) {
Switching s(4); using namespace optimize_fixture;
size_t N = 4;
HybridGaussianISAM isam; HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
// Then add the Gaussian factors, 1 prior on X(0),
// Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 // 3 measurements on X(1), X(2), X(3)
for (size_t i = 1; i < 4; i++) { // Finally add the discrete factors
graph1.push_back(s.linearizedFactorGraph.at(i)); // m0, m1-m0, m2-m1
} HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N);
// 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));
}
isam.update(graph1); isam.update(graph1);
HybridValues delta = isam.optimize(); HybridValues delta = isam.optimize();
Switching s(N);
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k));
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering); s.linearizedFactorGraph().eliminatePartialSequential(ordering);
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto&& f : *remainingFactorGraph) { for (auto&& f : *remainingFactorGraph) {
@ -481,27 +483,18 @@ TEST(HybridBayesTree, Optimize) {
/* ****************************************************************************/ /* ****************************************************************************/
// Test for choosing a GaussianBayesTree from a HybridBayesTree. // Test for choosing a GaussianBayesTree from a HybridBayesTree.
TEST(HybridBayesTree, Choose) { TEST(HybridBayesTree, Choose) {
Switching s(4); using namespace optimize_fixture;
size_t N = 4;
HybridGaussianISAM isam; HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1;
// Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3
for (size_t i = 1; i < 4; i++) { // Then add the Gaussian factors, 1 prior on X(0),
graph1.push_back(s.linearizedFactorGraph.at(i)); // 3 measurements on X(1), X(2), X(3)
} // Finally add the discrete factors
// m0, m1-m0, m2-m1
// Add the Gaussian factors, 1 prior on X(0), HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N);
// 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));
}
isam.update(graph1); isam.update(graph1);
@ -513,8 +506,9 @@ TEST(HybridBayesTree, Choose) {
GaussianBayesTree gbt = isam.choose(assignment); GaussianBayesTree gbt = isam.choose(assignment);
// Specify ordering so it matches that of HybridGaussianISAM. // 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)}); 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); 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 // Switching example of robot moving in 1D
// with given measurements and equal mode priors. // with given measurements and equal mode priors.
Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1"); Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1");
HybridGaussianFactorGraph graph = switching.linearizedFactorGraph; HybridGaussianFactorGraph graph = switching.linearizedFactorGraph();
Ordering hybridOrdering; Ordering hybridOrdering;
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
@ -325,7 +325,7 @@ TEST(HybridEstimation, Probability) {
// given measurements and equal mode priors. // given measurements and equal mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements, Switching switching(K, between_sigma, measurement_sigma, measurements,
"1/1 1/1"); "1/1 1/1");
auto graph = switching.linearizedFactorGraph; auto graph = switching.linearizedFactorGraph();
// Continuous elimination // Continuous elimination
Ordering continuous_ordering(graph.continuousKeySet()); Ordering continuous_ordering(graph.continuousKeySet());
@ -365,7 +365,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
// mode priors. // mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements, Switching switching(K, between_sigma, measurement_sigma, measurements,
"1/1 1/1"); "1/1 1/1");
auto graph = switching.linearizedFactorGraph; auto graph = switching.linearizedFactorGraph();
// Get the tree of unnormalized probabilities for each mode sequence. // Get the tree of unnormalized probabilities for each mode sequence.
AlgebraicDecisionTree<Key> expected_probPrimeTree = GetProbPrimeTree(graph); 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); Switching s(2, betweenSigma, priorSigma);
// Check size of linearized factor graph // Check size of linearized factor graph
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph();
EXPECT_LONGS_EQUAL(4, graph.size()); EXPECT_LONGS_EQUAL(4, graph.size());
// Create some continuous and discrete values // Create some continuous and discrete values
@ -184,7 +184,7 @@ TEST(HybridBayesNet, Switching) {
const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}}; const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}};
// Get the hybrid gaussian factor and check it is as expected // 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); CHECK(hgf);
// Get factors and scalars for both modes // Get factors and scalars for both modes
@ -298,7 +298,7 @@ TEST(HybridBayesNet, Switching) {
factors_x1.push_back( factors_x1.push_back(
factor); // Use the remaining factor from previous elimination factor); // Use the remaining factor from previous elimination
factors_x1.push_back( 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 // Test collectProductFactor for x1 clique
auto productFactor_x1 = factors_x1.collectProductFactor(); auto productFactor_x1 = factors_x1.collectProductFactor();
@ -356,7 +356,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
Switching s(3); Switching s(3);
// Check size of linearized factor graph // Check size of linearized factor graph
const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph();
EXPECT_LONGS_EQUAL(7, graph.size()); EXPECT_LONGS_EQUAL(7, graph.size());
// Eliminate the graph // Eliminate the graph
@ -383,16 +383,16 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
TEST(HybridGaussianFactorGraph, DiscreteSelection) { TEST(HybridGaussianFactorGraph, DiscreteSelection) {
Switching s(3); Switching s(3);
HybridGaussianFactorGraph graph = s.linearizedFactorGraph; HybridGaussianFactorGraph graph = s.linearizedFactorGraph();
DiscreteValues dv00{{M(0), 0}, {M(1), 0}}; DiscreteValues dv00{{M(0), 0}, {M(1), 0}};
GaussianFactorGraph continuous_00 = graph(dv00); GaussianFactorGraph continuous_00 = graph(dv00);
GaussianFactorGraph expected_00; GaussianFactorGraph expected_00;
expected_00.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); 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(1), I_1x1 * 10, Vector1(-10)));
expected_00.push_back(JacobianFactor(X(2), 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)); EXPECT(assert_equal(expected_00, continuous_00));
@ -400,10 +400,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) {
GaussianFactorGraph continuous_01 = graph(dv01); GaussianFactorGraph continuous_01 = graph(dv01);
GaussianFactorGraph expected_01; GaussianFactorGraph expected_01;
expected_01.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); 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(1), I_1x1 * 10, Vector1(-10)));
expected_01.push_back(JacobianFactor(X(2), 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)); EXPECT(assert_equal(expected_01, continuous_01));
@ -411,10 +411,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) {
GaussianFactorGraph continuous_10 = graph(dv10); GaussianFactorGraph continuous_10 = graph(dv10);
GaussianFactorGraph expected_10; GaussianFactorGraph expected_10;
expected_10.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-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(1), I_1x1 * 10, Vector1(-10)));
expected_10.push_back(JacobianFactor(X(2), 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)); EXPECT(assert_equal(expected_10, continuous_10));
@ -422,16 +422,16 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) {
GaussianFactorGraph continuous_11 = graph(dv11); GaussianFactorGraph continuous_11 = graph(dv11);
GaussianFactorGraph expected_11; GaussianFactorGraph expected_11;
expected_11.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); 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(1), I_1x1 * 10, Vector1(-10)));
expected_11.push_back(JacobianFactor(X(2), 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)); EXPECT(assert_equal(expected_11, continuous_11));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HybridGaussianFactorGraph, optimize) { TEST(HybridGaussianFactorGraph, Optimize) {
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
@ -451,16 +451,16 @@ TEST(HybridGaussianFactorGraph, Conditionals) {
Switching switching(4); Switching switching(4);
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;
hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X0) hfg.push_back(switching.linearUnaryFactors.at(0)); // P(X0)
Ordering ordering; Ordering ordering;
ordering.push_back(X(0)); ordering.push_back(X(0));
HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering);
HybridGaussianFactorGraph hfg2; HybridGaussianFactorGraph hfg2;
hfg2.push_back(*bayes_net); // P(X0) hfg2.push_back(*bayes_net); // P(X0)
hfg2.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0) hfg2.push_back(switching.linearBinaryFactors.at(0)); // P(X0, X1 | M0)
hfg2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1) hfg2.push_back(switching.linearBinaryFactors.at(1)); // P(X1, X2 | M1)
hfg2.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) hfg2.push_back(switching.linearUnaryFactors.at(2)); // P(X2)
ordering += X(1), X(2), M(0), M(1); ordering += X(1), X(2), M(0), M(1);
// Created product of first two factors and check eliminate: // Created product of first two factors and check eliminate:
@ -510,13 +510,13 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
Switching s(4); Switching s(4);
HybridGaussianFactorGraph graph; HybridGaussianFactorGraph graph;
graph.push_back(s.linearizedFactorGraph.at(0)); // f(X0) graph.push_back(s.linearUnaryFactors.at(0)); // f(X0)
graph.push_back(s.linearizedFactorGraph.at(1)); // f(X0, X1, M0) graph.push_back(s.linearBinaryFactors.at(0)); // f(X0, X1, M0)
graph.push_back(s.linearizedFactorGraph.at(2)); // f(X1, X2, M1) graph.push_back(s.linearBinaryFactors.at(1)); // f(X1, X2, M1)
graph.push_back(s.linearizedFactorGraph.at(4)); // f(X1) graph.push_back(s.linearUnaryFactors.at(1)); // f(X1)
graph.push_back(s.linearizedFactorGraph.at(5)); // f(X2) graph.push_back(s.linearUnaryFactors.at(2)); // f(X2)
graph.push_back(s.linearizedFactorGraph.at(7)); // f(M0) graph.push_back(s.modeChain.at(0)); // f(M0)
graph.push_back(s.linearizedFactorGraph.at(8)); // f(M0, M1) graph.push_back(s.modeChain.at(1)); // f(M0, M1)
HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
@ -531,8 +531,8 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
graph = HybridGaussianFactorGraph(); graph = HybridGaussianFactorGraph();
graph.push_back(*hybridBayesNet); graph.push_back(*hybridBayesNet);
graph.push_back(s.linearizedFactorGraph.at(3)); // f(X2, X3, M2) graph.push_back(s.linearBinaryFactors.at(2)); // f(X2, X3, M2)
graph.push_back(s.linearizedFactorGraph.at(6)); // f(X3) graph.push_back(s.linearUnaryFactors.at(3)); // f(X3)
hybridBayesNet = graph.eliminateSequential(); hybridBayesNet = graph.eliminateSequential();
EXPECT_LONGS_EQUAL(7, hybridBayesNet->size()); EXPECT_LONGS_EQUAL(7, hybridBayesNet->size());

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

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

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

@ -189,8 +189,10 @@ TEST(HybridGaussianProductFactor, AddPruned) {
product += prunedFactorB; product += prunedFactorB;
EXPECT_LONGS_EQUAL(6, product.nrLeaves()); EXPECT_LONGS_EQUAL(6, product.nrLeaves());
#ifdef GTSAM_DT_MERGING
auto pruned = product.removeEmpty(); auto pruned = product.removeEmpty();
EXPECT_LONGS_EQUAL(5, pruned.nrLeaves()); 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); Switching self(3);
EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph().size()); 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)); for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
// Create elimination tree. // Create elimination tree.
HybridEliminationTree etree(self.linearizedFactorGraph, ordering); HybridEliminationTree etree(self.linearizedFactorGraph(), ordering);
EXPECT_LONGS_EQUAL(1, etree.roots().size()) EXPECT_LONGS_EQUAL(1, etree.roots().size())
} }
@ -286,12 +286,12 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) {
TEST(GaussianElimination, Eliminate_x0) { TEST(GaussianElimination, Eliminate_x0) {
Switching self(3); 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; HybridGaussianFactorGraph factors;
// Add gaussian prior // Add gaussian prior
factors.push_back(self.linearizedFactorGraph[0]); factors.push_back(self.linearUnaryFactors[0]);
// Add first hybrid factor // Add first hybrid factor
factors.push_back(self.linearizedFactorGraph[1]); factors.push_back(self.linearBinaryFactors[0]);
// Eliminate x0 // Eliminate x0
const Ordering ordering{X(0)}; 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.). // Gather factors on x1, will be two hybrid factors (with x0 and x2, resp.).
HybridGaussianFactorGraph factors; HybridGaussianFactorGraph factors;
factors.push_back(self.linearizedFactorGraph[1]); // involves m0 factors.push_back(self.linearBinaryFactors[0]); // involves m0
factors.push_back(self.linearizedFactorGraph[2]); // involves m1 factors.push_back(self.linearBinaryFactors[1]); // involves m1
// Eliminate x1 // Eliminate x1
const Ordering ordering{X(1)}; const Ordering ordering{X(1)};
@ -349,7 +349,7 @@ GaussianFactorGraph::shared_ptr batchGFG(double between,
TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
Switching self(2, 1.0, 0.1); Switching self(2, 1.0, 0.1);
auto factors = self.linearizedFactorGraph; auto factors = self.linearizedFactorGraph();
// Eliminate x0 // Eliminate x0
const Ordering ordering{X(0), X(1)}; const Ordering ordering{X(0), X(1)};
@ -380,15 +380,13 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
TEST(HybridNonlinearFactorGraph, Partial_Elimination) { TEST(HybridNonlinearFactorGraph, Partial_Elimination) {
Switching self(3); Switching self(3);
auto linearizedFactorGraph = self.linearizedFactorGraph;
// Create ordering of only continuous variables. // Create ordering of only continuous variables.
Ordering ordering; Ordering ordering;
for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k));
// Eliminate partially i.e. only continuous part. // Eliminate partially i.e. only continuous part.
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
linearizedFactorGraph.eliminatePartialSequential(ordering); self.linearizedFactorGraph().eliminatePartialSequential(ordering);
CHECK(hybridBayesNet); CHECK(hybridBayesNet);
EXPECT_LONGS_EQUAL(3, hybridBayesNet->size()); EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
@ -444,11 +442,11 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) {
// Get nonlinear factor graph and add linear factors to be holistic. // Get nonlinear factor graph and add linear factors to be holistic.
// TODO(Frank): ??? // TODO(Frank): ???
HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph(); HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph();
fg.add(self.linearizedFactorGraph); fg.add(self.linearizedFactorGraph());
// Optimize to get HybridValues // Optimize to get HybridValues
HybridBayesNet::shared_ptr bn = HybridBayesNet::shared_ptr bn =
self.linearizedFactorGraph.eliminateSequential(); self.linearizedFactorGraph().eliminateSequential();
HybridValues hv = bn->optimize(); HybridValues hv = bn->optimize();
// Print and verify // Print and verify
@ -465,8 +463,6 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) {
TEST(HybridNonlinearFactorGraph, Full_Elimination) { TEST(HybridNonlinearFactorGraph, Full_Elimination) {
Switching self(3); Switching self(3);
auto linearizedFactorGraph = self.linearizedFactorGraph;
// We first do a partial elimination // We first do a partial elimination
DiscreteBayesNet discreteBayesNet; DiscreteBayesNet discreteBayesNet;
@ -477,7 +473,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
// Eliminate partially. // Eliminate partially.
const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
linearizedFactorGraph.eliminatePartialSequential(ordering); self.linearizedFactorGraph().eliminatePartialSequential(ordering);
DiscreteFactorGraph discrete_fg; DiscreteFactorGraph discrete_fg;
// TODO(Varun) Make this a function of HybridGaussianFactorGraph? // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
@ -500,7 +496,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
// Eliminate partially. // Eliminate partially.
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
linearizedFactorGraph.eliminateSequential(ordering); self.linearizedFactorGraph().eliminateSequential(ordering);
CHECK(hybridBayesNet); CHECK(hybridBayesNet);
EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
@ -533,7 +529,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) {
TEST(HybridNonlinearFactorGraph, Printing) { TEST(HybridNonlinearFactorGraph, Printing) {
Switching self(3); Switching self(3);
auto linearizedFactorGraph = self.linearizedFactorGraph; auto linearizedFactorGraph = self.linearizedFactorGraph();
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
@ -556,6 +552,24 @@ GaussianFactor:
No noise model 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: HybridGaussianFactor:
Hybrid [x0 x1; m0]{ Hybrid [x0 x1; m0]{
Choice(m0) Choice(m0)
@ -583,7 +597,7 @@ scalar: 0.918939
} }
Factor 2 Factor 4
HybridGaussianFactor: HybridGaussianFactor:
Hybrid [x1 x2; m1]{ Hybrid [x1 x2; m1]{
Choice(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 Factor 5
DiscreteFactor: DiscreteFactor:
P( m0 ): P( m0 ):
@ -651,16 +647,38 @@ DiscreteFactor:
#else #else
string expected_hybridFactorGraph = R"( string expected_hybridFactorGraph = R"(
size: 7 size: 7
factor 0: Factor 0
GaussianFactor:
A[x0] = [ A[x0] = [
10 10
] ]
b = [ -10 ] b = [ -10 ]
No noise model 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]{ Hybrid [x0 x1; m0]{
Choice(m0) Choice(m0)
0 Leaf: 0 Leaf :
A[x0] = [ A[x0] = [
-1 -1
] ]
@ -669,8 +687,9 @@ Hybrid [x0 x1; m0]{
] ]
b = [ -1 ] b = [ -1 ]
No noise model No noise model
scalar: 0.918939
1 Leaf: 1 Leaf :
A[x0] = [ A[x0] = [
-1 -1
] ]
@ -679,12 +698,15 @@ Hybrid [x0 x1; m0]{
] ]
b = [ -0 ] b = [ -0 ]
No noise model No noise model
scalar: 0.918939
} }
factor 2:
Factor 4
HybridGaussianFactor:
Hybrid [x1 x2; m1]{ Hybrid [x1 x2; m1]{
Choice(m1) Choice(m1)
0 Leaf: 0 Leaf :
A[x1] = [ A[x1] = [
-1 -1
] ]
@ -693,8 +715,9 @@ Hybrid [x1 x2; m1]{
] ]
b = [ -1 ] b = [ -1 ]
No noise model No noise model
scalar: 0.918939
1 Leaf: 1 Leaf :
A[x1] = [ A[x1] = [
-1 -1
] ]
@ -703,26 +726,21 @@ Hybrid [x1 x2; m1]{
] ]
b = [ -0 ] b = [ -0 ]
No noise model 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) Choice(m1)
0 Choice(m0) 0 Choice(m0)
0 0 Leaf 0.33333333 0 0 Leaf 0.33333333
@ -731,6 +749,7 @@ factor 6: P( m1 | m0 ):
1 0 Leaf 0.66666667 1 0 Leaf 0.66666667
1 1 Leaf 0.4 1 1 Leaf 0.4
)"; )";
#endif #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 // Now we calculate the actual factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] = const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph switching.linearizedFactorGraph()
.BaseEliminateable::eliminatePartialMultifrontal(ordering); .BaseEliminateable::eliminatePartialMultifrontal(ordering);
// The densities on X(1) should be the same // The densities on X(1) should be the same
@ -214,8 +214,6 @@ TEST(HybridNonlinearISAM, Approx_inference) {
initial.insert<double>(X(i), i + 1); initial.insert<double>(X(i), i + 1);
} }
// TODO(Frank): no mode chain?
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t j = 0; j < 4; j++) { 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 // Now we calculate the actual factors using full elimination
const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] =
switching.linearizedFactorGraph switching.linearizedFactorGraph()
.BaseEliminateable::eliminatePartialMultifrontal(ordering); .BaseEliminateable::eliminatePartialMultifrontal(ordering);
size_t maxNrLeaves = 5; size_t maxNrLeaves = 5;
@ -325,7 +323,6 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
// TODO(Frank): no mode chain? // TODO(Frank): no mode chain?
// Run update with pruning // Run update with pruning
size_t maxComponents = 5; size_t maxComponents = 5;
incrementalHybrid.update(graph1, initial); incrementalHybrid.update(graph1, initial);
@ -347,7 +344,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
/***** Run Round 2 *****/ /***** Run Round 2 *****/
HybridGaussianFactorGraph graph2; HybridGaussianFactorGraph graph2;
graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 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 = Values();
initial.insert<double>(X(4), 5); 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 HybridBayesNet serialization.
TEST(HybridSerialization, HybridBayesNet) { TEST(HybridSerialization, HybridBayesNet) {
Switching s(2); Switching s(2);
HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential()); HybridBayesNet hbn = *(s.linearizedFactorGraph().eliminateSequential());
EXPECT(equalsObj<HybridBayesNet>(hbn)); EXPECT(equalsObj<HybridBayesNet>(hbn));
EXPECT(equalsXML<HybridBayesNet>(hbn)); EXPECT(equalsXML<HybridBayesNet>(hbn));
@ -141,7 +141,7 @@ TEST(HybridSerialization, HybridBayesNet) {
// Test HybridBayesTree serialization. // Test HybridBayesTree serialization.
TEST(HybridSerialization, HybridBayesTree) { TEST(HybridSerialization, HybridBayesTree) {
Switching s(2); Switching s(2);
HybridBayesTree hbt = *(s.linearizedFactorGraph.eliminateMultifrontal()); HybridBayesTree hbt = *(s.linearizedFactorGraph().eliminateMultifrontal());
EXPECT(equalsObj<HybridBayesTree>(hbt)); EXPECT(equalsObj<HybridBayesTree>(hbt));
EXPECT(equalsXML<HybridBayesTree>(hbt)); EXPECT(equalsXML<HybridBayesTree>(hbt));