diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 6df1086d6..de5d34bf3 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -40,7 +40,7 @@ using namespace boost::algorithm; using symbol_shorthand::X; // Testing params -const size_t max_loop_count = 8000; // 200 //2000 //8000 +const size_t max_loop_count = 2000; // 200 //2000 //8000 const bool is_with_ambiguity = false; // run original iSAM2 without ambiguities // const bool is_with_ambiguity = true; // run original iSAM2 with ambiguities @@ -53,8 +53,29 @@ noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas( (Vector(3) << 1.0 / 50.0, 1.0 / 50.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 @@ -64,19 +85,18 @@ int main(int argc, char* argv[]) { // ifstream in("../data/mh_All_city10000_groundtruth.txt"); size_t pose_count = 0; - size_t loop_count = 0; + size_t index = 0; std::list time_list; ISAM2Params parameters; - parameters.optimizationParams = - gtsam::ISAM2GaussNewtonParams(0.0); //_wildfireThreshold = 0.001 + parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); parameters.relinearizeThreshold = 0.01; parameters.relinearizeSkip = 1; - ISAM2* isam2 = new ISAM2(parameters); //[2] MHISAM2 + ISAM2* isam2 = new ISAM2(parameters); NonlinearFactorGraph* graph = new NonlinearFactorGraph(); @@ -105,37 +125,37 @@ int main(int argc, char* argv[]) { clock_t start_time = clock(); string str; - while (getline(in, str) && loop_count < max_loop_count) { + while (getline(in, str) && index < max_loop_count) { // cout << str << endl; vector parts; split(parts, str, is_any_of(" ")); key_s = stoi(parts[1]); key_t = stoi(parts[3]); - int m_num = stoi(parts[5]); - vector x_arr(m_num); - vector y_arr(m_num); - vector rad_arr(m_num); - for (int i = 0; i < m_num; ++i) { - x_arr[i] = stod(parts[6 + 3 * i]); - y_arr[i] = stod(parts[7 + 3 * i]); - rad_arr[i] = stod(parts[8 + 3 * i]); + + 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 = loop_count % m_num; - odom_pose = Pose2(x_arr[id], y_arr[id], rad_arr[id]); + int id = index % num_measurements; + odom_pose = Pose2(pose_array[id]); } else { - odom_pose = Pose2(x_arr[0], y_arr[0], rad_arr[0]); + 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); pose_count++; } else { // loop - loop_count++; + index++; } graph->add( BetweenFactor(X(key_s), X(key_t), odom_pose, pose_noise_model)); @@ -146,8 +166,8 @@ int main(int argc, char* argv[]) { results = isam2->calculateBestEstimate(); //* - if (loop_count % 50 == 0 && key_s != key_t - 1) { - std::cout << "loop_count: " << loop_count << std::endl; + if (index % 50 == 0 && key_s != key_t - 1) { + std::cout << "index: " << index << std::endl; std::cout << "acc_time: " << time_list.back() << std::endl; } // */ @@ -161,8 +181,7 @@ int main(int argc, char* argv[]) { string step_file_idx = std::to_string(100000 + time_list.size()); ofstream step_outfile; - string step_file_name = - "step_files/ISAM2_TEST_city10000_S" + step_file_idx; + 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)); @@ -176,31 +195,18 @@ int main(int argc, char* argv[]) { clock_t end_time = clock(); clock_t total_time = end_time - start_time; cout << "total_time: " << total_time << endl; - //* - ofstream outfile; - string file_name = "ISAM2_TEST_city10000"; - outfile.open(file_name + ".txt"); + /// Write results to file + write_results(results, (key_t + 1)); - for (size_t i = 0; i < (key_t + 1); ++i) { - Pose2 out_pose = results.at(X(i)); - - outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() - << endl; - } - outfile.close(); - cout << "output " << file_name << ".txt file." << endl; - // */ - - //* ofstream outfile_time; - string time_file_name = "ISAM2_TEST_city10000_time"; - outfile_time.open(time_file_name + ".txt"); + 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 << endl; // WRONG FORMAT... JUST FOR SIMPLE SHOW + outfile_time << acc_time << std::endl; } outfile_time.close(); - cout << "output " << time_file_name << ".txt file." << endl; - // */ + cout << "output " << time_file_name << " file." << endl; + return 0; } diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp new file mode 100644 index 000000000..de5d34bf3 --- /dev/null +++ b/examples/ISAM2_City10000.cpp @@ -0,0 +1,212 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Hybrid_City10000.cpp + * @brief Example of using hybrid estimation + * with multiple odometry measurements. + * @author Varun Agrawal + * @date January 22, 2025 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::algorithm; + +using symbol_shorthand::X; + +// Testing params +const size_t max_loop_count = 2000; // 200 //2000 //8000 + +const bool is_with_ambiguity = false; // run original iSAM2 without ambiguities +// const bool is_with_ambiguity = true; // run original iSAM2 with ambiguities + +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 / 50.0, 1.0 / 50.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; + + 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; + Values results; + + double x = 0.0; + double y = 0.0; + double rad = 0.0; + + Pose2 prior_pose(x, y, rad); + + init_values.insert(X(0), prior_pose); + pose_count++; + + graph->add(PriorFactor(X(0), prior_pose, prior_noise_model)); + + isam2->update(*graph, init_values); + graph->resize(0); + init_values.clear(); + results = isam2->calculateBestEstimate(); + + //* + size_t key_s = 0; + size_t key_t = 0; + + 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(" ")); + + 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) { + 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); + pose_count++; + } else { // loop + index++; + } + graph->add( + BetweenFactor(X(key_s), X(key_t), odom_pose, pose_noise_model)); + + isam2->update(*graph, init_values); + graph->resize(0); + init_values.clear(); + results = isam2->calculateBestEstimate(); + + //* + if (index % 50 == 0 && key_s != key_t - 1) { + std::cout << "index: " << index << std::endl; + std::cout << "acc_time: " << time_list.back() << 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; + } + step_outfile.close(); + } + } + + clock_t end_time = clock(); + clock_t total_time = end_time - start_time; + cout << "total_time: " << total_time << endl; + + /// 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 << "output " << time_file_name << " file." << endl; + + return 0; +}