Merge pull request borglab#1883 from borglab/feature/hybridISAM

Some Hybrid iSAM cleanup

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -132,6 +132,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   explicit HybridGaussianFactorGraph(const CONTAINER& factors)
       : Base(factors) {}
 
+  /**
+   * Construct from an initializer lists of GaussianFactor shared pointers.
+   * Example:
+   *   HybridGaussianFactorGraph graph = { factor1, factor2, factor3 };
+   */
+  HybridGaussianFactorGraph(std::initializer_list<sharedFactor> factors)
+      : Base(factors) {}
+
   /**
    * Implicit copy/downcast constructor to override explicit template container
    * constructor. In BayesTree this is used for:

gtsam/hybrid/HybridGaussianISAM.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridGaussianISAM.h
+ * @file HybridGaussianISAM.cpp
  * @date March 31, 2022
  * @author Fan Jiang
  * @author Frank Dellaert

gtsam/hybrid/HybridNonlinearISAM.cpp

@@ -39,8 +39,8 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
   if (newFactors.size() > 0) {
     // Reorder and relinearize every reorderInterval updates
     if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
-      // TODO(Varun) Relinearization doesn't take into account pruning
-      reorder_relinearize();
+      // TODO(Varun) Re-linearization doesn't take into account pruning
+      reorderRelinearize();
       reorderCounter_ = 0;
     }
 
@@ -60,7 +60,7 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
 }
 
 /* ************************************************************************* */
-void HybridNonlinearISAM::reorder_relinearize() {
+void HybridNonlinearISAM::reorderRelinearize() {
   if (factors_.size() > 0) {
     // Obtain the new linearization point
     const Values newLinPoint = estimate();
@@ -69,7 +69,7 @@ void HybridNonlinearISAM::reorder_relinearize() {
 
     // Just recreate the whole BayesTree
     // TODO: allow for constrained ordering here
-    // TODO: decouple relinearization and reordering to avoid
+    // TODO: decouple re-linearization and reordering to avoid
     isam_.update(*factors_.linearize(newLinPoint), {}, {},
                  eliminationFunction_);
 

gtsam/hybrid/HybridNonlinearISAM.h

@@ -37,7 +37,7 @@ class GTSAM_EXPORT HybridNonlinearISAM {
   /// The discrete assignment
   DiscreteValues assignment_;
 
-  /** The original factors, used when relinearizing */
+  /** The original factors, used when re-linearizing */
   HybridNonlinearFactorGraph factors_;
 
   /** The reordering interval and counter */
@@ -52,8 +52,8 @@ class GTSAM_EXPORT HybridNonlinearISAM {
   /// @{
 
   /**
-   * Periodically reorder and relinearize
-   * @param reorderInterval is the number of updates between reorderings,
+   * Periodically reorder and re-linearize
+   * @param reorderInterval is the number of updates between re-orderings,
    *   0 never reorders (and is dangerous for memory consumption)
    *  1 (default) reorders every time, in worse case is batch every update
    *  typical values are 50 or 100
@@ -124,8 +124,8 @@ class GTSAM_EXPORT HybridNonlinearISAM {
               const std::optional<size_t>& maxNrLeaves = {},
               const std::optional<Ordering>& ordering = {});
 
-  /** Relinearization and reordering of variables */
-  void reorder_relinearize();
+  /** Re-linearization and reordering of variables */
+  void reorderRelinearize();
 
   /// @}
 };

gtsam/hybrid/tests/Switching.h

@@ -120,12 +120,21 @@ using MotionModel = BetweenFactor<double>;
 // Test fixture with switching network.
 /// ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
 struct Switching {
+ private:
+  HybridNonlinearFactorGraph nonlinearFactorGraph_;
+
+ public:
   size_t K;
   DiscreteKeys modes;
-  HybridNonlinearFactorGraph nonlinearFactorGraph;
+  HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain;
   HybridGaussianFactorGraph linearizedFactorGraph;
   Values linearizationPoint;
 
+  // Access the flat nonlinear factor graph.
+  const HybridNonlinearFactorGraph &nonlinearFactorGraph() const {
+    return nonlinearFactorGraph_;
+  }
+
   /**
    * @brief Create with given number of time steps.
    *
@@ -136,7 +145,7 @@ struct Switching {
    */
   Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1,
             std::vector<double> measurements = {},
-            std::string discrete_transition_prob = "1/2 3/2")
+            std::string transitionProbabilityTable = "1/2 3/2")
       : K(K) {
     using noiseModel::Isotropic;
 
@@ -155,32 +164,36 @@ struct Switching {
     // Create hybrid factor graph.
 
     // Add a prior ϕ(X(0)) on X(0).
-    nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
+    nonlinearFactorGraph_.emplace_shared<PriorFactor<double>>(
         X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
+    unaryFactors.push_back(nonlinearFactorGraph_.back());
 
     // Add "motion models" ϕ(X(k),X(k+1),M(k)).
     for (size_t k = 0; k < K - 1; k++) {
       auto motion_models = motionModels(k, between_sigma);
-      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(modes[k],
+      nonlinearFactorGraph_.emplace_shared<HybridNonlinearFactor>(modes[k],
                                                                  motion_models);
+      binaryFactors.push_back(nonlinearFactorGraph_.back());
     }
 
     // Add measurement factors ϕ(X(k);z_k).
     auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
     for (size_t k = 1; k < K; k++) {
-      nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
+      nonlinearFactorGraph_.emplace_shared<PriorFactor<double>>(
           X(k), measurements.at(k), measurement_noise);
+      unaryFactors.push_back(nonlinearFactorGraph_.back());
     }
 
     // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2))
-    addModeChain(&nonlinearFactorGraph, discrete_transition_prob);
+    modeChain = createModeChain(transitionProbabilityTable);
+    nonlinearFactorGraph_ += modeChain;
 
     // Create the linearization point.
     for (size_t k = 0; k < K; k++) {
       linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
     }
 
-    linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint);
+    linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint);
   }
 
   // Create motion models for a given time step
@@ -200,15 +213,16 @@ struct Switching {
    *
    * @param fg The factor graph to which the mode chain is added.
    */
-  template <typename FACTORGRAPH>
-  void addModeChain(FACTORGRAPH *fg,
-                    std::string discrete_transition_prob = "1/2 3/2") {
-    fg->template emplace_shared<DiscreteDistribution>(modes[0], "1/1");
+  HybridNonlinearFactorGraph createModeChain(
+      std::string transitionProbabilityTable = "1/2 3/2") {
+    HybridNonlinearFactorGraph chain;
+    chain.emplace_shared<DiscreteDistribution>(modes[0], "1/1");
     for (size_t k = 0; k < K - 2; k++) {
       auto parents = {modes[k]};
-      fg->template emplace_shared<DiscreteConditional>(
-          modes[k + 1], parents, discrete_transition_prob);
+      chain.emplace_shared<DiscreteConditional>(modes[k + 1], parents,
+                                                transitionProbabilityTable);
     }
+    return chain;
   }
 };
 

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -37,6 +37,8 @@
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
 
+#include <string>
+
 #include "Switching.h"
 
 using namespace std;
@@ -55,13 +57,16 @@ std::vector<size_t> discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0,
 Switching InitializeEstimationProblem(
     const size_t K, const double between_sigma, const double measurement_sigma,
     const std::vector<double>& measurements,
-    const std::string& discrete_transition_prob,
+    const std::string& transitionProbabilityTable,
     HybridNonlinearFactorGraph& graph, Values& initial) {
   Switching switching(K, between_sigma, measurement_sigma, measurements,
-                      discrete_transition_prob);
+                      transitionProbabilityTable);
+
+  // Add prior on M(0)
+  graph.push_back(switching.modeChain.at(0));
 
   // Add the X(0) prior
-  graph.push_back(switching.nonlinearFactorGraph.at(0));
+  graph.push_back(switching.unaryFactors.at(0));
   initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
 
   return switching;
@@ -128,10 +133,9 @@ TEST(HybridEstimation, IncrementalSmoother) {
 
   constexpr size_t maxNrLeaves = 3;
   for (size_t k = 1; k < K; k++) {
-    // Motion Model
-    graph.push_back(switching.nonlinearFactorGraph.at(k));
-    // Measurement
-    graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1));
+    if (k > 1) graph.push_back(switching.modeChain.at(k - 1));  // Mode chain
+    graph.push_back(switching.binaryFactors.at(k - 1));         // Motion Model
+    graph.push_back(switching.unaryFactors.at(k));              // Measurement
 
     initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
 
@@ -176,10 +180,9 @@ TEST(HybridEstimation, ValidPruningError) {
 
   constexpr size_t maxNrLeaves = 3;
   for (size_t k = 1; k < K; k++) {
-    // Motion Model
-    graph.push_back(switching.nonlinearFactorGraph.at(k));
-    // Measurement
-    graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1));
+    if (k > 1) graph.push_back(switching.modeChain.at(k - 1));  // Mode chain
+    graph.push_back(switching.binaryFactors.at(k - 1));         // Motion Model
+    graph.push_back(switching.unaryFactors.at(k));              // Measurement
 
     initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
 
@@ -225,15 +228,17 @@ TEST(HybridEstimation, ISAM) {
 
   HybridGaussianFactorGraph linearized;
 
+  const size_t maxNrLeaves = 3;
   for (size_t k = 1; k < K; k++) {
-    // Motion Model
-    graph.push_back(switching.nonlinearFactorGraph.at(k));
-    // Measurement
-    graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1));
+    if (k > 1) graph.push_back(switching.modeChain.at(k - 1));  // Mode chain
+    graph.push_back(switching.binaryFactors.at(k - 1));         // Motion Model
+    graph.push_back(switching.unaryFactors.at(k));              // Measurement
 
     initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
 
-    isam.update(graph, initial, 3);
+    isam.update(graph, initial, maxNrLeaves);
+    // isam.saveGraph("NLiSAM" + std::to_string(k) + ".dot");
+    // GTSAM_PRINT(isam);
 
     graph.resize(0);
     initial.clear();
@@ -339,12 +344,8 @@ TEST(HybridEstimation, Probability) {
   HybridValues hybrid_values = bayesNet->optimize();
 
   // This is the correct sequence as designed
-  DiscreteValues discrete_seq;
-  discrete_seq[M(0)] = 1;
-  discrete_seq[M(1)] = 1;
-  discrete_seq[M(2)] = 0;
-
-  EXPECT(assert_equal(discrete_seq, hybrid_values.discrete()));
+  DiscreteValues expectedSequence{{M(0), 1}, {M(1), 1}, {M(2), 0}};
+  EXPECT(assert_equal(expectedSequence, hybrid_values.discrete()));
 }
 
 /****************************************************************************/
@@ -411,12 +412,8 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
   HybridValues hybrid_values = discreteBayesTree->optimize();
 
   // This is the correct sequence as designed
-  DiscreteValues discrete_seq;
-  discrete_seq[M(0)] = 1;
-  discrete_seq[M(1)] = 1;
-  discrete_seq[M(2)] = 0;
-
-  EXPECT(assert_equal(discrete_seq, hybrid_values.discrete()));
+  DiscreteValues expectedSequence{{M(0), 1}, {M(1), 1}, {M(2), 0}};
+  EXPECT(assert_equal(expectedSequence, hybrid_values.discrete()));
 }
 
 /*********************************************************************************