add nonlinear switching system tests

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
+
  * -------------------------------------------------------------------------- */
 
 /**

gtsam/hybrid/tests/Switching.h

@@ -18,12 +18,19 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
+#include <gtsam/hybrid/MixtureFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
+#include <vector>
+
 #pragma once
 
 using gtsam::symbol_shorthand::C;
@@ -31,8 +38,6 @@ using gtsam::symbol_shorthand::X;
 
 namespace gtsam {
 
-using MotionModel = BetweenFactor<double>;
-
 inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
     size_t n, std::function<Key(int)> keyFunc = X,
     std::function<Key(int)> dKeyFunc = C) {
@@ -87,4 +92,103 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
   return {new_order, levels};
 }
 
+/* ***************************************************************************
+ */
+using MotionModel = BetweenFactor<double>;
+// using MotionMixture = MixtureFactor<MotionModel>;
+
+// 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<PriorFactor<double>>(
+        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<NonlinearFactor::shared_ptr> components;
+      for (auto &&f : motion_models) {
+        components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
+      }
+      nonlinearFactorGraph.emplace_hybrid<MixtureFactor>(
+          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<PriorFactor<double>>(
+          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<double>(X(k), static_cast<double>(k));
+    }
+
+    linearizedFactorGraph = nonlinearFactorGraph.linearize(linearizationPoint);
+  }
+
+  // Create motion models for a given time step
+  static std::vector<MotionModel::shared_ptr> 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<MotionModel>(X(k), X(k + 1), 0.0, noise_model),
+         moving =
+             boost::make_shared<MotionModel>(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<DiscreteDistribution>(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<DiscreteConditional>(
+          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<DiscreteDistribution>(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<DiscreteConditional>(
+          modes[k + 1], parents, "1/2 3/2");
+      fg->push_discrete(conditional);
+    }
+  }
+};
+
 }  // namespace gtsam

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -66,9 +66,8 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
 }
 
 /***************************************************************************
+ * 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<BetweenFactor<double>>();
@@ -87,9 +86,9 @@ TEST(HybridNonlinearFactorGraph, Resize) {
 }
 
 /***************************************************************************
+ * 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<BetweenFactor<double>>(
@@ -123,8 +122,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
   EXPECT_LONGS_EQUAL(gfg.size(), 0);
 }
 
-/*
+/*****************************************************************************
-****************************************************************************
  * Test push_back on HFG makes the correct distinction.
  */
 TEST(HybridFactorGraph, PushBack) {
@@ -168,53 +166,44 @@ TEST(HybridFactorGraph, PushBack) {
   EXPECT_LONGS_EQUAL(ghfg.size(), 1);
 }
 
-// /*
+/****************************************************************************
-// ****************************************************************************/
+ * Test construction of switching-like hybrid factor graph.
-// // Test construction of switching-like hybrid factor graph.
+ */
-// TEST(HybridFactorGraph, Switching) {
+TEST(HybridFactorGraph, Switching) {
-//   Switching self(3);
+  Switching self(3);
 
-//   EXPECT_LONGS_EQUAL(8, self.nonlinearFactorGraph.size());
+  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.linearizedFactorGraph.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());
-// }
 
-// /*
+/****************************************************************************
-// ****************************************************************************/
+ * Test linearization on a switching-like hybrid factor graph.
-// // Test linearization on a switching-like hybrid factor graph.
+ */
-// TEST(HybridFactorGraph, Linearization) {
+TEST(HybridFactorGraph, Linearization) {
-//   Switching self(3);
+  Switching self(3);
 
-//   // Linearize here:
+  // Linearize here:
-//   HybridGaussianFactorGraph actualLinearized =
+  HybridGaussianFactorGraph actualLinearized =
-//       self.nonlinearFactorGraph.linearize(self.linearizationPoint);
+      self.nonlinearFactorGraph.linearize(self.linearizationPoint);
 
-//   EXPECT_LONGS_EQUAL(8, actualLinearized.size());
+  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());
-// }
 
-// /*
+/****************************************************************************
-// ****************************************************************************/
+ * Test elimination tree construction
-// // Test elimination tree construction
+ */
-// TEST(HybridFactorGraph, EliminationTree) {
+TEST(HybridFactorGraph, EliminationTree) {
-//   Switching self(3);
+  Switching self(3);
 
-//   // Create ordering.
+  // Create ordering.
-//   Ordering ordering;
+  Ordering ordering;
-//   for (size_t k = 1; k <= self.K; k++) ordering += X(k);
+  for (size_t k = 1; k <= self.K; k++) ordering += X(k);
 
-//   // Create elimination tree.
+  // Create elimination tree.
-//   HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
+  HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
-//   EXPECT_LONGS_EQUAL(1, etree.roots().size())
+  EXPECT_LONGS_EQUAL(1, etree.roots().size())
-// }
+}
 
 // /*
 // ****************************************************************************/