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

Hybrid Gaussian Conditional Elimination #1301

Merged
merged 4 commits into from
Oct 7, 2022
Merged

Hybrid Gaussian Conditional Elimination #1301

Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
bc8c77c
rename test file to correct form
varunagrawal Oct 4, 2022
8820bf2
Add test to expose bug in elimination with gaussian conditionals
varunagrawal Oct 4, 2022
9002b68
fix the bug
varunagrawal Oct 4, 2022
6238a1f
more docs for Switching example
varunagrawal Oct 4, 2022
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
Prev Previous commit
Next Next commit
Add test to expose bug in elimination with gaussian conditionals
  • Loading branch information
@varunagrawal
varunagrawal committed Oct 4, 2022
commit 8820bf272c268ae298d99d8e13fe81052af80fc2
41 changes: 41 additions & 0 deletions gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -500,6 +500,7 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
}
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, optimize) {
HybridGaussianFactorGraph hfg;

Expand All @@ -521,6 +522,46 @@ TEST(HybridGaussianFactorGraph, optimize) {

EXPECT(assert_equal(hv.atDiscrete(C(1)), int(0)));
}

/* ************************************************************************* */
// Test adding of gaussian conditional and re-elimination.
TEST(HybridGaussianFactorGraph, Conditionals) {
Switching switching(4);
HybridGaussianFactorGraph hfg;

hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X1)
Ordering ordering;
ordering.push_back(X(1));
HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering);

hfg.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1)
hfg.push_back(*bayes_net);
hfg.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2)
hfg.push_back(switching.linearizedFactorGraph.at(5)); // P(M1)
ordering.push_back(X(2));
ordering.push_back(X(3));
ordering.push_back(M(1));
ordering.push_back(M(2));

bayes_net = hfg.eliminateSequential(ordering);

HybridValues result = bayes_net->optimize();

Values expected_continuous;
expected_continuous.insert<double>(X(1), 0);
expected_continuous.insert<double>(X(2), 1);
expected_continuous.insert<double>(X(3), 2);
expected_continuous.insert<double>(X(4), 4);
Values result_continuous =
switching.linearizationPoint.retract(result.continuous());
EXPECT(assert_equal(expected_continuous, result_continuous));

DiscreteValues expected_discrete;
expected_discrete[M(1)] = 1;
expected_discrete[M(2)] = 1;
EXPECT(assert_equal(expected_discrete, result.discrete()));
}

/* ************************************************************************* */
int main() {
TestResult tr;
Expand Down