From e711a62e2d1f2ee3011024bfdc7b5081d65d5212 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 May 2022 17:51:25 -0400 Subject: [PATCH] More tests working --- gtsam/hybrid/tests/Switching.h | 1 + .../tests/testNonlinearHybridFactorGraph.cpp | 155 ++++++++---------- 2 files changed, 69 insertions(+), 87 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index be58824e0..42c61767e 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -22,6 +22,7 @@ #include #include #include +#include #pragma once diff --git a/gtsam/hybrid/tests/testNonlinearHybridFactorGraph.cpp b/gtsam/hybrid/tests/testNonlinearHybridFactorGraph.cpp index 8043f78df..6e4019992 100644 --- a/gtsam/hybrid/tests/testNonlinearHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testNonlinearHybridFactorGraph.cpp @@ -58,7 +58,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { Values linearizationPoint; linearizationPoint.insert(X(0), 0); - GaussianHybridFactorGraph ghfg = *fg.linearize(linearizationPoint); + GaussianHybridFactorGraph ghfg = fg.linearize(linearizationPoint); // Add a factor to the GaussianFactorGraph ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5))); @@ -72,121 +72,102 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { /// NonlinearHybridFactorGraph. TEST(NonlinearHybridFactorGraph, Resize) { NonlinearHybridFactorGraph fg; - // auto nonlinearFactor = boost::make_shared>(); - // fg.push_back(nonlinearFactor); + auto nonlinearFactor = boost::make_shared>(); + fg.push_back(nonlinearFactor); - // auto discreteFactor = boost::make_shared(); - // fg.push_back(discreteFactor); + auto discreteFactor = boost::make_shared(); + fg.push_back(discreteFactor); - // auto dcFactor = boost::make_shared>(); - // fg.push_back(dcFactor); - // EXPECT_LONGS_EQUAL(fg.size(), 3); + auto dcFactor = boost::make_shared(); + fg.push_back(dcFactor); + + EXPECT_LONGS_EQUAL(fg.size(), 3); fg.resize(0); EXPECT_LONGS_EQUAL(fg.size(), 0); } -// /* ************************************************************************** -// */ -// /// Test that the resize method works correctly for a -// /// GaussianHybridFactorGraph. -// TEST(GaussianHybridFactorGraph, Resize) { -// NonlinearHybridFactorGraph nhfg; -// auto nonlinearFactor = boost::make_shared>( -// X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); -// nhfg.push_back(nonlinearFactor); -// auto discreteFactor = boost::make_shared(); -// nhfg.push_back(discreteFactor); +/* ************************************************************************** + */ +/// Test that the resize method works correctly for a +/// GaussianHybridFactorGraph. +TEST(GaussianHybridFactorGraph, Resize) { + NonlinearHybridFactorGraph nhfg; + auto nonlinearFactor = boost::make_shared>( + X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); + nhfg.push_back(nonlinearFactor); + auto discreteFactor = boost::make_shared(); + nhfg.push_back(discreteFactor); -// KeyVector contKeys = {X(0), X(1)}; -// auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); -// auto still = boost::make_shared(X(0), X(1), 0.0, noise_model), -// moving = boost::make_shared(X(0), X(1), 1.0, -// noise_model); -// std::vector components = {still, moving}; -// auto dcFactor = boost::make_shared>( -// contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); -// nhfg.push_back(dcFactor); + KeyVector contKeys = {X(0), X(1)}; + auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); + auto still = boost::make_shared(X(0), X(1), 0.0, noise_model), + moving = boost::make_shared(X(0), X(1), 1.0, noise_model); -// Values linearizationPoint; -// linearizationPoint.insert(X(0), 0); -// linearizationPoint.insert(X(1), 1); + // TODO(Varun) This is declared as NonlinearFactor instead of MotionModel, aka + // not clear!! + std::vector components = {still, moving}; + auto dcFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + nhfg.push_back(dcFactor); -// // Generate `GaussianHybridFactorGraph` by linearizing -// GaussianHybridFactorGraph fg = nhfg.linearize(linearizationPoint); + Values linearizationPoint; + linearizationPoint.insert(X(0), 0); + linearizationPoint.insert(X(1), 1); -// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 1); -// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 1); -// EXPECT_LONGS_EQUAL(fg.gaussianGraph().size(), 1); + // Generate `GaussianHybridFactorGraph` by linearizing + GaussianHybridFactorGraph gfg = nhfg.linearize(linearizationPoint); -// EXPECT_LONGS_EQUAL(fg.size(), 3); + EXPECT_LONGS_EQUAL(gfg.size(), 3); -// fg.resize(0); -// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); -// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); -// EXPECT_LONGS_EQUAL(fg.gaussianGraph().size(), 0); + gfg.resize(0); + EXPECT_LONGS_EQUAL(gfg.size(), 0); +} -// EXPECT_LONGS_EQUAL(fg.size(), 0); -// } +/* +**************************************************************************** +* Test push_back on HFG makes the correct distinction. +*/ +TEST(HybridFactorGraph, PushBack) { + NonlinearHybridFactorGraph fg; -// /* -// **************************************************************************** -// * Test push_back on HFG makes the correct distinction. -// */ -// TEST(HybridFactorGraph, PushBack) { -// NonlinearHybridFactorGraph fg; + auto nonlinearFactor = boost::make_shared>(); + fg.push_back(nonlinearFactor); -// auto nonlinearFactor = boost::make_shared>(); -// fg.push_back(nonlinearFactor); + EXPECT_LONGS_EQUAL(fg.size(), 1); -// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); -// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); -// EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 1); + fg = NonlinearHybridFactorGraph(); -// fg = NonlinearHybridFactorGraph(); + auto discreteFactor = boost::make_shared(); + fg.push_back(discreteFactor); -// auto discreteFactor = boost::make_shared(); -// fg.push_back(discreteFactor); + EXPECT_LONGS_EQUAL(fg.size(), 1); -// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); -// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 1); -// EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 0); + fg = NonlinearHybridFactorGraph(); -// fg = NonlinearHybridFactorGraph(); + auto dcFactor = boost::make_shared(); + fg.push_back(dcFactor); -// auto dcFactor = boost::make_shared>(); -// fg.push_back(dcFactor); + EXPECT_LONGS_EQUAL(fg.size(), 1); -// EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 1); -// EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); -// EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 0); + // Now do the same with GaussianHybridFactorGraph + GaussianHybridFactorGraph ghfg; -// // Now do the same with GaussianHybridFactorGraph -// GaussianHybridFactorGraph ghfg; + auto gaussianFactor = boost::make_shared(); + ghfg.push_back(gaussianFactor); -// auto gaussianFactor = boost::make_shared(); -// ghfg.push_back(gaussianFactor); + EXPECT_LONGS_EQUAL(ghfg.size(), 1); -// EXPECT_LONGS_EQUAL(ghfg.dcGraph().size(), 0); -// EXPECT_LONGS_EQUAL(ghfg.discreteGraph().size(), 0); -// EXPECT_LONGS_EQUAL(ghfg.gaussianGraph().size(), 1); + ghfg = GaussianHybridFactorGraph(); + ghfg.push_back(discreteFactor); -// ghfg = GaussianHybridFactorGraph(); + EXPECT_LONGS_EQUAL(ghfg.size(), 1); -// ghfg.push_back(discreteFactor); + ghfg = GaussianHybridFactorGraph(); + ghfg.push_back(dcFactor); -// EXPECT_LONGS_EQUAL(ghfg.dcGraph().size(), 0); -// EXPECT_LONGS_EQUAL(ghfg.discreteGraph().size(), 1); -// EXPECT_LONGS_EQUAL(ghfg.gaussianGraph().size(), 0); - -// ghfg = GaussianHybridFactorGraph(); - -// ghfg.push_back(dcFactor); - -// EXPECT_LONGS_EQUAL(ghfg.dcGraph().size(), 1); -// EXPECT_LONGS_EQUAL(ghfg.discreteGraph().size(), 0); -// EXPECT_LONGS_EQUAL(ghfg.gaussianGraph().size(), 0); -// } + EXPECT_LONGS_EQUAL(ghfg.size(), 1); +} // /* // ****************************************************************************/