From ea82d51d101a8d1f688b08babc2c4f458252e00b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 23 Jan 2025 12:52:45 -0500 Subject: [PATCH] working example --- examples/Hybrid_City10000.cpp | 102 ++++++++++++++++++---------------- 1 file changed, 55 insertions(+), 47 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index de5d34bf3..f2050ea14 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -18,9 +18,11 @@ */ #include +#include +#include +#include +#include #include -#include -#include #include #include #include @@ -37,13 +39,11 @@ using namespace std; using namespace gtsam; using namespace boost::algorithm; +using symbol_shorthand::M; 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 +const size_t max_loop_count = 1000; // 2000; // 200 //2000 //8000 noiseModel::Diagonal::shared_ptr prior_noise_model = noiseModel::Diagonal::Sigmas( @@ -51,7 +51,7 @@ noiseModel::Diagonal::shared_ptr prior_noise_model = 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()); + (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); /** * @brief Write the results of optimization to filename. @@ -84,25 +84,20 @@ int main(int argc, char* argv[]) { // ifstream in("../data/mh_All_city10000_groundtruth.txt"); - size_t pose_count = 0; + size_t pose_count = 0, discrete_count = 0; size_t index = 0; std::list time_list; - ISAM2Params parameters; + HybridNonlinearISAM isam; - parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); - - parameters.relinearizeThreshold = 0.01; - - parameters.relinearizeSkip = 1; - ISAM2* isam2 = new ISAM2(parameters); - - NonlinearFactorGraph* graph = new NonlinearFactorGraph(); + HybridNonlinearFactorGraph graph; Values init_values; Values results; + size_t maxNrHypotheses = 3; + double x = 0.0; double y = 0.0; double rad = 0.0; @@ -110,23 +105,21 @@ int main(int argc, char* argv[]) { Pose2 prior_pose(x, y, rad); init_values.insert(X(0), prior_pose); - pose_count++; + pose_count += 1; - graph->add(PriorFactor(X(0), prior_pose, prior_noise_model)); + graph.push_back(PriorFactor(X(0), prior_pose, prior_noise_model)); - isam2->update(*graph, init_values); - graph->resize(0); + isam.update(graph, init_values, maxNrHypotheses); + + graph.resize(0); init_values.clear(); - results = isam2->calculateBestEstimate(); + results = isam.estimate(); - //* - size_t key_s = 0; - size_t key_t = 0; + size_t key_s, key_t; clock_t start_time = clock(); - string str; + std::string str; while (getline(in, str) && index < max_loop_count) { - // cout << str << endl; vector parts; split(parts, str, is_any_of(" ")); @@ -142,35 +135,47 @@ int main(int argc, char* argv[]) { 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]; - } - + // Take the first one as the initial estimate + Pose2 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++; + // index++; } - graph->add( - BetweenFactor(X(key_s), X(key_t), odom_pose, pose_noise_model)); - isam2->update(*graph, init_values); - graph->resize(0); + if (num_measurements == 2) { + // Add hybrid factor which considers both measurements + DiscreteKey m(M(discrete_count), num_measurements); + discrete_count++; + + graph.push_back(DecisionTreeFactor(m, "0.6 0.4")); + + auto f0 = std::make_shared>( + X(key_s), X(key_t), pose_array[0], pose_noise_model); + auto f1 = std::make_shared>( + X(key_s), X(key_t), pose_array[1], pose_noise_model); + std::vector factors{{f0, 0.0}, {f1, 0.0}}; + // HybridNonlinearFactor mixtureFactor(m, factors); + HybridNonlinearFactor mixtureFactor(m, {f0, f1}); + graph.push_back(mixtureFactor); + + } else { + graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, + pose_noise_model)); + } + + isam.update(graph, init_values, maxNrHypotheses); + graph.resize(0); init_values.clear(); - results = isam2->calculateBestEstimate(); + results = isam.estimate(); + isam.assignment().print("The Discrete Assignment"); - //* + // 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() << std::endl; } - // */ if (key_s == key_t - 1) { clock_t cur_time = clock(); @@ -181,7 +186,8 @@ 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_city10000_S" + step_file_idx; + string step_file_name = + "step_files/HybridISAM_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)); @@ -190,6 +196,8 @@ int main(int argc, char* argv[]) { } step_outfile.close(); } + + index += 1; } clock_t end_time = clock(); @@ -197,10 +205,10 @@ int main(int argc, char* argv[]) { cout << "total_time: " << total_time << endl; /// Write results to file - write_results(results, (key_t + 1)); + write_results(results, (key_t + 1), "HybridISAM_city10000.txt"); ofstream outfile_time; - std::string time_file_name = "ISAM2_city10000_time.txt"; + std::string time_file_name = "HybridISAM_city10000_time.txt"; outfile_time.open(time_file_name); for (auto acc_time : time_list) { outfile_time << acc_time << std::endl;