From a18857a117e5af9038d4bae1ebd91c2bc77b7270 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 6 Feb 2025 16:32:24 -0500 Subject: [PATCH] major refactor to add Experiment class --- examples/ISAM2_City10000.cpp | 278 ++++++++++++++++------------------- 1 file changed, 130 insertions(+), 148 deletions(-) diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index a1ec97d50..7f159cf45 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -38,181 +38,163 @@ using namespace boost::algorithm; using symbol_shorthand::X; -// Testing params -const size_t max_loop_count = 2000; // 200 //2000 //8000 +// Experiment Class +class Experiment { + /// The City10000 dataset + City10000Dataset dataset_; -const bool is_with_ambiguity = false; // run original iSAM2 without ambiguities -// const bool is_with_ambiguity = true; // run original iSAM2 with ambiguities + public: + // Parameters with default values + size_t maxLoopCount = 2000; // 200 //2000 //8000 -noiseModel::Diagonal::shared_ptr 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( - (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); - -/** - * @brief Write the results of optimization to filename. - * - * @param results The Values object with the final results. - * @param num_poses The number of poses to write to the file. - * @param filename The file name to save the results to. - */ -void write_results(const Values& results, size_t num_poses, - const std::string& filename = "ISAM2_city10000.txt") { - ofstream outfile; - outfile.open(filename); - - for (size_t i = 0; i < num_poses; ++i) { - Pose2 out_pose = results.at(X(i)); - - outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() - << std::endl; - } - outfile.close(); - std::cout << "output written to " << filename << std::endl; -} - -/* ************************************************************************* */ -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"); - - size_t pose_count = 0; - size_t index = 0; - - std::list time_list; + // false: run original iSAM2 without ambiguities + // true: run original iSAM2 with ambiguities + const bool is_with_ambiguity = false; + private: ISAM2Params parameters; parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); parameters.relinearizeThreshold = 0.01; parameters.relinearizeSkip = 1; - ISAM2* isam2 = new ISAM2(parameters); - - NonlinearFactorGraph* graph = new NonlinearFactorGraph(); - - Values init_values; + ISAM2 isam2(parameters); + NonlinearFactorGraph graph; + Values initial_; Values results; - double x = 0.0; - double y = 0.0; - double rad = 0.0; + public: + /// Construct with filename of experiment to run + explicit Experiment(const std::string& filename) : dataset_(filename) {} - Pose2 prior_pose(x, y, rad); + /// @brief Run the main experiment with a given maxLoopCount. + void run() { + // Initialize local variables + size_t pose_count = 0, index = 0; - init_values.insert(X(0), prior_pose); - pose_count++; + std::list timeList; - graph->addPrior(X(0), prior_pose, prior_noise_model); + // Set up initial prior + Pose2 priorPose(0, 0, 0); + initial_.insert(X(0), priorPose); + graph.addPrior(X(0), priorPose, kPriorNoiseModel); + pose_count++; - isam2->update(*graph, init_values); - graph->resize(0); - init_values.clear(); - results = isam2->calculateBestEstimate(); + // Initial update + isam2.update(*graph, initial_); + graph.resize(0); + initial_.clear(); + results = isam2.calculateBestEstimate(); - //* - size_t key_s = 0; - size_t key_t = 0; + // Start main loop + size_t keyS = 0; + size_t keyT = 0; + clock_t start_time = clock(); - clock_t start_time = clock(); - string str; - while (getline(in, str) && index < max_loop_count) { - // cout << str << endl; - vector parts; - split(parts, str, is_any_of(" ")); + 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]); + keyS = stoi(parts[1]); + keyT = stoi(parts[3]); - int num_measurements = stoi(parts[5]); - vector 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); - } + int num_measurements = stoi(parts[5]); + vector 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); + } - Pose2 odom_pose; - if (is_with_ambiguity) { - // Get wrong intentionally - int id = index % num_measurements; - odom_pose = Pose2(pose_array[id]); - } else { - odom_pose = pose_array[0]; - } - - if (key_s == key_t - 1) { // new X(key) - init_values.insert(X(key_t), results.at(X(key_s)) * odom_pose); - graph->add(BetweenFactor(X(key_s), X(key_t), odom_pose, - pose_noise_model)); - pose_count++; - } else { // loop - int id = index % num_measurements; - if (is_with_ambiguity && id % 2 == 0) { - graph->add(BetweenFactor(X(key_s), X(key_t), odom_pose, - pose_noise_model)); + Pose2 odom_pose; + if (is_with_ambiguity) { + // Get wrong intentionally + int id = index % num_measurements; + odom_pose = Pose2(pose_array[id]); } else { - graph->add(BetweenFactor( - X(key_s), X(key_t), odom_pose, - noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0))); + odom_pose = pose_array[0]; } - index++; - } - isam2->update(*graph, init_values); - graph->resize(0); - init_values.clear(); - results = isam2->calculateBestEstimate(); - - // Print loop index and time taken in processor clock ticks - if (index % 50 == 0 && key_s != key_t - 1) { - std::cout << "index: " << index << std::endl; - std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC - << std::endl; - } - - if (key_s == key_t - 1) { - clock_t cur_time = clock(); - time_list.push_back(cur_time - start_time); - } - - if (time_list.size() % 100 == 0 && (key_s == key_t - 1)) { - string step_file_idx = std::to_string(100000 + time_list.size()); - - ofstream step_outfile; - string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx; - step_outfile.open(step_file_name + ".txt"); - for (size_t i = 0; i < (key_t + 1); ++i) { - Pose2 out_pose = results.at(X(i)); - step_outfile << out_pose.x() << " " << out_pose.y() << " " - << out_pose.theta() << endl; + if (keyS == keyT - 1) { // new X(key) + initial_.insert(X(keyT), results.at(X(keyS)) * odom_pose); + graph->add( + BetweenFactor(X(keyS), X(keyT), odom_pose, kPoseNoiseModel)); + pose_count++; + } else { // loop + int id = index % num_measurements; + if (is_with_ambiguity && id % 2 == 0) { + graph->add(BetweenFactor(X(keyS), X(keyT), odom_pose, + kPoseNoiseModel)); + } else { + graph->add(BetweenFactor( + X(keyS), X(keyT), odom_pose, + noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0))); + } + index++; + } + + isam2->update(*graph, initial_); + graph->resize(0); + initial_.clear(); + results = isam2->calculateBestEstimate(); + + // Print loop index and time taken in processor clock ticks + if (index % 50 == 0 && keyS != keyT - 1) { + std::cout << "index: " << index << std::endl; + std::cout << "acc_time: " << timeList.back() / CLOCKS_PER_SEC + << std::endl; + } + + if (keyS == keyT - 1) { + clock_t cur_time = clock(); + timeList.push_back(cur_time - start_time); + } + + if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) { + string step_file_idx = std::to_string(100000 + timeList.size()); + + ofstream step_outfile; + string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx; + step_outfile.open(step_file_name + ".txt"); + for (size_t i = 0; i < (keyT + 1); ++i) { + Pose2 out_pose = results.at(X(i)); + step_outfile << out_pose.x() << " " << out_pose.y() << " " + << out_pose.theta() << endl; + } + step_outfile.close(); } - step_outfile.close(); } + + clock_t end_time = clock(); + clock_t total_time = end_time - start_time; + cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; + + /// Write results to file + writeResult(results, (keyT + 1), "ISAM2_city10000.txt"); + + ofstream outfile_time; + std::string time_file_name = "ISAM2_city10000_time.txt"; + outfile_time.open(time_file_name); + for (auto acc_time : timeList) { + outfile_time << acc_time << std::endl; + } + outfile_time.close(); + cout << "Written cumulative time to: " << time_file_name << " file." + << endl; } +}; - clock_t end_time = clock(); - clock_t total_time = end_time - start_time; - cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; +/* ************************************************************************* */ +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 - /// Write results to file - write_results(results, (key_t + 1)); - - ofstream outfile_time; - std::string time_file_name = "ISAM2_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 << "Written cumulative time to: " << time_file_name << " file." << endl; + // Run the experiment + experiment.run(); return 0; }