update hybrid City10000 example
							parent
							
								
									b6e3b18776
								
							
						
					
					
						commit
						39ee58a962
					
				|  | @ -43,247 +43,273 @@ using symbol_shorthand::L; | |||
| using symbol_shorthand::M; | ||||
| using symbol_shorthand::X; | ||||
| 
 | ||||
| // Testing params
 | ||||
| const size_t max_loop_count = 2000;  // 2000;  // 200 //2000 //8000
 | ||||
| const size_t kMaxLoopCount = 2000;  // Example default value
 | ||||
| const size_t kMaxNrHypotheses = 10; | ||||
| 
 | ||||
| noiseModel::Diagonal::shared_ptr prior_noise_model = | ||||
|     noiseModel::Diagonal::Sigmas( | ||||
|         (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); | ||||
| auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); | ||||
| 
 | ||||
| noiseModel::Diagonal::shared_ptr pose_noise_model = | ||||
|     noiseModel::Diagonal::Sigmas( | ||||
|         (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); | ||||
| auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( | ||||
|     (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); | ||||
| 
 | ||||
| /**
 | ||||
|  * @brief Write the result of optimization to filename. | ||||
|  * | ||||
|  * @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, | ||||
|                   const std::string& filename = "Hybrid_city10000.txt") { | ||||
|   ofstream outfile; | ||||
|   outfile.open(filename); | ||||
| auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas( | ||||
|     (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); | ||||
| 
 | ||||
|   for (size_t i = 0; i < num_poses; ++i) { | ||||
|     Pose2 out_pose = result.at<Pose2>(X(i)); | ||||
| // Experiment Class
 | ||||
| class Experiment { | ||||
|  private: | ||||
|   std::string filename_; | ||||
|   HybridSmoother smoother_; | ||||
|   HybridNonlinearFactorGraph graph_; | ||||
|   Values initial_; | ||||
|   Values result_; | ||||
| 
 | ||||
|     outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() | ||||
|             << std::endl; | ||||
|   /**
 | ||||
|    * @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 writeResult(const Values& result, size_t numPoses, | ||||
|                    const std::string& filename = "Hybrid_city10000.txt") { | ||||
|     ofstream outfile; | ||||
|     outfile.open(filename); | ||||
| 
 | ||||
|     for (size_t i = 0; i < numPoses; ++i) { | ||||
|       Pose2 outPose = result.at<Pose2>(X(i)); | ||||
|       outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta() | ||||
|               << std::endl; | ||||
|     } | ||||
|     outfile.close(); | ||||
|     std::cout << "Output written to " << filename << std::endl; | ||||
|   } | ||||
|   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, | ||||
|                                               const Pose2& measurement) { | ||||
|   DiscreteKey l(L(loop_counter), 2); | ||||
|   /**
 | ||||
|    * @brief Create a hybrid loop closure factor where | ||||
|    * 0 - loose noise model and 1 - loop noise model. | ||||
|    */ | ||||
|   HybridNonlinearFactor hybridLoopClosureFactor(size_t loopCounter, size_t keyS, | ||||
|                                                 size_t keyT, | ||||
|                                                 const Pose2& measurement) { | ||||
|     DiscreteKey l(L(loopCounter), 2); | ||||
| 
 | ||||
|   auto open_loop_model = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); | ||||
|   auto f0 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|       X(key_s), X(key_t), measurement, open_loop_model); | ||||
|   auto f1 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|       X(key_s), X(key_t), measurement, pose_noise_model); | ||||
|   std::vector<NonlinearFactorValuePair> factors{ | ||||
|       {f0, open_loop_model->negLogConstant()}, | ||||
|       {f1, pose_noise_model->negLogConstant()}}; | ||||
|   HybridNonlinearFactor mixtureFactor(l, {f0, f1}); | ||||
|   return mixtureFactor; | ||||
| } | ||||
|     auto f0 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|         X(keyS), X(keyT), measurement, kOpenLoopModel); | ||||
|     auto f1 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|         X(keyS), X(keyT), measurement, kPoseNoiseModel); | ||||
| 
 | ||||
| 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) { | ||||
|   auto f0 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|       X(key_s), X(key_t), pose_array[0], pose_noise_model); | ||||
|   auto f1 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|       X(key_s), X(key_t), pose_array[1], pose_noise_model); | ||||
|   std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}}; | ||||
|   HybridNonlinearFactor mixtureFactor(m, factors); | ||||
|   // HybridNonlinearFactor mixtureFactor(m, {f0, f1});
 | ||||
|   return mixtureFactor; | ||||
| } | ||||
|     std::vector<NonlinearFactorValuePair> factors{ | ||||
|         {f0, kOpenLoopModel->negLogConstant()}, | ||||
|         {f1, kPoseNoiseModel->negLogConstant()}}; | ||||
|     HybridNonlinearFactor mixtureFactor(l, factors); | ||||
|     return mixtureFactor; | ||||
|   } | ||||
| 
 | ||||
| void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, | ||||
|                     const Values& initial, size_t maxNrHypotheses, | ||||
|                     Values* result) { | ||||
|   HybridGaussianFactorGraph linearized = *graph.linearize(initial); | ||||
|   smoother.update(linearized, maxNrHypotheses); | ||||
|   graph.resize(0); | ||||
|   // HybridValues delta = smoother.hybridBayesNet().optimize();
 | ||||
|   // result->insert_or_assign(initial.retract(delta.continuous()));
 | ||||
| } | ||||
|   /// @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) { | ||||
|     auto f0 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|         X(keyS), X(keyT), poseArray[0], kPoseNoiseModel); | ||||
|     auto f1 = std::make_shared<BetweenFactor<Pose2>>( | ||||
|         X(keyS), X(keyT), poseArray[1], kPoseNoiseModel); | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 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
 | ||||
|     std::vector<NonlinearFactorValuePair> factors{ | ||||
|         {f0, kPoseNoiseModel->negLogConstant()}, | ||||
|         {f1, kPoseNoiseModel->negLogConstant()}}; | ||||
|     HybridNonlinearFactor mixtureFactor(m, factors); | ||||
|     return mixtureFactor; | ||||
|   } | ||||
| 
 | ||||
|   // ifstream in("../data/mh_All_city10000_groundtruth.txt");
 | ||||
|   /// @brief Perform smoother update and optimize the graph.
 | ||||
|   void smootherUpdate(HybridSmoother& smoother, | ||||
|                       HybridNonlinearFactorGraph& graph, const Values& initial, | ||||
|                       size_t kMaxNrHypotheses, Values* result) { | ||||
|     HybridGaussianFactorGraph linearized = *graph.linearize(initial); | ||||
|     smoother.update(linearized, kMaxNrHypotheses); | ||||
|     // throw if x0 not in hybridBayesNet_:
 | ||||
|     const KeySet& keys = smoother.hybridBayesNet().keys(); | ||||
|     if (keys.find(X(0)) == keys.end()) { | ||||
|       throw std::runtime_error("x0 not in hybridBayesNet_"); | ||||
|     } | ||||
|     graph.resize(0); | ||||
|     // HybridValues delta = smoother.hybridBayesNet().optimize();
 | ||||
|     // result->insert_or_assign(initial.retract(delta.continuous()));
 | ||||
|   } | ||||
| 
 | ||||
|   size_t discrete_count = 0, index = 0; | ||||
|   size_t loop_count = 0; | ||||
|   size_t nrHybridFactors = 0; | ||||
|  public: | ||||
|   /// Construct with filename of experiment to run
 | ||||
|   explicit Experiment(const std::string& filename) | ||||
|       : filename_(filename), smoother_(0.99) {} | ||||
| 
 | ||||
|   std::list<double> time_list; | ||||
| 
 | ||||
|   HybridSmoother smoother(0.99); | ||||
| 
 | ||||
|   HybridNonlinearFactorGraph graph; | ||||
| 
 | ||||
|   Values initial, result; | ||||
| 
 | ||||
|   size_t maxNrHypotheses = 3; | ||||
| 
 | ||||
|   double x = 0.0; | ||||
|   double y = 0.0; | ||||
|   double rad = 0.0; | ||||
| 
 | ||||
|   Pose2 prior_pose(x, y, rad); | ||||
| 
 | ||||
|   initial.insert(X(0), prior_pose); | ||||
| 
 | ||||
|   graph.push_back(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model)); | ||||
| 
 | ||||
|   std::vector<std::pair<size_t, double>> smoother_update_times; | ||||
| 
 | ||||
|   clock_t before_update = clock(); | ||||
|   SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); | ||||
|   clock_t after_update = clock(); | ||||
|   smoother_update_times.push_back({index, after_update - before_update}); | ||||
| 
 | ||||
|   size_t key_s, key_t{0}; | ||||
| 
 | ||||
|   clock_t start_time = clock(); | ||||
|   std::string str; | ||||
|   while (getline(in, str) && index < max_loop_count) { | ||||
|     vector<string> parts; | ||||
|     split(parts, str, is_any_of(" ")); | ||||
| 
 | ||||
|     key_s = stoi(parts[1]); | ||||
|     key_t = stoi(parts[3]); | ||||
| 
 | ||||
|     int num_measurements = stoi(parts[5]); | ||||
|     vector<Pose2> pose_array(num_measurements); | ||||
|     for (int i = 0; i < num_measurements; ++i) { | ||||
|       x = stod(parts[6 + 3 * i]); | ||||
|       y = stod(parts[7 + 3 * i]); | ||||
|       rad = stod(parts[8 + 3 * i]); | ||||
|       pose_array[i] = Pose2(x, y, rad); | ||||
|   /// @brief Run the main experiment with a given maxLoopCount.
 | ||||
|   void run(size_t maxLoopCount) { | ||||
|     // Prepare reading
 | ||||
|     ifstream in(filename_); | ||||
|     if (!in.is_open()) { | ||||
|       cerr << "Failed to open file: " << filename_ << endl; | ||||
|       return; | ||||
|     } | ||||
| 
 | ||||
|     // Flag if we should run smoother update
 | ||||
|     bool smoother_update = false; | ||||
|     // Initialize local variables
 | ||||
|     size_t discreteCount = 0, index = 0; | ||||
|     size_t loopCount = 0; | ||||
| 
 | ||||
|     // Take the first one as the initial estimate
 | ||||
|     Pose2 odom_pose = pose_array[0]; | ||||
|     if (key_s == key_t - 1) {  // odometry
 | ||||
|     std::list<double> timeList; | ||||
| 
 | ||||
|       if (num_measurements > 1) { | ||||
|         DiscreteKey m(M(discrete_count), num_measurements); | ||||
|     // Set up initial prior
 | ||||
|     double x = 0.0; | ||||
|     double y = 0.0; | ||||
|     double rad = 0.0; | ||||
| 
 | ||||
|         // Add hybrid factor which considers both measurements
 | ||||
|         HybridNonlinearFactor mixtureFactor = HybridOdometryFactor( | ||||
|             num_measurements, key_s, key_t, m, pose_array, pose_noise_model); | ||||
|         graph.push_back(mixtureFactor); | ||||
|     Pose2 priorPose(x, y, rad); | ||||
|     initial_.insert(X(0), priorPose); | ||||
|     graph_.push_back(PriorFactor<Pose2>(X(0), priorPose, kPriorNoiseModel)); | ||||
| 
 | ||||
|         discrete_count++; | ||||
|     // Initial update
 | ||||
|     clock_t beforeUpdate = clock(); | ||||
|     smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); | ||||
|     clock_t afterUpdate = clock(); | ||||
|     std::vector<std::pair<size_t, double>> smootherUpdateTimes; | ||||
|     smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); | ||||
| 
 | ||||
|         smoother_update = true; | ||||
|     // Start main loop
 | ||||
|     size_t keyS = 0, keyT = 0; | ||||
|     clock_t startTime = clock(); | ||||
|     std::string line; | ||||
|     while (getline(in, line) && index < maxLoopCount) { | ||||
|       std::vector<std::string> parts; | ||||
|       split(parts, line, is_any_of(" ")); | ||||
| 
 | ||||
|       } else { | ||||
|         graph.add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, | ||||
|                                        pose_noise_model)); | ||||
|       keyS = stoi(parts[1]); | ||||
|       keyT = stoi(parts[3]); | ||||
| 
 | ||||
|       int numMeasurements = stoi(parts[5]); | ||||
|       std::vector<Pose2> poseArray(numMeasurements); | ||||
|       for (int i = 0; i < numMeasurements; ++i) { | ||||
|         x = stod(parts[6 + 3 * i]); | ||||
|         y = stod(parts[7 + 3 * i]); | ||||
|         rad = stod(parts[8 + 3 * i]); | ||||
|         poseArray[i] = Pose2(x, y, rad); | ||||
|       } | ||||
| 
 | ||||
|       initial.insert(X(key_t), initial.at<Pose2>(X(key_s)) * odom_pose); | ||||
|       // Flag to decide whether to run smoother update
 | ||||
|       bool doSmootherUpdate = false; | ||||
| 
 | ||||
|     } else {  // loop
 | ||||
|       HybridNonlinearFactor loop_factor = | ||||
|           HybridLoopClosureFactor(loop_count, key_s, key_t, odom_pose); | ||||
|       graph.add(loop_factor); | ||||
|       // 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); | ||||
|           graph_.push_back(mixtureFactor); | ||||
|           discreteCount++; | ||||
|           doSmootherUpdate = true; | ||||
|           // std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl;
 | ||||
|         } else { | ||||
|           graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, | ||||
|                                           kPoseNoiseModel)); | ||||
|         } | ||||
|         // Insert next pose initial guess
 | ||||
|         initial_.insert(X(keyT), initial_.at<Pose2>(X(keyS)) * odomPose); | ||||
|       } else { | ||||
|         // Loop closure
 | ||||
|         HybridNonlinearFactor loopFactor = | ||||
|             hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose); | ||||
|         // print loop closure event keys:
 | ||||
|         // std::cout << "Loop closure: " << keyS << " " << keyT << std::endl;
 | ||||
|         graph_.add(loopFactor); | ||||
|         doSmootherUpdate = true; | ||||
|         loopCount++; | ||||
|       } | ||||
| 
 | ||||
|       smoother_update = true; | ||||
|       if (doSmootherUpdate) { | ||||
|         gttic_(SmootherUpdate); | ||||
|         beforeUpdate = clock(); | ||||
|         smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); | ||||
|         afterUpdate = clock(); | ||||
|         smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); | ||||
|         gttoc_(SmootherUpdate); | ||||
|         doSmootherUpdate = false; | ||||
|       } | ||||
| 
 | ||||
|       loop_count++; | ||||
|       // Record timing for odometry edges only
 | ||||
|       if (keyS == keyT - 1) { | ||||
|         clock_t curTime = clock(); | ||||
|         timeList.push_back(curTime - startTime); | ||||
|       } | ||||
| 
 | ||||
|       // Print some status every 100 steps
 | ||||
|       if (index % 100 == 0) { | ||||
|         std::cout << "Index: " << index << std::endl; | ||||
|         if (!timeList.empty()) { | ||||
|           std::cout << "Acc_time: " << timeList.back() / CLOCKS_PER_SEC | ||||
|                     << " seconds" << std::endl; | ||||
|           // delta.discrete().print("The Discrete Assignment");
 | ||||
|           tictoc_finishedIteration_(); | ||||
|           tictoc_print_(); | ||||
|         } | ||||
|       } | ||||
| 
 | ||||
|       index++; | ||||
|     } | ||||
| 
 | ||||
|     if (smoother_update) { | ||||
|       gttic_(SmootherUpdate); | ||||
|       before_update = clock(); | ||||
|       SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); | ||||
|       after_update = clock(); | ||||
|       smoother_update_times.push_back({index, after_update - before_update}); | ||||
|       gttoc_(SmootherUpdate); | ||||
|     } | ||||
|     // Final update
 | ||||
|     beforeUpdate = clock(); | ||||
|     smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); | ||||
|     afterUpdate = clock(); | ||||
|     smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); | ||||
| 
 | ||||
|     // Print loop index and time taken in processor clock ticks
 | ||||
|     // if (index % 50 == 0 && key_s != key_t - 1) {
 | ||||
|     if (index % 100 == 0) { | ||||
|       std::cout << "index: " << index << std::endl; | ||||
|       std::cout << "acc_time:  " << time_list.back() / CLOCKS_PER_SEC | ||||
|                 << std::endl; | ||||
|       // delta.discrete().print("The Discrete Assignment");
 | ||||
|       tictoc_finishedIteration_(); | ||||
|       tictoc_print_(); | ||||
|     } | ||||
|     // Final optimize
 | ||||
|     gttic_(HybridSmootherOptimize); | ||||
|     HybridValues delta = smoother_.optimize(); | ||||
|     gttoc_(HybridSmootherOptimize); | ||||
| 
 | ||||
|     if (key_s == key_t - 1) { | ||||
|       clock_t cur_time = clock(); | ||||
|       time_list.push_back(cur_time - start_time); | ||||
|     } | ||||
|     result_.insert_or_assign(initial_.retract(delta.continuous())); | ||||
| 
 | ||||
|     index += 1; | ||||
|     std::cout << "Final error: " << smoother_.hybridBayesNet().error(delta) | ||||
|               << std::endl; | ||||
| 
 | ||||
|     clock_t endTime = clock(); | ||||
|     clock_t totalTime = endTime - startTime; | ||||
|     std::cout << "Total time: " << totalTime / CLOCKS_PER_SEC << " seconds" | ||||
|               << std::endl; | ||||
| 
 | ||||
|     // Write results to file
 | ||||
|     writeResult(result_, keyT + 1, "Hybrid_City10000.txt"); | ||||
| 
 | ||||
|     // 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;
 | ||||
|     //  }
 | ||||
| 
 | ||||
|     // Write timing info to file
 | ||||
|     ofstream outfileTime; | ||||
|     std::string timeFileName = "Hybrid_City10000_time.txt"; | ||||
|     outfileTime.open(timeFileName); | ||||
|     for (auto accTime : timeList) { | ||||
|       outfileTime << accTime << std::endl; | ||||
|     } | ||||
|     outfileTime.close(); | ||||
|     std::cout << "Output " << timeFileName << " file." << std::endl; | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|   before_update = clock(); | ||||
|   SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); | ||||
|   after_update = clock(); | ||||
|   smoother_update_times.push_back({index, after_update - before_update}); | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   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
 | ||||
| 
 | ||||
|   gttic_(HybridSmootherOptimize); | ||||
|   HybridValues delta = smoother.hybridBayesNet().optimize(); | ||||
|   gttoc_(HybridSmootherOptimize); | ||||
|   result.insert_or_assign(initial.retract(delta.continuous())); | ||||
| 
 | ||||
|   std::cout << "Final error: " << smoother.hybridBayesNet().error(delta) | ||||
|             << std::endl; | ||||
|   clock_t end_time = clock(); | ||||
|   clock_t total_time = end_time - start_time; | ||||
|   cout << "total_time: " << total_time / CLOCKS_PER_SEC << " seconds" << endl; | ||||
| 
 | ||||
|   /// Write result to file
 | ||||
|   write_result(result, (key_t + 1), "Hybrid_City10000.txt"); | ||||
| 
 | ||||
|   // 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;
 | ||||
|   //  }
 | ||||
|   ofstream outfile_time; | ||||
|   std::string time_file_name = "Hybrid_City10000_time.txt"; | ||||
|   outfile_time.open(time_file_name); | ||||
|   for (auto acc_time : time_list) { | ||||
|     outfile_time << acc_time << std::endl; | ||||
|   } | ||||
|   outfile_time.close(); | ||||
|   cout << "output " << time_file_name << " file." << endl; | ||||
|   std::cout << nrHybridFactors << std::endl; | ||||
|   // Run the experiment
 | ||||
|   experiment.run(kMaxLoopCount); | ||||
| 
 | ||||
|   return 0; | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue