Skip to content

Commit

Permalink
Merge pull request #1892 from borglab/cleanup-hybrid-tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Oct 30, 2024
2 parents ecc7366 + 31f0011 commit f4f54dd
Show file tree
Hide file tree
Showing 10 changed files with 223 additions and 205 deletions.
45 changes: 26 additions & 19 deletions gtsam/hybrid/tests/Switching.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 changes: 7 additions & 6 deletions gtsam/hybrid/tests/testHybridBayesNet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

// Include for test suite
#include <CppUnitLite/TestHarness.h>

#include <memory>

using namespace std;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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();

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand All @@ -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;
Expand Down
112 changes: 53 additions & 59 deletions gtsam/hybrid/tests/testHybridBayesTree.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

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));
}
size_t N = 4;

// 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));
}
HybridGaussianISAM isam;

// 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);

Expand All @@ -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();
Expand All @@ -425,38 +436,29 @@ TEST(HybridBayesTree, OptimizeAssignment) {
/* ****************************************************************************/
// Test for optimizing a HybridBayesTree.
TEST(HybridBayesTree, Optimize) {
Switching s(4);
using namespace optimize_fixture;

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));
}
size_t N = 4;

// 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));
}
HybridGaussianISAM isam;
// 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) {
Expand All @@ -481,27 +483,18 @@ TEST(HybridBayesTree, Optimize) {
/* ****************************************************************************/
// Test for choosing a GaussianBayesTree from a HybridBayesTree.
TEST(HybridBayesTree, Choose) {
Switching s(4);

HybridGaussianISAM isam;
HybridGaussianFactorGraph graph1;
using namespace optimize_fixture;

// 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));
}
size_t N = 4;

// 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));
}
HybridGaussianISAM isam;

// 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);

Expand All @@ -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);

Expand Down
6 changes: 3 additions & 3 deletions gtsam/hybrid/tests/testHybridEstimation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++) {
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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);
Expand Down
Loading

0 comments on commit f4f54dd

Please sign in to comment.