Make class

examples/Hybrid_City10000.cpp

@@ -46,22 +46,21 @@ using symbol_shorthand::X;
 // Testing params
 const size_t max_loop_count = 3000;  // 2000;  // 200 //2000 //8000
 
-noiseModel::Diagonal::shared_ptr prior_noise_model =
-    noiseModel::Diagonal::Sigmas(
+auto prior_noise_model = noiseModel::Diagonal::Sigmas(
     (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
 
-noiseModel::Diagonal::shared_ptr pose_noise_model =
-    noiseModel::Diagonal::Sigmas(
+auto pose_noise_model = noiseModel::Diagonal::Sigmas(
     (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
 
-/**
- * @brief Write the result of optimization to filename.
+class Experiment {
+  /**
+   * @brief Write the result of optimization to file.
    *
    * @param result The Values object with the final result.
    * @param num_poses The number of poses to write to the file.
    * @param filename The file name to save the result to.
    */
-void write_result(const Values& result, size_t num_poses,
+  void write_result(const Values& result, size_t num_poses,
                     const std::string& filename = "Hybrid_city10000.txt") {
     ofstream outfile;
     outfile.open(filename);
@@ -74,20 +73,14 @@ void write_result(const Values& result, size_t num_poses,
     }
     outfile.close();
     std::cout << "output written to " << filename << std::endl;
-}
+  }
 
-/**
+  /**
    * @brief Create a hybrid loop closure factor where
    * 0 - loose noise model and 1 - loop noise model.
- *
- * @param loop_counter
- * @param key_s
- * @param key_t
- * @param measurement
- * @return HybridNonlinearFactor
    */
-HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s,
-                                              size_t key_t,
+  HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter,
+                                                size_t key_s, size_t key_t,
                                                 const Pose2& measurement) {
     DiscreteKey l(L(loop_counter), 2);
 
@@ -99,9 +92,10 @@ HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s,
     std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
     HybridNonlinearFactor mixtureFactor(l, {f0, f1});
     return mixtureFactor;
-}
+  }
 
-HybridNonlinearFactor HybridOdometryFactor(
+  // Create hybrid odometry factor with discrete measurement choices
+  HybridNonlinearFactor HybridOdometryFactor(
       size_t num_measurements, size_t key_s, size_t key_t, const DiscreteKey& m,
       const std::vector<Pose2>& pose_array,
       const SharedNoiseModel& pose_noise_model) {
@@ -113,28 +107,24 @@ HybridNonlinearFactor HybridOdometryFactor(
     HybridNonlinearFactor mixtureFactor(m, factors);
     // HybridNonlinearFactor mixtureFactor(m, {f0, f1});
     return mixtureFactor;
-}
+  }
 
-void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph,
-                    const Values& initial, size_t maxNrHypotheses,
-                    Values* result) {
+  // Perform smoother update and optimize the graph
+  void SmootherUpdate(HybridSmoother& smoother,
+                      HybridNonlinearFactorGraph& graph, const Values& initial,
+                      size_t maxNrHypotheses, Values* result) {
     HybridGaussianFactorGraph linearized = *graph.linearize(initial);
     // std::cout << "index: " << index << std::endl;
     smoother.update(linearized, maxNrHypotheses);
     graph.resize(0);
     HybridValues delta = smoother.hybridBayesNet().optimize();
     result->insert_or_assign(initial.retract(delta.continuous()));
-}
-
-/* ************************************************************************* */
-int main(int argc, char* argv[]) {
-  ifstream in(findExampleDataFile("T1_city10000_04.txt"));
-  // ifstream in("../data/mh_T1_city10000_04.txt"); //Type #1 only
-  // ifstream in("../data/mh_T3b_city10000_10.txt"); //Type #3 only
-  // ifstream in("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3
-
-  // ifstream in("../data/mh_All_city10000_groundtruth.txt");
+  }
 
+ public:
+  /// Construct with filename of experiment to run
+  Experiment(const std::string& filename) {
+    ifstream in(filename);
     size_t discrete_count = 0, index = 0;
     size_t loop_count = 0;
     size_t nrHybridFactors = 0;
@@ -269,10 +259,11 @@ int main(int argc, char* argv[]) {
     /// Write result to file
     write_result(result, (key_t + 1), "Hybrid_City10000.txt");
 
-  //TODO Write to file
+    // TODO Write to file
     //  for (size_t i = 0; i < smoother_update_times.size(); i++) {
     //    auto p = smoother_update_times.at(i);
-  //   std::cout << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
+    //    std::cout << p.first << ", " << p.second / CLOCKS_PER_SEC <<
+    //    std::endl;
     //  }
     ofstream outfile_time;
     std::string time_file_name = "Hybrid_City10000_time.txt";
@@ -283,6 +274,16 @@ int main(int argc, char* argv[]) {
     outfile_time.close();
     cout << "output " << time_file_name << " file." << endl;
     std::cout << nrHybridFactors << std::endl;
+  }
+};
 
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  Experiment(findExampleDataFile("T1_city10000_04.txt"));
+  // Experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only
+  // Experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only
+  // Experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3
+
+  // Experiment("../data/mh_All_city10000_groundtruth.txt");
   return 0;
 }