Merge pull request #2073 from borglab/hybrid-relinearize

Hybrid Fixes

examples/Hybrid_City10000.cpp

@@ -109,11 +109,10 @@ class Experiment {
     std::cout << "Smoother update: " << newFactors_.size() << std::endl;
     gttic_(SmootherUpdate);
     clock_t beforeUpdate = clock();
-    auto linearized = newFactors_.linearize(initial_);
-    smoother_.update(*linearized, initial_);
+    smoother_.update(newFactors_, initial_, maxNrHypotheses);
+    clock_t afterUpdate = clock();
     allFactors_.push_back(newFactors_);
     newFactors_.resize(0);
-    clock_t afterUpdate = clock();
     return afterUpdate - beforeUpdate;
   }
 
@@ -262,10 +261,20 @@ class Experiment {
     std::string timeFileName = "Hybrid_City10000_time.txt";
     outfileTime.open(timeFileName);
     for (auto accTime : timeList) {
-      outfileTime << accTime << std::endl;
+      outfileTime << accTime / CLOCKS_PER_SEC << std::endl;
     }
     outfileTime.close();
     std::cout << "Output " << timeFileName << " file." << std::endl;
+
+    std::ofstream timingFile;
+    std::string timingFileName = "Hybrid_City10000_timing.txt";
+    timingFile.open(timingFileName);
+    for (size_t i = 0; i < smootherUpdateTimes.size(); i++) {
+      auto p = smootherUpdateTimes.at(i);
+      timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
+    }
+    timingFile.close();
+    std::cout << "Wrote timing information to " << timingFileName << std::endl;
   }
 };
 

examples/ISAM2_City10000.cpp

@@ -74,6 +74,8 @@ class Experiment {
     // Initialize local variables
     size_t index = 0;
 
+    std::vector<std::pair<size_t, double>> smootherUpdateTimes;
+
     std::list<double> timeList;
 
     // Set up initial prior
@@ -82,10 +84,15 @@ class Experiment {
     graph_.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
 
     // Initial update
+    clock_t beforeUpdate = clock();
     isam2_.update(graph_, initial_);
+    results = isam2_.calculateBestEstimate();
+    clock_t afterUpdate = clock();
+    smootherUpdateTimes.push_back(
+        std::make_pair(index, afterUpdate - beforeUpdate));
     graph_.resize(0);
     initial_.clear();
-    results = isam2_.calculateBestEstimate();
+    index += 1;
 
     // Start main loop
     size_t keyS = 0;
@@ -127,10 +134,15 @@ class Experiment {
         index++;
       }
 
+      clock_t beforeUpdate = clock();
       isam2_.update(graph_, initial_);
+      results = isam2_.calculateBestEstimate();
+      clock_t afterUpdate = clock();
+      smootherUpdateTimes.push_back(
+          std::make_pair(index, afterUpdate - beforeUpdate));
       graph_.resize(0);
       initial_.clear();
-      results = isam2_.calculateBestEstimate();
+      index += 1;
 
       // Print loop index and time taken in processor clock ticks
       if (index % 50 == 0 && keyS != keyT - 1) {
@@ -175,6 +187,16 @@ class Experiment {
     outfileTime.close();
     std::cout << "Written cumulative time to: " << timeFileName << " file."
               << std::endl;
+
+    std::ofstream timingFile;
+    std::string timingFileName = "ISAM2_City10000_timing.txt";
+    timingFile.open(timingFileName);
+    for (size_t i = 0; i < smootherUpdateTimes.size(); i++) {
+      auto p = smootherUpdateTimes.at(i);
+      timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
+    }
+    timingFile.close();
+    std::cout << "Wrote timing information to " << timingFileName << std::endl;
   }
 };
 

gtsam/discrete/DecisionTreeFactor.cpp

@@ -547,7 +547,9 @@ namespace gtsam {
 /* ************************************************************************ */
 DiscreteFactor::shared_ptr DecisionTreeFactor::restrict(
     const DiscreteValues& assignment) const {
-  throw std::runtime_error("DecisionTreeFactor::restrict not implemented");
+  ADT restricted_tree = ADT::restrict(assignment);
+  return std::make_shared<DecisionTreeFactor>(this->discreteKeys(),
+                                              restricted_tree);
 }
 
   /* ************************************************************************ */

gtsam/hybrid/HybridSmoother.cpp

@@ -292,13 +292,19 @@ HybridValues HybridSmoother::optimize() const {
 }
 
 /* ************************************************************************* */
-void HybridSmoother::relinearize() {
+void HybridSmoother::relinearize(const std::optional<Ordering> givenOrdering) {
   allFactors_ = allFactors_.restrict(fixedValues_);
   HybridGaussianFactorGraph::shared_ptr linearized =
       allFactors_.linearize(linearizationPoint_);
-  HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential();
+
+  // Compute ordering if not given
+  Ordering ordering = this->maybeComputeOrdering(*linearized, givenOrdering);
+
+  HybridBayesNet::shared_ptr bayesNet =
+      linearized->eliminateSequential(ordering);
   HybridValues delta = bayesNet->optimize();
   linearizationPoint_ = linearizationPoint_.retract(delta.continuous());
+
   reInitialize(*bayesNet);
 }
 

gtsam/hybrid/HybridSmoother.h

@@ -126,9 +126,13 @@ class GTSAM_EXPORT HybridSmoother {
   /// Optimize the hybrid Bayes Net, taking into accound fixed values.
   HybridValues optimize() const;
 
-  /// Relinearize the nonlinear factor graph
-  /// with the latest linearization point.
-  void relinearize();
+  /**
+   * @brief Relinearize the nonlinear factor graph with
+   * the latest stored linearization point.
+   *
+   * @param givenOrdering An optional elimination ordering.
+   */
+  void relinearize(const std::optional<Ordering> givenOrdering = {});
 
   /// Return the current linearization point.
   Values linearizationPoint() const;

gtsam/hybrid/hybrid.i

@@ -288,7 +288,8 @@ class HybridSmoother {
       std::optional<size_t> maxNrLeaves = std::nullopt,
       const std::optional<gtsam::Ordering> given_ordering = std::nullopt);
 
-  void relinearize();
+  void relinearize(
+      const std::optional<gtsam::Ordering> givenOrdering = std::nullopt);
 
   gtsam::Values linearizationPoint() const;
   gtsam::HybridNonlinearFactorGraph allFactors() const;