Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Clean up Switching tests #1892

Merged
merged 3 commits into from
Oct 30, 2024
Merged

Clean up Switching tests #1892

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7f07509
remove linearizedFactorGraph and use linearized unary and binary factors
varunagrawal Oct 30, 2024
be9fb06
method to get the nonlinear factors
varunagrawal Oct 30, 2024
31f0011
update all tests
varunagrawal Oct 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 26 additions & 19 deletions gtsam/hybrid/tests/Switching.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading