12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

formatting testHybridGaussianFactorGraph

release/4.3a0
Varun Agrawal 2024-10-08 15:05:05 -04:00
parent 21b4c4c8d3
commit 8b8466e046
1 changed files with 18 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -17,6 +17,8 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
@ -37,9 +39,6 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <cstddef> #include <cstddef>
#include <memory> #include <memory>
#include <vector> #include <vector>
@ -73,8 +72,8 @@ TEST(HybridGaussianFactorGraph, Creation) {
HybridGaussianConditional gm( HybridGaussianConditional gm(
m0, m0,
{std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3), {std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
std::make_shared<GaussianConditional>( std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)}); I_3x3)});
hfg.add(gm); hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size()); EXPECT_LONGS_EQUAL(2, hfg.size());
@ -118,8 +117,8 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second); auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
CHECK(factor); CHECK(factor);
// regression test // regression test
EXPECT( // Originally 15.74961, which is normalized to 1
assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5)); EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -177,7 +176,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
@ -203,20 +202,20 @@ TEST(HybridBayesNet, Switching) {
// Check error for M(0) = 0 // Check error for M(0) = 0
const HybridValues values0{continuousValues, modeZero}; const HybridValues values0{continuousValues, modeZero};
double expectedError0 = 0; double expectedError0 = 0;
for (const auto& factor : graph) expectedError0 += factor->error(values0); for (const auto &factor : graph) expectedError0 += factor->error(values0);
EXPECT_DOUBLES_EQUAL(expectedError0, graph.error(values0), 1e-5); EXPECT_DOUBLES_EQUAL(expectedError0, graph.error(values0), 1e-5);
// Check error for M(0) = 1 // Check error for M(0) = 1
const HybridValues values1{continuousValues, modeOne}; const HybridValues values1{continuousValues, modeOne};
double expectedError1 = 0; double expectedError1 = 0;
for (const auto& factor : graph) expectedError1 += factor->error(values1); for (const auto &factor : graph) expectedError1 += factor->error(values1);
EXPECT_DOUBLES_EQUAL(expectedError1, graph.error(values1), 1e-5); EXPECT_DOUBLES_EQUAL(expectedError1, graph.error(values1), 1e-5);
// Check errorTree // Check errorTree
AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues); AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues);
// Create expected error tree // Create expected error tree
const AlgebraicDecisionTree<Key> expectedErrors( const AlgebraicDecisionTree<Key> expectedErrors(M(0), expectedError0,
M(0), expectedError0, expectedError1); expectedError1);
// Check that the actual error tree matches the expected one // Check that the actual error tree matches the expected one
EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5)); EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5));
@ -232,8 +231,8 @@ TEST(HybridBayesNet, Switching) {
const AlgebraicDecisionTree<Key> graphPosterior = const AlgebraicDecisionTree<Key> graphPosterior =
graph.discretePosterior(continuousValues); graph.discretePosterior(continuousValues);
const double sum = probPrime0 + probPrime1; const double sum = probPrime0 + probPrime1;
const AlgebraicDecisionTree<Key> expectedPosterior( const AlgebraicDecisionTree<Key> expectedPosterior(M(0), probPrime0 / sum,
M(0), probPrime0 / sum, probPrime1 / sum); probPrime1 / sum);
EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5)); EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5));
// Make the clique of factors connected to x0: // Make the clique of factors connected to x0:
@ -275,15 +274,13 @@ TEST(HybridBayesNet, Switching) {
// Check that the scalars incorporate the negative log constant of the // Check that the scalars incorporate the negative log constant of the
// conditional // conditional
EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(), EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(),
(*phi_x1_m)(modeZero).second, (*phi_x1_m)(modeZero).second, 1e-9);
1e-9);
EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(), EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(),
(*phi_x1_m)(modeOne).second, (*phi_x1_m)(modeOne).second, 1e-9);
1e-9);
// Check that the conditional and remaining factor are consistent for both // Check that the conditional and remaining factor are consistent for both
// modes // modes
for (auto&& mode : {modeZero, modeOne}) { for (auto &&mode : {modeZero, modeOne}) {
const auto gc = (*p_x0_given_x1_m)(mode); const auto gc = (*p_x0_given_x1_m)(mode);
const auto [gf, scalar] = (*phi_x1_m)(mode); const auto [gf, scalar] = (*phi_x1_m)(mode);
@ -342,7 +339,7 @@ TEST(HybridBayesNet, Switching) {
// However, we can still check the total error for the clique factors_x1 and // However, we can still check the total error for the clique factors_x1 and
// the elimination results are equal, modulo -again- the negative log constant // the elimination results are equal, modulo -again- the negative log constant
// of the conditional. // of the conditional.
for (auto&& mode : {modeZero, modeOne}) { for (auto &&mode : {modeZero, modeOne}) {
auto gc_x1 = (*p_x1_given_m)(mode); auto gc_x1 = (*p_x1_given_m)(mode);
double originalError_x1 = factors_x1.error({continuousValues, mode}); double originalError_x1 = factors_x1.error({continuousValues, mode});
const double actualError = gc_x1->negLogConstant() + const double actualError = gc_x1->negLogConstant() +
@ -372,7 +369,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