From b7b0f5b57a160351e6f6f7758bba246e2f60d40a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 28 Jan 2025 21:42:11 -0500 Subject: [PATCH] Run method and style --- examples/Hybrid_City10000.cpp | 285 ++++++++++++++++++---------------- 1 file changed, 151 insertions(+), 134 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 667b6f6f4..b704b7e0d 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -43,16 +43,23 @@ using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::X; -// Testing params -const size_t max_loop_count = 3000; // 2000; // 200 //2000 //8000 +const size_t kMaxLoopCount = 3000; // Example default value -auto prior_noise_model = noiseModel::Diagonal::Sigmas( +auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); -auto pose_noise_model = noiseModel::Diagonal::Sigmas( +auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas( (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); +// Experiment Class class Experiment { + private: + std::string filename_; + HybridSmoother smoother_; + HybridNonlinearFactorGraph graph_; + Values initial_; + Values result_; + /** * @brief Write the result of optimization to file. * @@ -60,61 +67,60 @@ class Experiment { * @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") { + 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 < num_poses; ++i) { - Pose2 out_pose = result.at(X(i)); - - outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() + for (size_t i = 0; i < numPoses; ++i) { + Pose2 outPose = result.at(X(i)); + outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta() << std::endl; } outfile.close(); - std::cout << "output written to " << filename << std::endl; + 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. */ - HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, - size_t key_s, size_t key_t, + HybridNonlinearFactor hybridLoopClosureFactor(size_t loopCounter, size_t keyS, + size_t keyT, const Pose2& measurement) { - DiscreteKey l(L(loop_counter), 2); + DiscreteKey l(L(loopCounter), 2); auto f0 = std::make_shared>( - X(key_s), X(key_t), measurement, + X(keyS), X(keyT), measurement, noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100)); auto f1 = std::make_shared>( - X(key_s), X(key_t), measurement, pose_noise_model); + X(keyS), X(keyT), measurement, kPoseNoiseModel); + std::vector factors{{f0, 0.0}, {f1, 0.0}}; - HybridNonlinearFactor mixtureFactor(l, {f0, f1}); + HybridNonlinearFactor mixtureFactor(l, factors); return mixtureFactor; } - // 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& pose_array, - const SharedNoiseModel& pose_noise_model) { + /// @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& poseArray, + const SharedNoiseModel& poseNoiseModel) { auto f0 = std::make_shared>( - X(key_s), X(key_t), pose_array[0], pose_noise_model); + X(keyS), X(keyT), poseArray[0], poseNoiseModel); auto f1 = std::make_shared>( - X(key_s), X(key_t), pose_array[1], pose_noise_model); + X(keyS), X(keyT), poseArray[1], poseNoiseModel); + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridNonlinearFactor mixtureFactor(m, factors); - // HybridNonlinearFactor mixtureFactor(m, {f0, f1}); return mixtureFactor; } - // Perform smoother update and optimize the graph - void SmootherUpdate(HybridSmoother& smoother, + /// @brief 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(); @@ -123,141 +129,146 @@ class Experiment { 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; + explicit Experiment(const std::string& filename) + : filename_(filename), smoother_(0.99) {} - std::list time_list; + /// @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; + } - HybridSmoother smoother(0.99); + // Initialize local variables + size_t discreteCount = 0, index = 0; + size_t loopCount = 0; + size_t nrHybridFactors = 0; // for demonstration; never incremented below - HybridNonlinearFactorGraph graph; - - Values initial, result; + std::list timeList; + // We'll reuse the smoother_, graph_, initial_, result_ from the class size_t maxNrHypotheses = 3; + // Set up initial prior double x = 0.0; double y = 0.0; double rad = 0.0; - Pose2 prior_pose(x, y, rad); + Pose2 priorPose(x, y, rad); + initial_.insert(X(0), priorPose); + graph_.push_back(PriorFactor(X(0), priorPose, kPriorNoiseModel)); - initial.insert(X(0), prior_pose); + // Initial update + clock_t beforeUpdate = clock(); + smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); + clock_t afterUpdate = clock(); + std::vector> smootherUpdateTimes; + smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); - graph.push_back(PriorFactor(X(0), prior_pose, prior_noise_model)); + // Start main loop + size_t keyS = 0, keyT = 0; + clock_t startTime = clock(); + std::string line; + while (getline(in, line) && index < maxLoopCount) { + std::vector parts; + split(parts, line, is_any_of(" ")); - std::vector> smoother_update_times; + keyS = stoi(parts[1]); + keyT = stoi(parts[3]); - 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 parts; - split(parts, str, is_any_of(" ")); - - key_s = stoi(parts[1]); - key_t = stoi(parts[3]); - - int num_measurements = stoi(parts[5]); - vector pose_array(num_measurements); - for (int i = 0; i < num_measurements; ++i) { + int numMeasurements = stoi(parts[5]); + std::vector 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]); - pose_array[i] = Pose2(x, y, rad); + poseArray[i] = Pose2(x, y, rad); } - // Flag if we should run smoother update - bool smoother_update = false; + // Flag to decide whether to run smoother update + bool doSmootherUpdate = false; // Take the first one as the initial estimate - Pose2 odom_pose = pose_array[0]; - if (key_s == key_t - 1) { // odometry - - if (num_measurements > 1) { - DiscreteKey m(M(discrete_count), num_measurements); - - // 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); - - discrete_count++; - - smoother_update = true; - + 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); + graph_.push_back(mixtureFactor); + discreteCount++; + doSmootherUpdate = true; } else { - graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, - pose_noise_model)); + graph_.add(BetweenFactor(X(keyS), X(keyT), odomPose, + kPoseNoiseModel)); } - - initial.insert(X(key_t), initial.at(X(key_s)) * odom_pose); - - } else { // loop - HybridNonlinearFactor loop_factor = - HybridLoopClosureFactor(loop_count, key_s, key_t, odom_pose); - graph.add(loop_factor); - - smoother_update = true; - - loop_count++; + // Insert next pose initial guess + initial_.insert(X(keyT), initial_.at(X(keyS)) * odomPose); + } else { + // Loop closure + HybridNonlinearFactor loopFactor = + hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose); + graph_.add(loopFactor); + doSmootherUpdate = true; + loopCount++; } - if (smoother_update) { + if (doSmootherUpdate) { gttic_(SmootherUpdate); - before_update = clock(); - SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); - after_update = clock(); - smoother_update_times.push_back({index, after_update - before_update}); + beforeUpdate = clock(); + smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); + afterUpdate = clock(); + smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); gttoc_(SmootherUpdate); } - // Print loop index and time taken in processor clock ticks - // if (index % 50 == 0 && key_s != key_t - 1) { + // 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; - std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC - << std::endl; + 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_(); + } } - if (key_s == key_t - 1) { - clock_t cur_time = clock(); - time_list.push_back(cur_time - start_time); - } - - index += 1; + index++; } - before_update = clock(); - SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); - after_update = clock(); - smoother_update_times.push_back({index, after_update - before_update}); + // Final update + beforeUpdate = clock(); + smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); + afterUpdate = clock(); + smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); + // Final optimize gttic_(HybridSmootherOptimize); - HybridValues delta = smoother.hybridBayesNet().optimize(); + HybridValues delta = smoother_.hybridBayesNet().optimize(); gttoc_(HybridSmootherOptimize); - result.insert_or_assign(initial.retract(delta.continuous())); + result_.insert_or_assign(initial_.retract(delta.continuous())); - std::cout << "Final error: " << smoother.hybridBayesNet().error(delta) + 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"); + 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++) { @@ -265,25 +276,31 @@ class Experiment { // 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; + + // 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; } - outfile_time.close(); - cout << "output " << time_file_name << " file." << endl; + outfileTime.close(); + std::cout << "Output " << timeFileName << " file." << std::endl; + + // Just to show usage of nrHybridFactors 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 +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 + + // Run the experiment + experiment.run(kMaxLoopCount); - // Experiment("../data/mh_All_city10000_groundtruth.txt"); return 0; -} +} \ No newline at end of file