From 8471c97b9f5f2bb5eca3d95e2a17afbe3d230f52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Jul 2022 20:10:29 -0400 Subject: [PATCH] add nonlinear switching system tests --- gtsam/hybrid/MixtureFactor.h | 8 +- gtsam/hybrid/tests/Switching.h | 108 +++++++++++++++++- .../tests/testHybridNonlinearFactorGraph.cpp | 87 ++++++-------- 3 files changed, 151 insertions(+), 52 deletions(-) diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index d2d1a8d74..ba449976a 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -1,6 +1,12 @@ /* ---------------------------------------------------------------------------- - * Copyright 2020 The Ambitious Folks of the MRG + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ /** diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 4b1c27637..2707ae382 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,12 +18,19 @@ #include #include +#include #include #include +#include +#include #include #include +#include +#include #include +#include + #pragma once using gtsam::symbol_shorthand::C; @@ -31,8 +38,6 @@ using gtsam::symbol_shorthand::X; namespace gtsam { -using MotionModel = BetweenFactor; - inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { @@ -87,4 +92,103 @@ inline std::pair> makeBinaryOrdering( return {new_order, levels}; } +/* *************************************************************************** + */ +using MotionModel = BetweenFactor; +// using MotionMixture = MixtureFactor; + +// Test fixture with switching network. +struct Switching { + size_t K; + DiscreteKeys modes; + HybridNonlinearFactorGraph nonlinearFactorGraph; + HybridGaussianFactorGraph linearizedFactorGraph; + Values linearizationPoint; + + /// Create with given number of time steps. + Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1) + : K(K) { + using symbol_shorthand::M; + using symbol_shorthand::X; + + // Create DiscreteKeys for binary K modes, modes[0] will not be used. + for (size_t k = 0; k <= K; k++) { + modes.emplace_back(M(k), 2); + } + + // Create hybrid factor graph. + // Add a prior on X(1). + auto prior = boost::make_shared>( + X(1), 0, noiseModel::Isotropic::Sigma(1, prior_sigma)); + nonlinearFactorGraph.push_nonlinear(prior); + + // Add "motion models". + for (size_t k = 1; k < K; k++) { + KeyVector keys = {X(k), X(k + 1)}; + auto motion_models = motionModels(k); + std::vector components; + for (auto &&f : motion_models) { + components.push_back(boost::dynamic_pointer_cast(f)); + } + nonlinearFactorGraph.emplace_hybrid( + keys, DiscreteKeys{modes[k]}, components); + } + + // Add measurement factors + auto measurement_noise = noiseModel::Isotropic::Sigma(1, 0.1); + for (size_t k = 1; k <= K; k++) { + nonlinearFactorGraph.emplace_nonlinear>( + X(k), 1.0 * (k - 1), measurement_noise); + } + + // Add "mode chain" + addModeChain(&nonlinearFactorGraph); + + // Create the linearization point. + for (size_t k = 1; k <= K; k++) { + linearizationPoint.insert(X(k), static_cast(k)); + } + + linearizedFactorGraph = nonlinearFactorGraph.linearize(linearizationPoint); + } + + // Create motion models for a given time step + static std::vector motionModels(size_t k, + double sigma = 1.0) { + using symbol_shorthand::M; + using symbol_shorthand::X; + + auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); + auto still = + boost::make_shared(X(k), X(k + 1), 0.0, noise_model), + moving = + boost::make_shared(X(k), X(k + 1), 1.0, noise_model); + return {still, moving}; + } + + // Add "mode chain" to HybridNonlinearFactorGraph + void addModeChain(HybridNonlinearFactorGraph *fg) { + auto prior = boost::make_shared(modes[1], "1/1"); + fg->push_discrete(prior); + for (size_t k = 1; k < K - 1; k++) { + auto parents = {modes[k]}; + auto conditional = boost::make_shared( + modes[k + 1], parents, "1/2 3/2"); + fg->push_discrete(conditional); + } + } + + // Add "mode chain" to HybridGaussianFactorGraph + void addModeChain(HybridGaussianFactorGraph *fg) { + auto prior = boost::make_shared(modes[1], "1/1"); + fg->push_discrete(prior); + for (size_t k = 1; k < K - 1; k++) { + auto parents = {modes[k]}; + auto conditional = boost::make_shared( + modes[k + 1], parents, "1/2 3/2"); + fg->push_discrete(conditional); + } + } +}; + } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 6c5e94921..fb0778ed8 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -65,10 +65,9 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { EXPECT_LONGS_EQUAL(2, ghfg.size()); } -/* ************************************************************************** +/*************************************************************************** + * Test that the resize method works correctly for a HybridNonlinearFactorGraph. */ -/// Test that the resize method works correctly for a -/// HybridNonlinearFactorGraph. TEST(HybridNonlinearFactorGraph, Resize) { HybridNonlinearFactorGraph fg; auto nonlinearFactor = boost::make_shared>(); @@ -86,10 +85,10 @@ TEST(HybridNonlinearFactorGraph, Resize) { EXPECT_LONGS_EQUAL(fg.size(), 0); } -/* ************************************************************************** +/*************************************************************************** + * Test that the resize method works correctly for a + * HybridGaussianFactorGraph. */ -/// Test that the resize method works correctly for a -/// HybridGaussianFactorGraph. TEST(HybridGaussianFactorGraph, Resize) { HybridNonlinearFactorGraph nhfg; auto nonlinearFactor = boost::make_shared>( @@ -123,10 +122,9 @@ TEST(HybridGaussianFactorGraph, Resize) { EXPECT_LONGS_EQUAL(gfg.size(), 0); } -/* -**************************************************************************** -* Test push_back on HFG makes the correct distinction. -*/ +/***************************************************************************** + * Test push_back on HFG makes the correct distinction. + */ TEST(HybridFactorGraph, PushBack) { HybridNonlinearFactorGraph fg; @@ -168,53 +166,44 @@ TEST(HybridFactorGraph, PushBack) { EXPECT_LONGS_EQUAL(ghfg.size(), 1); } -// /* -// ****************************************************************************/ -// // Test construction of switching-like hybrid factor graph. -// TEST(HybridFactorGraph, Switching) { -// Switching self(3); +/**************************************************************************** + * Test construction of switching-like hybrid factor graph. + */ +TEST(HybridFactorGraph, Switching) { + Switching self(3); -// EXPECT_LONGS_EQUAL(8, self.nonlinearFactorGraph.size()); -// EXPECT_LONGS_EQUAL(4, self.nonlinearFactorGraph.nonlinearGraph().size()); -// EXPECT_LONGS_EQUAL(2, self.nonlinearFactorGraph.discreteGraph().size()); -// EXPECT_LONGS_EQUAL(2, self.nonlinearFactorGraph.dcGraph().size()); + EXPECT_LONGS_EQUAL(8, self.nonlinearFactorGraph.size()); -// EXPECT_LONGS_EQUAL(8, self.linearizedFactorGraph.size()); -// EXPECT_LONGS_EQUAL(2, self.linearizedFactorGraph.discreteGraph().size()); -// EXPECT_LONGS_EQUAL(2, self.linearizedFactorGraph.dcGraph().size()); -// EXPECT_LONGS_EQUAL(4, self.linearizedFactorGraph.gaussianGraph().size()); -// } + EXPECT_LONGS_EQUAL(8, self.linearizedFactorGraph.size()); +} -// /* -// ****************************************************************************/ -// // Test linearization on a switching-like hybrid factor graph. -// TEST(HybridFactorGraph, Linearization) { -// Switching self(3); +/**************************************************************************** + * Test linearization on a switching-like hybrid factor graph. + */ +TEST(HybridFactorGraph, Linearization) { + Switching self(3); -// // Linearize here: -// HybridGaussianFactorGraph actualLinearized = -// self.nonlinearFactorGraph.linearize(self.linearizationPoint); + // Linearize here: + HybridGaussianFactorGraph actualLinearized = + self.nonlinearFactorGraph.linearize(self.linearizationPoint); -// EXPECT_LONGS_EQUAL(8, actualLinearized.size()); -// EXPECT_LONGS_EQUAL(2, actualLinearized.discreteGraph().size()); -// EXPECT_LONGS_EQUAL(2, actualLinearized.dcGraph().size()); -// EXPECT_LONGS_EQUAL(4, actualLinearized.gaussianGraph().size()); -// } + EXPECT_LONGS_EQUAL(8, actualLinearized.size()); +} -// /* -// ****************************************************************************/ -// // Test elimination tree construction -// TEST(HybridFactorGraph, EliminationTree) { -// Switching self(3); +/**************************************************************************** + * Test elimination tree construction + */ +TEST(HybridFactorGraph, EliminationTree) { + Switching self(3); -// // Create ordering. -// Ordering ordering; -// for (size_t k = 1; k <= self.K; k++) ordering += X(k); + // Create ordering. + Ordering ordering; + for (size_t k = 1; k <= self.K; k++) ordering += X(k); -// // Create elimination tree. -// HybridEliminationTree etree(self.linearizedFactorGraph, ordering); -// EXPECT_LONGS_EQUAL(1, etree.roots().size()) -// } + // Create elimination tree. + HybridEliminationTree etree(self.linearizedFactorGraph, ordering); + EXPECT_LONGS_EQUAL(1, etree.roots().size()) +} // /* // ****************************************************************************/