Merge pull request #2016 from borglab/feature/hybrid_profiling

Lots of savings!

Author: dellaert

examples/Hybrid_City10000.cpp

@@ -31,31 +31,43 @@
 
 #include <boost/algorithm/string/classification.hpp>
 #include <boost/algorithm/string/split.hpp>
+#include <cstdlib>
 #include <fstream>
+#include <iostream>
 #include <string>
 #include <vector>
 
-using namespace std;
 using namespace gtsam;
 using namespace boost::algorithm;
 
 using symbol_shorthand::L;
 using symbol_shorthand::M;
 using symbol_shorthand::X;
 
-const size_t kMaxLoopCount = 2000;  // Example default value
-const size_t kMaxNrHypotheses = 10;
-
 auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10);
+const double kOpenLoopConstant = kOpenLoopModel->negLogConstant();
 
 auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
     (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
 
 auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas(
     (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
+const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant();
 
 // Experiment Class
 class Experiment {
+ public:
+  // Parameters with default values
+  size_t maxLoopCount = 3000;
+
+  // 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5: 39s} No DT optimizations
+  // 3000: {1: 65s, 2: 20s, 3: 16s, 4: 21s, 5: 28s} With DT optimizations
+  // 3000: {1: 59s, 2: 19s, 3: 18s, 4: 26s, 5: 33s} With DT optimizations +
+  // merge
+  size_t updateFrequency = 3;
+
+  size_t maxNrHypotheses = 10;
+
  private:
   std::string filename_;
   HybridSmoother smoother_;
@@ -72,7 +84,7 @@ class Experiment {
    */
   void writeResult(const Values& result, size_t numPoses,
                    const std::string& filename = "Hybrid_city10000.txt") {
-    ofstream outfile;
+    std::ofstream outfile;
     outfile.open(filename);
 
     for (size_t i = 0; i < numPoses; ++i) {
@@ -98,34 +110,33 @@ class Experiment {
     auto f1 = std::make_shared<BetweenFactor<Pose2>>(
         X(keyS), X(keyT), measurement, kPoseNoiseModel);
 
-    std::vector<NonlinearFactorValuePair> factors{
-        {f0, kOpenLoopModel->negLogConstant()},
-        {f1, kPoseNoiseModel->negLogConstant()}};
+    std::vector<NonlinearFactorValuePair> factors{{f0, kOpenLoopConstant},
+                                                  {f1, kPoseNoiseConstant}};
     HybridNonlinearFactor mixtureFactor(l, factors);
     return mixtureFactor;
   }
 
   /// @brief Create hybrid odometry factor with discrete measurement choices.
   HybridNonlinearFactor hybridOdometryFactor(
       size_t numMeasurements, size_t keyS, size_t keyT, const DiscreteKey& m,
-      const std::vector<Pose2>& poseArray,
-      const SharedNoiseModel& poseNoiseModel) {
+      const std::vector<Pose2>& poseArray) {
     auto f0 = std::make_shared<BetweenFactor<Pose2>>(
-        X(keyS), X(keyT), poseArray[0], poseNoiseModel);
+        X(keyS), X(keyT), poseArray[0], kPoseNoiseModel);
     auto f1 = std::make_shared<BetweenFactor<Pose2>>(
-        X(keyS), X(keyT), poseArray[1], poseNoiseModel);
+        X(keyS), X(keyT), poseArray[1], kPoseNoiseModel);
 
-    std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
+    std::vector<NonlinearFactorValuePair> factors{{f0, kPoseNoiseConstant},
+                                                  {f1, kPoseNoiseConstant}};
     HybridNonlinearFactor mixtureFactor(m, factors);
     return mixtureFactor;
   }
 
   /// @brief Perform smoother update and optimize the graph.
   void smootherUpdate(HybridSmoother& smoother,
                       HybridNonlinearFactorGraph& graph, const Values& initial,
-                      size_t kMaxNrHypotheses, Values* result) {
+                      size_t maxNrHypotheses, Values* result) {
     HybridGaussianFactorGraph linearized = *graph.linearize(initial);
-    smoother.update(linearized, kMaxNrHypotheses);
+    smoother.update(linearized, maxNrHypotheses);
     // throw if x0 not in hybridBayesNet_:
     const KeySet& keys = smoother.hybridBayesNet().keys();
     if (keys.find(X(0)) == keys.end()) {
@@ -142,11 +153,11 @@ class Experiment {
       : filename_(filename), smoother_(0.99) {}
 
   /// @brief Run the main experiment with a given maxLoopCount.
-  void run(size_t maxLoopCount) {
+  void run() {
     // Prepare reading
-    ifstream in(filename_);
+    std::ifstream in(filename_);
     if (!in.is_open()) {
-      cerr << "Failed to open file: " << filename_ << endl;
+      std::cerr << "Failed to open file: " << filename_ << std::endl;
       return;
     }
 
@@ -167,11 +178,14 @@ class Experiment {
 
     // Initial update
     clock_t beforeUpdate = clock();
-    smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_);
+    smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
     clock_t afterUpdate = clock();
     std::vector<std::pair<size_t, double>> smootherUpdateTimes;
     smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
 
+    // Flag to decide whether to run smoother update
+    size_t numberOfHybridFactors = 0;
+
     // Start main loop
     size_t keyS = 0, keyT = 0;
     clock_t startTime = clock();
@@ -192,21 +206,18 @@ class Experiment {
         poseArray[i] = Pose2(x, y, rad);
       }
 
-      // Flag to decide whether to run smoother update
-      bool doSmootherUpdate = false;
-
       // Take the first one as the initial estimate
       Pose2 odomPose = poseArray[0];
       if (keyS == keyT - 1) {
         // Odometry factor
         if (numMeasurements > 1) {
           // Add hybrid factor
           DiscreteKey m(M(discreteCount), numMeasurements);
-          HybridNonlinearFactor mixtureFactor = hybridOdometryFactor(
-              numMeasurements, keyS, keyT, m, poseArray, kPoseNoiseModel);
+          HybridNonlinearFactor mixtureFactor =
+              hybridOdometryFactor(numMeasurements, keyS, keyT, m, poseArray);
           graph_.push_back(mixtureFactor);
           discreteCount++;
-          doSmootherUpdate = true;
+          numberOfHybridFactors += 1;
           std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl;
         } else {
           graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
@@ -221,18 +232,20 @@ class Experiment {
         // print loop closure event keys:
         std::cout << "Loop closure: " << keyS << " " << keyT << std::endl;
         graph_.add(loopFactor);
-        doSmootherUpdate = true;
+        numberOfHybridFactors += 1;
         loopCount++;
       }
 
-      if (doSmootherUpdate) {
+      if (numberOfHybridFactors >= updateFrequency) {
+        // print the keys involved in the smoother update
+        std::cout << "Smoother update: " << graph_.size() << std::endl;
         gttic_(SmootherUpdate);
         beforeUpdate = clock();
-        smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_);
+        smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
         afterUpdate = clock();
         smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
         gttoc_(SmootherUpdate);
-        doSmootherUpdate = false;
+        numberOfHybridFactors = 0;
       }
 
       // Record timing for odometry edges only
@@ -258,7 +271,7 @@ class Experiment {
 
     // Final update
     beforeUpdate = clock();
-    smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_);
+    smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_);
     afterUpdate = clock();
     smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
 
@@ -288,7 +301,7 @@ class Experiment {
     //  }
 
     // Write timing info to file
-    ofstream outfileTime;
+    std::ofstream outfileTime;
     std::string timeFileName = "Hybrid_City10000_time.txt";
     outfileTime.open(timeFileName);
     for (auto accTime : timeList) {
@@ -300,15 +313,47 @@ class Experiment {
 };
 
 /* ************************************************************************* */
-int main() {
+// Function to parse command-line arguments
+void parseArguments(int argc, char* argv[], size_t& maxLoopCount,
+                    size_t& updateFrequency, size_t& maxNrHypotheses) {
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg == "--max-loop-count" && i + 1 < argc) {
+      maxLoopCount = std::stoul(argv[++i]);
+    } else if (arg == "--update-frequency" && i + 1 < argc) {
+      updateFrequency = std::stoul(argv[++i]);
+    } else if (arg == "--max-nr-hypotheses" && i + 1 < argc) {
+      maxNrHypotheses = std::stoul(argv[++i]);
+    } else if (arg == "--help") {
+      std::cout << "Usage: " << argv[0] << " [options]\n"
+                << "Options:\n"
+                << "  --max-loop-count <value>       Set the maximum loop "
+                   "count (default: 3000)\n"
+                << "  --update-frequency <value>     Set the update frequency "
+                   "(default: 3)\n"
+                << "  --max-nr-hypotheses <value>    Set the maximum number of "
+                   "hypotheses (default: 10)\n"
+                << "  --help                         Show this help message\n";
+      std::exit(0);
+    }
+  }
+}
+
+/* ************************************************************************* */
+// Main function
+int main(int argc, char* argv[]) {
   Experiment experiment(findExampleDataFile("T1_city10000_04.txt"));
   // Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only
   // Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only
   // Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 +
   // Type #3
 
+  // Parse command-line arguments
+  parseArguments(argc, argv, experiment.maxLoopCount,
+                 experiment.updateFrequency, experiment.maxNrHypotheses);
+
   // Run the experiment
-  experiment.run(kMaxLoopCount);
+  experiment.run();
 
   return 0;
 }