From 4e34ca1eacb05d05125de82afbceb428fbc0b196 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jan 2025 16:35:50 -0500 Subject: [PATCH 01/30] function for hybrid odometry factor --- examples/Hybrid_City10000.cpp | 75 +++++++++++++++++++++++++---------- 1 file changed, 53 insertions(+), 22 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 2051a75d6..2a956a090 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -39,6 +39,7 @@ using namespace std; using namespace gtsam; using namespace boost::algorithm; +using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::X; @@ -75,6 +76,30 @@ void write_results(const Values& results, size_t num_poses, std::cout << "output written to " << filename << std::endl; } +// HybridNonlinearFactor LoopClosureHybridFactor() { +// DiscreteKey l(L(loop_counter), 2); +// 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(l, {f0, f1}); +// } + +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) { + 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}); + return mixtureFactor; +} + void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, const Values& initial, size_t maxNrHypotheses, Values* results) { @@ -82,7 +107,9 @@ void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, // std::cout << "index: " << index << std::endl; smoother.update(linearized, maxNrHypotheses); graph.resize(0); + gttic_(HybridSmootherOptimize); HybridValues delta = smoother.hybridBayesNet().optimize(); + gttoc_(HybridSmootherOptimize); results->insert_or_assign(initial.retract(delta.continuous())); } @@ -96,6 +123,8 @@ int main(int argc, char* argv[]) { // ifstream in("../data/mh_All_city10000_groundtruth.txt"); size_t discrete_count = 0, index = 0; + size_t pose_count = 0, loop_count = 0; + size_t nrHybridFactors = 0; std::list time_list; @@ -131,6 +160,9 @@ int main(int argc, char* argv[]) { key_s = stoi(parts[1]); key_t = stoi(parts[3]); + int empty = stoi(parts[4]); // 0 or 1 + bool allow_empty = !(empty == 0); + int num_measurements = stoi(parts[5]); vector pose_array(num_measurements); for (int i = 0; i < num_measurements; ++i) { @@ -140,34 +172,31 @@ int main(int argc, char* argv[]) { pose_array[i] = Pose2(x, y, rad); } - // 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), init_values.at(X(key_s)) * odom_pose); - - } else { // loop - // index++; - } - // Flag if we should run smoother update bool smoother_update = false; - if (num_measurements == 2) { - // Add hybrid factor which considers both measurements - DiscreteKey m(M(discrete_count), num_measurements); - discrete_count++; + // Take the first one as the initial estimate + Pose2 odom_pose = pose_array[0]; + if (key_s == key_t - 1) { // odometry + init_values.insert(X(key_t), init_values.at(X(key_s)) * odom_pose); + pose_count++; + + } else { // loop + loop_count++; + } + + if (num_measurements > 1) { + DiscreteKey m(M(discrete_count), num_measurements); 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}); + // 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; } else { @@ -183,10 +212,11 @@ int main(int argc, char* argv[]) { // 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; + std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC + << std::endl; // delta.discrete().print("The Discrete Assignment"); tictoc_finishedIteration_(); - tictoc_print_(); + // tictoc_print_(); } if (key_s == key_t - 1) { @@ -214,6 +244,7 @@ int main(int argc, char* argv[]) { } outfile_time.close(); cout << "output " << time_file_name << " file." << endl; + std::cout << nrHybridFactors << std::endl; return 0; } From 244a046c675a28762876fd6ef966f9d528db5693 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jan 2025 19:27:39 -0500 Subject: [PATCH 02/30] check for nullptr --- gtsam/hybrid/HybridGaussianConditional.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 78e1f5324..92ae05df6 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -322,6 +322,10 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( auto pruner = [&](const Assignment &choices, const GaussianFactorValuePair &pair) -> GaussianFactorValuePair { + // If Gaussian factor is nullptr, return infinity + if (!pair.first) { + return {nullptr, std::numeric_limits::infinity()}; + } if (max->evaluate(choices) == 0.0) return {nullptr, std::numeric_limits::infinity()}; else { From 8ad3216afc798f6cadabc0688960409d76530047 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jan 2025 23:17:52 -0500 Subject: [PATCH 03/30] update to add loop closures --- examples/Hybrid_City10000.cpp | 101 ++++++++++++++++++++++------------ 1 file changed, 65 insertions(+), 36 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 2a956a090..7da5ffbf9 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -44,7 +44,7 @@ using symbol_shorthand::M; using symbol_shorthand::X; // Testing params -const size_t max_loop_count = 2000; // 2000; // 200 //2000 //8000 +const size_t max_loop_count = 3000; // 2000; // 200 //2000 //8000 noiseModel::Diagonal::shared_ptr prior_noise_model = noiseModel::Diagonal::Sigmas( @@ -76,15 +76,34 @@ void write_results(const Values& results, size_t num_poses, std::cout << "output written to " << filename << std::endl; } -// HybridNonlinearFactor LoopClosureHybridFactor() { -// DiscreteKey l(L(loop_counter), 2); -// 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(l, {f0, f1}); -// } +/** + * @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); + noiseModel::Diagonal::shared_ptr loop_noise_model = + noiseModel::Diagonal::Sigmas( + Vector3(1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0)); + noiseModel::Diagonal::shared_ptr loose_noise_model = + noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100); + + auto f0 = std::make_shared>( + X(key_s), X(key_t), measurement, loose_noise_model); + auto f1 = std::make_shared>( + X(key_s), X(key_t), measurement, loop_noise_model); + std::vector factors{{f0, 0.0}, {f1, 0.0}}; + HybridNonlinearFactor mixtureFactor(l, {f0, f1}); + return mixtureFactor; +} HybridNonlinearFactor HybridOdometryFactor( size_t num_measurements, size_t key_s, size_t key_t, const DiscreteKey& m, @@ -107,9 +126,7 @@ void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, // std::cout << "index: " << index << std::endl; smoother.update(linearized, maxNrHypotheses); graph.resize(0); - gttic_(HybridSmootherOptimize); HybridValues delta = smoother.hybridBayesNet().optimize(); - gttoc_(HybridSmootherOptimize); results->insert_or_assign(initial.retract(delta.continuous())); } @@ -123,7 +140,7 @@ int main(int argc, char* argv[]) { // ifstream in("../data/mh_All_city10000_groundtruth.txt"); size_t discrete_count = 0, index = 0; - size_t pose_count = 0, loop_count = 0; + size_t loop_count = 0; size_t nrHybridFactors = 0; std::list time_list; @@ -160,9 +177,6 @@ int main(int argc, char* argv[]) { key_s = stoi(parts[1]); key_t = stoi(parts[3]); - int empty = stoi(parts[4]); // 0 or 1 - bool allow_empty = !(empty == 0); - int num_measurements = stoi(parts[5]); vector pose_array(num_measurements); for (int i = 0; i < num_measurements; ++i) { @@ -179,33 +193,41 @@ int main(int argc, char* argv[]) { Pose2 odom_pose = pose_array[0]; if (key_s == key_t - 1) { // odometry + if (num_measurements > 1) { + DiscreteKey m(M(discrete_count), num_measurements); + // graph.push_back(DecisionTreeFactor(m, "0.6 0.4")); + + // 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; + + } else { + graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, + pose_noise_model)); + } + init_values.insert(X(key_t), init_values.at(X(key_s)) * odom_pose); - pose_count++; } else { // loop - loop_count++; - } - - if (num_measurements > 1) { - DiscreteKey m(M(discrete_count), num_measurements); - graph.push_back(DecisionTreeFactor(m, "0.6 0.4")); - - // 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++; + HybridNonlinearFactor loop_factor = + HybridLoopClosureFactor(loop_count, key_s, key_t, odom_pose); + graph.add(loop_factor); smoother_update = true; - } else { - graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, - pose_noise_model)); + loop_count++; } if (smoother_update) { + gttic_(SmootherUpdate); SmootherUpdate(smoother, graph, init_values, maxNrHypotheses, &results); + init_values.update(results); + gttoc_(SmootherUpdate); } // Print loop index and time taken in processor clock ticks @@ -216,7 +238,7 @@ int main(int argc, char* argv[]) { << std::endl; // delta.discrete().print("The Discrete Assignment"); tictoc_finishedIteration_(); - // tictoc_print_(); + tictoc_print_(); } if (key_s == key_t - 1) { @@ -229,15 +251,22 @@ int main(int argc, char* argv[]) { SmootherUpdate(smoother, graph, init_values, maxNrHypotheses, &results); + gttic_(HybridSmootherOptimize); + HybridValues delta = smoother.hybridBayesNet().optimize(); + gttoc_(HybridSmootherOptimize); + results.insert_or_assign(init_values.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 results to file - write_results(results, (key_t + 1), "HybridISAM_city10000.txt"); + write_results(results, (key_t + 1), "Hybrid_City10000.txt"); ofstream outfile_time; - std::string time_file_name = "HybridISAM_city10000_time.txt"; + 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; From 761d19ab4f159a7797382c5e9616d97c38a6e6ab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Jan 2025 04:18:19 -0500 Subject: [PATCH 04/30] major script improvements --- examples/Hybrid_City10000.cpp | 37 ++++++++++++++++------------------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 7da5ffbf9..0c12a440e 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -55,19 +55,19 @@ noiseModel::Diagonal::shared_ptr pose_noise_model = (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); /** - * @brief Write the results of optimization to filename. + * @brief Write the result of optimization to filename. * - * @param results The Values object with the final results. + * @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 results to. + * @param filename The file name to save the result to. */ -void write_results(const Values& results, size_t num_poses, - const std::string& filename = "ISAM2_city10000.txt") { +void write_result(const Values& result, size_t num_poses, + const std::string& filename = "Hybrid_city10000.txt") { ofstream outfile; outfile.open(filename); for (size_t i = 0; i < num_poses; ++i) { - Pose2 out_pose = results.at(X(i)); + Pose2 out_pose = result.at(X(i)); outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() << std::endl; @@ -121,13 +121,13 @@ HybridNonlinearFactor HybridOdometryFactor( void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, const Values& initial, size_t maxNrHypotheses, - Values* results) { + 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(); - results->insert_or_assign(initial.retract(delta.continuous())); + result->insert_or_assign(initial.retract(delta.continuous())); } /* ************************************************************************* */ @@ -149,8 +149,7 @@ int main(int argc, char* argv[]) { HybridNonlinearFactorGraph graph; - Values init_values; - Values results; + Values initial, result; size_t maxNrHypotheses = 3; @@ -160,11 +159,11 @@ int main(int argc, char* argv[]) { Pose2 prior_pose(x, y, rad); - init_values.insert(X(0), prior_pose); + initial.insert(X(0), prior_pose); graph.push_back(PriorFactor(X(0), prior_pose, prior_noise_model)); - SmootherUpdate(smoother, graph, init_values, maxNrHypotheses, &results); + SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); size_t key_s, key_t{0}; @@ -195,7 +194,6 @@ int main(int argc, char* argv[]) { if (num_measurements > 1) { DiscreteKey m(M(discrete_count), num_measurements); - // graph.push_back(DecisionTreeFactor(m, "0.6 0.4")); // Add hybrid factor which considers both measurements HybridNonlinearFactor mixtureFactor = HybridOdometryFactor( @@ -211,7 +209,7 @@ int main(int argc, char* argv[]) { pose_noise_model)); } - init_values.insert(X(key_t), init_values.at(X(key_s)) * odom_pose); + initial.insert(X(key_t), initial.at(X(key_s)) * odom_pose); } else { // loop HybridNonlinearFactor loop_factor = @@ -225,8 +223,7 @@ int main(int argc, char* argv[]) { if (smoother_update) { gttic_(SmootherUpdate); - SmootherUpdate(smoother, graph, init_values, maxNrHypotheses, &results); - init_values.update(results); + SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); gttoc_(SmootherUpdate); } @@ -249,12 +246,12 @@ int main(int argc, char* argv[]) { index += 1; } - SmootherUpdate(smoother, graph, init_values, maxNrHypotheses, &results); + SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); gttic_(HybridSmootherOptimize); HybridValues delta = smoother.hybridBayesNet().optimize(); gttoc_(HybridSmootherOptimize); - results.insert_or_assign(init_values.retract(delta.continuous())); + result.insert_or_assign(initial.retract(delta.continuous())); std::cout << "Final error: " << smoother.hybridBayesNet().error(delta) << std::endl; @@ -262,8 +259,8 @@ int main(int argc, char* argv[]) { clock_t total_time = end_time - start_time; cout << "total_time: " << total_time / CLOCKS_PER_SEC << " seconds" << endl; - /// Write results to file - write_results(results, (key_t + 1), "Hybrid_City10000.txt"); + /// Write result to file + write_result(result, (key_t + 1), "Hybrid_City10000.txt"); ofstream outfile_time; std::string time_file_name = "Hybrid_City10000_time.txt"; From 5e99b0e34d5f8be92580467ebf2862504789ca10 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Jan 2025 16:10:20 -0500 Subject: [PATCH 05/30] simplify and add smoother_update_times --- examples/Hybrid_City10000.cpp | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 0c12a440e..8c67e8ed6 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -90,16 +90,12 @@ HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s, size_t key_t, const Pose2& measurement) { DiscreteKey l(L(loop_counter), 2); - noiseModel::Diagonal::shared_ptr loop_noise_model = - noiseModel::Diagonal::Sigmas( - Vector3(1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0)); - noiseModel::Diagonal::shared_ptr loose_noise_model = - noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100); auto f0 = std::make_shared>( - X(key_s), X(key_t), measurement, loose_noise_model); + X(key_s), X(key_t), measurement, + noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100)); auto f1 = std::make_shared>( - X(key_s), X(key_t), measurement, loop_noise_model); + X(key_s), X(key_t), measurement, pose_noise_model); std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridNonlinearFactor mixtureFactor(l, {f0, f1}); return mixtureFactor; @@ -145,7 +141,7 @@ int main(int argc, char* argv[]) { std::list time_list; - HybridSmoother smoother(0.99); + HybridSmoother smoother; HybridNonlinearFactorGraph graph; @@ -163,7 +159,12 @@ int main(int argc, char* argv[]) { graph.push_back(PriorFactor(X(0), prior_pose, prior_noise_model)); + std::vector> 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}; @@ -223,7 +224,10 @@ int main(int argc, char* argv[]) { 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); } @@ -246,7 +250,10 @@ int main(int argc, char* argv[]) { index += 1; } + before_update = clock(); SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); + after_update = clock(); + smoother_update_times.push_back({index, after_update - before_update}); gttic_(HybridSmootherOptimize); HybridValues delta = smoother.hybridBayesNet().optimize(); @@ -262,6 +269,11 @@ int main(int argc, char* argv[]) { /// 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); From 90fd416e5da13679ad5c3286a1ab3c8e05b20cca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Jan 2025 16:10:54 -0500 Subject: [PATCH 06/30] human-friendly time printing --- examples/ISAM2_City10000.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index e5fe50951..aaad608ac 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -165,7 +165,8 @@ int main(int argc, char* argv[]) { // 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; + std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC + << std::endl; } if (key_s == key_t - 1) { @@ -190,7 +191,7 @@ 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; + cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; /// Write results to file write_results(results, (key_t + 1)); From 1bdaa5062c73e616fd6ab9ccd6221cf1f0ca8239 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Jan 2025 16:22:23 -0500 Subject: [PATCH 07/30] switch between TableFactor and DecisionTreeFactor --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 33 +++++++++++++++++----- 1 file changed, 26 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index bc36ec94d..cfe5c4b59 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -50,6 +50,8 @@ #include #include +#define GTSAM_HYBRID_WITH_TABLEFACTOR 0 + namespace gtsam { /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph: @@ -253,7 +255,11 @@ static DiscreteFactor::shared_ptr DiscreteFactorFromErrors( double min_log = errors.min(); AlgebraicDecisionTree potentials( errors, [&min_log](const double x) { return exp(-(x - min_log)); }); +#if GTSAM_HYBRID_WITH_TABLEFACTOR return std::make_shared(discreteKeys, potentials); +#else + return std::make_shared(discreteKeys, potentials); +#endif } /* ************************************************************************ */ @@ -290,9 +296,13 @@ static DiscreteFactorGraph CollectDiscreteFactors( /// Get the underlying TableFactor dfg.push_back(dtc->table()); } else { +#if GTSAM_HYBRID_WITH_TABLEFACTOR // Convert DiscreteConditional to TableFactor auto tdc = std::make_shared(*dc); dfg.push_back(tdc); +#else + dfg.push_back(dc); +#endif } #if GTSAM_HYBRID_TIMING gttoc_(ConvertConditionalToTableFactor); @@ -309,11 +319,18 @@ static DiscreteFactorGraph CollectDiscreteFactors( static std::pair> discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { +#if GTSAM_HYBRID_TIMING + gttic_(CollectDiscreteFactors); +#endif DiscreteFactorGraph dfg = CollectDiscreteFactors(factors); +#if GTSAM_HYBRID_TIMING + gttoc_(CollectDiscreteFactors); +#endif #if GTSAM_HYBRID_TIMING gttic_(EliminateDiscrete); #endif +#if GTSAM_HYBRID_WITH_TABLEFACTOR // Check if separator is empty. // This is the same as checking if the number of frontal variables // is the same as the number of variables in the DiscreteFactorGraph. @@ -323,9 +340,6 @@ discreteElimination(const HybridGaussianFactorGraph &factors, // Get product factor DiscreteFactor::shared_ptr product = dfg.scaledProduct(); -#if GTSAM_HYBRID_TIMING - gttic_(EliminateDiscreteFormDiscreteConditional); -#endif // Check type of product, and get as TableFactor for efficiency. // Use object instead of pointer since we need it // for the TableDistribution constructor. @@ -337,19 +351,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } auto conditional = std::make_shared(p); -#if GTSAM_HYBRID_TIMING - gttoc_(EliminateDiscreteFormDiscreteConditional); -#endif - DiscreteFactor::shared_ptr sum = p.sum(frontalKeys); return {std::make_shared(conditional), sum}; } else { +#endif // Perform sum-product. auto result = EliminateDiscrete(dfg, frontalKeys); return {std::make_shared(result.first), result.second}; +#if GTSAM_HYBRID_WITH_TABLEFACTOR } +#endif #if GTSAM_HYBRID_TIMING gttoc_(EliminateDiscrete); #endif @@ -411,8 +424,14 @@ static std::shared_ptr createHybridGaussianFactor( throw std::runtime_error("createHybridGaussianFactors has mixed NULLs"); } }; +#if GTSAM_HYBRID_TIMING + gttic_(HybridCreateGaussianFactor); +#endif DecisionTree newFactors(eliminationResults, correct); +#if GTSAM_HYBRID_TIMING + gttoc_(HybridCreateGaussianFactor); +#endif return std::make_shared(discreteSeparator, newFactors); } From 90d3f2e84aa389698daf890e26ff48e6f0572cfb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Jan 2025 16:22:49 -0500 Subject: [PATCH 08/30] specify dead mode removal threshold --- examples/Hybrid_City10000.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 8c67e8ed6..f79f88970 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -141,7 +141,7 @@ int main(int argc, char* argv[]) { std::list time_list; - HybridSmoother smoother; + HybridSmoother smoother(0.99); HybridNonlinearFactorGraph graph; From 18be91b917001f995e6082fd93d0c6b42bec4a02 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 28 Jan 2025 21:31:32 -0500 Subject: [PATCH 09/30] Make class --- examples/Hybrid_City10000.cpp | 425 +++++++++++++++++----------------- 1 file changed, 213 insertions(+), 212 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index f79f88970..667b6f6f4 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -46,243 +46,244 @@ using symbol_shorthand::X; // Testing params const size_t max_loop_count = 3000; // 2000; // 200 //2000 //8000 -noiseModel::Diagonal::shared_ptr prior_noise_model = - noiseModel::Diagonal::Sigmas( - (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); +auto 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()); +auto pose_noise_model = noiseModel::Diagonal::Sigmas( + (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).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); +class Experiment { + /** + * @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 write_result(const Values& result, size_t num_poses, + 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)); + 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() - << 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); - - auto f0 = std::make_shared>( - X(key_s), X(key_t), measurement, - noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100)); - auto f1 = std::make_shared>( - X(key_s), X(key_t), measurement, pose_noise_model); - std::vector factors{{f0, 0.0}, {f1, 0.0}}; - HybridNonlinearFactor mixtureFactor(l, {f0, f1}); - return mixtureFactor; -} - -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) { - 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}); - return mixtureFactor; -} - -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(); - result->insert_or_assign(initial.retract(delta.continuous())); -} - -/* ************************************************************************* */ -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 discrete_count = 0, index = 0; - size_t loop_count = 0; - size_t nrHybridFactors = 0; - - std::list 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(X(0), prior_pose, prior_noise_model)); - - std::vector> 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 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); + outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() + << std::endl; } + outfile.close(); + std::cout << "output written to " << filename << std::endl; + } - // Flag if we should run smoother update - bool smoother_update = false; + /** + * @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, + const Pose2& measurement) { + DiscreteKey l(L(loop_counter), 2); - // Take the first one as the initial estimate - Pose2 odom_pose = pose_array[0]; - if (key_s == key_t - 1) { // odometry + auto f0 = std::make_shared>( + X(key_s), X(key_t), measurement, + noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100)); + auto f1 = std::make_shared>( + X(key_s), X(key_t), measurement, pose_noise_model); + std::vector factors{{f0, 0.0}, {f1, 0.0}}; + HybridNonlinearFactor mixtureFactor(l, {f0, f1}); + return mixtureFactor; + } - if (num_measurements > 1) { - DiscreteKey m(M(discrete_count), num_measurements); + // 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) { + 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}); + return mixtureFactor; + } - // 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); + // 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(); + result->insert_or_assign(initial.retract(delta.continuous())); + } - discrete_count++; + 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; + + std::list 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(X(0), prior_pose, prior_noise_model)); + + std::vector> 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 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); + } + + // Flag if we should run smoother update + bool smoother_update = 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; + + } else { + graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, + pose_noise_model)); + } + + 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; - } else { - graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, - pose_noise_model)); + loop_count++; } - initial.insert(X(key_t), initial.at(X(key_s)) * odom_pose); + 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); + } - } else { // loop - HybridNonlinearFactor loop_factor = - HybridLoopClosureFactor(loop_count, key_s, key_t, odom_pose); - graph.add(loop_factor); + // 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_(); + } - smoother_update = true; + if (key_s == key_t - 1) { + clock_t cur_time = clock(); + time_list.push_back(cur_time - start_time); + } - loop_count++; + index += 1; } - 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); - } + before_update = clock(); + SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); + after_update = clock(); + smoother_update_times.push_back({index, after_update - before_update}); - // 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_(); - } + gttic_(HybridSmootherOptimize); + HybridValues delta = smoother.hybridBayesNet().optimize(); + gttoc_(HybridSmootherOptimize); + result.insert_or_assign(initial.retract(delta.continuous())); - if (key_s == key_t - 1) { - clock_t cur_time = clock(); - time_list.push_back(cur_time - start_time); - } + 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; - index += 1; + /// 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; } +}; - before_update = clock(); - SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); - after_update = clock(); - smoother_update_times.push_back({index, after_update - before_update}); - - 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; +/* ************************************************************************* */ +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 + // Experiment("../data/mh_All_city10000_groundtruth.txt"); return 0; } From b7b0f5b57a160351e6f6f7758bba246e2f60d40a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 28 Jan 2025 21:42:11 -0500 Subject: [PATCH 10/30] 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 From 09bf00b54583a505087e7f0ade72b8e237c9cdb6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2025 00:51:16 -0500 Subject: [PATCH 11/30] Catch pruned away --- gtsam/hybrid/HybridGaussianConditional.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 92ae05df6..964af7ffb 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -313,22 +313,21 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(), std::back_inserter(diff)); - // Find maximum probability value for every combination of our keys. - Ordering keys(diff); - auto max = discreteProbs.max(keys); + // Find maximum probability value for every combination of *our* keys. + Ordering ordering(diff); + auto max = discreteProbs.max(ordering); // Check the max value for every combination of our keys. // If the max value is 0.0, we can prune the corresponding conditional. + bool allPruned = true; auto pruner = [&](const Assignment &choices, const GaussianFactorValuePair &pair) -> GaussianFactorValuePair { - // If Gaussian factor is nullptr, return infinity - if (!pair.first) { + // If this choice is zero probability or Gaussian is null, return infinity + if (!pair.first || max->evaluate(choices) == 0.0) { return {nullptr, std::numeric_limits::infinity()}; - } - if (max->evaluate(choices) == 0.0) - return {nullptr, std::numeric_limits::infinity()}; - else { + } else { + allPruned = false; // Add negLogConstant_ back so that the minimum negLogConstant in the // HybridGaussianConditional is set correctly. return {pair.first, pair.second + negLogConstant_}; @@ -336,6 +335,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( }; FactorValuePairs prunedConditionals = factors().apply(pruner); + if (allPruned) return nullptr; return std::make_shared(discreteKeys(), prunedConditionals, true); } From 7022f5923729d1464c5e2f651c5c8e511cba6875 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2025 13:55:12 -0500 Subject: [PATCH 12/30] Undo scaling --- gtsam/discrete/DiscreteFactorGraph.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index f2bae4b9b..6f1f2d592 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -228,11 +228,11 @@ namespace gtsam { // We divide both `product` and `sum` by `max(sum)` // since it is faster to compute and when the conditional // is formed by `product/sum`, the scaling term cancels out. - gttic(scale); - DiscreteFactor::shared_ptr denominator = sum->max(sum->size()); - product = product->operator/(denominator); - sum = sum->operator/(denominator); - gttoc(scale); + // gttic(scale); + // DiscreteFactor::shared_ptr denominator = sum->max(sum->size()); + // product = product->operator/(denominator); + // sum = sum->operator/(denominator); + // gttoc(scale); // Ordering keys for the conditional so that frontalKeys are really in front Ordering orderedKeys; From cde2bebf36d2e21f0d859934005e369099253e1c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2025 13:55:22 -0500 Subject: [PATCH 13/30] Fix loop noise model --- examples/Hybrid_City10000.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index b704b7e0d..b8b3338dc 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -92,7 +92,7 @@ class Experiment { auto f0 = std::make_shared>( X(keyS), X(keyT), measurement, - noiseModel::Diagonal::Sigmas(Vector3::Ones() * 100)); + noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10)); auto f1 = std::make_shared>( X(keyS), X(keyT), measurement, kPoseNoiseModel); From 36f2a3d2983d94e79349d8661201ac4a47068ebe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Jan 2025 14:43:57 -0500 Subject: [PATCH 14/30] two pass to for addConditionals --- gtsam/hybrid/HybridSmoother.cpp | 30 ++++++++++++++++++++++-------- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 594c12825..66d1145b7 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -108,6 +108,28 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, // NOTE(Varun) Using a for-range loop doesn't work since some of the // conditionals are invalid pointers + + // First get all the keys involved. + // We do this by iterating over all conditionals, and checking if their + // frontals are involved in the factor graph. If yes, then also make the + // parent keys involved in the factor graph. + for (size_t i = 0; i < hybridBayesNet.size(); i++) { + auto conditional = hybridBayesNet.at(i); + + for (auto &key : conditional->frontals()) { + if (std::find(factorKeys.begin(), factorKeys.end(), key) != + factorKeys.end()) { + // Add the conditional parents to factorKeys + // so we add those conditionals too. + for (auto &&parentKey : conditional->parents()) { + factorKeys.insert(parentKey); + } + // Break so we don't add parents twice. + break; + } + } + } + for (size_t i = 0; i < hybridBayesNet.size(); i++) { auto conditional = hybridBayesNet.at(i); @@ -116,14 +138,6 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, factorKeys.end()) { newConditionals.push_back(conditional); - // Add the conditional parents to factorKeys - // so we add those conditionals too. - // NOTE: This assumes we have a structure where - // variables depend on those in the future. - for (auto &&parentKey : conditional->parents()) { - factorKeys.insert(parentKey); - } - // Remove the conditional from the updated Bayes net auto it = find(updatedHybridBayesNet.begin(), updatedHybridBayesNet.end(), conditional); From 555a2173a372655da555886ff0c1ab7102c8b131 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2025 17:54:04 -0500 Subject: [PATCH 15/30] Don't optimize every time --- examples/Hybrid_City10000.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index b8b3338dc..17a90d980 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -43,7 +43,7 @@ using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::X; -const size_t kMaxLoopCount = 3000; // Example default value +const size_t kMaxLoopCount = 2000; // Example default value auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); @@ -123,8 +123,8 @@ class Experiment { 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())); + // HybridValues delta = smoother.hybridBayesNet().optimize(); + // result->insert_or_assign(initial.retract(delta.continuous())); } public: @@ -202,6 +202,7 @@ class Experiment { graph_.push_back(mixtureFactor); discreteCount++; doSmootherUpdate = true; + std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl; } else { graph_.add(BetweenFactor(X(keyS), X(keyT), odomPose, kPoseNoiseModel)); @@ -212,6 +213,8 @@ class Experiment { // 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++; @@ -224,6 +227,7 @@ class Experiment { afterUpdate = clock(); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); gttoc_(SmootherUpdate); + doSmootherUpdate = false; } // Record timing for odometry edges only @@ -238,9 +242,9 @@ class Experiment { 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_(); + // delta.discrete().print("The Discrete Assignment"); + tictoc_finishedIteration_(); + tictoc_print_(); } } @@ -297,9 +301,10 @@ 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 + // Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + + // Type #3 - // Run the experiment + // Run the experiment experiment.run(kMaxLoopCount); return 0; From ce031e8e815b9a072cdcb128810cbccf625b03c9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2025 17:54:50 -0500 Subject: [PATCH 16/30] Add smoother printing --- gtsam/hybrid/HybridSmoother.cpp | 37 +++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 66d1145b7..831a92d2f 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -21,6 +21,7 @@ #include #include +// #define DEBUG_SMOOTHER namespace gtsam { /* ************************************************************************* */ @@ -56,10 +57,16 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, void HybridSmoother::update(const HybridGaussianFactorGraph &graph, std::optional maxNrLeaves, const std::optional given_ordering) { + std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() << std::endl; + std::cout << "newFactors size: " << graph.size() << std::endl; HybridGaussianFactorGraph updatedGraph; // Add the necessary conditionals from the previous timestep(s). std::tie(updatedGraph, hybridBayesNet_) = addConditionals(graph, hybridBayesNet_); + // print size of graph, updatedGraph, hybridBayesNet_ + std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; + std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() << std::endl; + std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size() << std::endl; Ordering ordering; // If no ordering provided, then we compute one @@ -77,6 +84,19 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, // Eliminate. HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering); +#ifdef DEBUG_SMOOTHER + for (auto conditional: bayesNetFragment) { + auto e =std::dynamic_pointer_cast(conditional); + GTSAM_PRINT(*e); + } +#endif + + // Print discrete keys in the bayesNetFragment: + std::cout << "Discrete keys in bayesNetFragment: "; + for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + /// Prune if (maxNrLeaves) { // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in @@ -84,6 +104,20 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_); } + // Print discrete keys in the bayesNetFragment: + std::cout << "\nAfter pruning: "; + for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl << std::endl; + +#ifdef DEBUG_SMOOTHER + for (auto conditional: bayesNetFragment) { + auto c =std::dynamic_pointer_cast(conditional); + GTSAM_PRINT(*c); + } +#endif + // Add the partial bayes net to the posterior bayes net. hybridBayesNet_.add(bayesNetFragment); } @@ -117,6 +151,8 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, auto conditional = hybridBayesNet.at(i); for (auto &key : conditional->frontals()) { + // GTSAM_PRINT(*std::dynamic_pointer_cast(conditional)); + // GTSAM_PRINT(*conditional); if (std::find(factorKeys.begin(), factorKeys.end(), key) != factorKeys.end()) { // Add the conditional parents to factorKeys @@ -129,6 +165,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, } } } + PrintKeySet(factorKeys); for (size_t i = 0; i < hybridBayesNet.size(); i++) { auto conditional = hybridBayesNet.at(i); From 64d4540e9a5466ec9999ce19a8a11878da88455a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2025 17:55:14 -0500 Subject: [PATCH 17/30] Nice new HC tests --- gtsam/hybrid/tests/testHybridConditional.cpp | 28 ++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 365dde3bc..216acdb4e 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -28,6 +28,28 @@ using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; +/* ****************************************************************************/ +// Test the HybridConditional constructor. +TEST(HybridConditional, Constructor) { + // Create a HybridGaussianConditional. + const KeyVector continuousKeys{X(0), X(1)}; + const DiscreteKeys discreteKeys{{M(0), 2}}; + const size_t nFrontals = 1; + const HybridConditional hc(continuousKeys, discreteKeys, nFrontals); + + // Check Frontals: + EXPECT_LONGS_EQUAL(1, hc.nrFrontals()); + const auto frontals = hc.frontals(); + EXPECT_LONGS_EQUAL(1, frontals.size()); + EXPECT_LONGS_EQUAL(X(0), *frontals.begin()); + + // Check parents: + const auto parents = hc.parents(); + EXPECT_LONGS_EQUAL(2, parents.size()); + EXPECT_LONGS_EQUAL(X(1), *parents.begin()); + EXPECT_LONGS_EQUAL(M(0), *(parents.begin() + 1)); +} + /* ****************************************************************************/ // Check invariants for all conditionals in a tiny Bayes net. TEST(HybridConditional, Invariants) { @@ -43,6 +65,12 @@ TEST(HybridConditional, Invariants) { auto hc0 = bn.at(0); CHECK(hc0->isHybrid()); + // Check parents: + const auto parents = hc0->parents(); + EXPECT_LONGS_EQUAL(2, parents.size()); + EXPECT_LONGS_EQUAL(X(0), *parents.begin()); + EXPECT_LONGS_EQUAL(M(0), *(parents.begin() + 1)); + // Check invariants as a HybridGaussianConditional. const auto conditional = hc0->asHybrid(); EXPECT(HybridGaussianConditional::CheckInvariants(*conditional, values)); From e7a68fa6f2231aac328c91f2867fdcd677fc3c94 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 00:16:22 -0500 Subject: [PATCH 18/30] Catch exception and print out things --- gtsam/hybrid/HybridGaussianConditional.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 964af7ffb..0546ff16b 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -191,11 +191,19 @@ size_t HybridGaussianConditional::nrComponents() const { /* *******************************************************************************/ GaussianConditional::shared_ptr HybridGaussianConditional::choose( const DiscreteValues &discreteValues) const { - auto &[factor, _] = factors()(discreteValues); - if (!factor) return nullptr; + try { + auto &[factor, _] = factors()(discreteValues); + if (!factor) return nullptr; - auto conditional = checkConditional(factor); - return conditional; + auto conditional = checkConditional(factor); + return conditional; + } catch (const std::out_of_range &e) { + GTSAM_PRINT(*this); + GTSAM_PRINT(discreteValues); + throw std::runtime_error( + "HybridGaussianConditional::choose: discreteValues does not contain " + "all discrete parents."); + } } /* *******************************************************************************/ From 2af9d2d35ab0dc6a5745d220e0148cda0f6d2ad2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 00:23:17 -0500 Subject: [PATCH 19/30] Add negLogConstants --- examples/Hybrid_City10000.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 17a90d980..8136b255d 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -45,6 +45,8 @@ using symbol_shorthand::X; const size_t kMaxLoopCount = 2000; // Example default value +auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); + auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); @@ -91,12 +93,13 @@ class Experiment { DiscreteKey l(L(loopCounter), 2); auto f0 = std::make_shared>( - X(keyS), X(keyT), measurement, - noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10)); + X(keyS), X(keyT), measurement, kOpenLoopModel); auto f1 = std::make_shared>( X(keyS), X(keyT), measurement, kPoseNoiseModel); - std::vector factors{{f0, 0.0}, {f1, 0.0}}; + std::vector factors{ + {f0, kOpenLoopModel->negLogConstant()}, + {f1, kPoseNoiseModel->negLogConstant()}}; HybridNonlinearFactor mixtureFactor(l, factors); return mixtureFactor; } From b5ddba9c3c1eb3e8a83c4ad787f79361af69fada Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 01:29:04 -0500 Subject: [PATCH 20/30] Return fixed values --- gtsam/hybrid/HybridSmoother.cpp | 24 ++++++++++++++++-------- gtsam/hybrid/HybridSmoother.h | 12 ++++++++++++ 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 831a92d2f..65e9d9bd6 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -57,7 +57,8 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, void HybridSmoother::update(const HybridGaussianFactorGraph &graph, std::optional maxNrLeaves, const std::optional given_ordering) { - std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() << std::endl; + std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() + << std::endl; std::cout << "newFactors size: " << graph.size() << std::endl; HybridGaussianFactorGraph updatedGraph; // Add the necessary conditionals from the previous timestep(s). @@ -65,8 +66,10 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, addConditionals(graph, hybridBayesNet_); // print size of graph, updatedGraph, hybridBayesNet_ std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; - std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() << std::endl; - std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size() << std::endl; + std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() + << std::endl; + std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size() + << std::endl; Ordering ordering; // If no ordering provided, then we compute one @@ -85,8 +88,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering); #ifdef DEBUG_SMOOTHER - for (auto conditional: bayesNetFragment) { - auto e =std::dynamic_pointer_cast(conditional); + for (auto conditional : bayesNetFragment) { + auto e = std::dynamic_pointer_cast( + conditional); GTSAM_PRINT(*e); } #endif @@ -101,7 +105,10 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, if (maxNrLeaves) { // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in // all the conditionals with the same keys in bayesNetFragment. - bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_); + DiscreteValues newlyFixedValues; + bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_, + &newlyFixedValues); + fixedValues_.insert(newlyFixedValues); } // Print discrete keys in the bayesNetFragment: @@ -112,8 +119,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, std::cout << std::endl << std::endl; #ifdef DEBUG_SMOOTHER - for (auto conditional: bayesNetFragment) { - auto c =std::dynamic_pointer_cast(conditional); + for (auto conditional : bayesNetFragment) { + auto c = std::dynamic_pointer_cast( + conditional); GTSAM_PRINT(*c); } #endif diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 2f7bfcebb..653df3957 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -106,6 +106,18 @@ class GTSAM_EXPORT HybridSmoother { /// Return the Bayes Net posterior. const HybridBayesNet& hybridBayesNet() const; + + /// Optimize the hybrid Bayes Net, taking into accound fixed values. + HybridValues optimize() const { + // Solve for the MPE + DiscreteValues mpe = hybridBayesNet_.mpe(); + + // Add fixed values to the MPE. + mpe.insert(fixedValues_); + + // Given the MPE, compute the optimal continuous values. + return HybridValues(hybridBayesNet_.optimize(mpe), mpe); + } }; } // namespace gtsam From efea6b8cb97a61808083363bf9fd99748ba231b2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 07:57:42 -0500 Subject: [PATCH 21/30] DiscreteNet::prune --- gtsam/discrete/DiscreteBayesNet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 8c04cb91c..12c607223 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -89,7 +89,7 @@ DiscreteBayesNet DiscreteBayesNet::prune( DiscreteValues deadModesValues; // If we have a dead mode threshold and discrete variables left after pruning, // then we run dead mode removal. - if (marginalThreshold.has_value() && pruned.keys().size() > 0) { + if (marginalThreshold && pruned.keys().size() > 0) { DiscreteMarginals marginals(DiscreteFactorGraph{pruned}); for (auto dkey : pruned.discreteKeys()) { const Vector probabilities = marginals.marginalProbabilities(dkey); From a1467c5e848ef19a08c4d8e8a112554bfb9cdc8e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 10:11:20 -0500 Subject: [PATCH 22/30] Guard all printing --- gtsam/hybrid/HybridSmoother.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 65e9d9bd6..89e091c42 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -57,19 +57,23 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, void HybridSmoother::update(const HybridGaussianFactorGraph &graph, std::optional maxNrLeaves, const std::optional given_ordering) { +#ifdef DEBUG_SMOOTHER std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() << std::endl; std::cout << "newFactors size: " << graph.size() << std::endl; +#endif HybridGaussianFactorGraph updatedGraph; // Add the necessary conditionals from the previous timestep(s). std::tie(updatedGraph, hybridBayesNet_) = addConditionals(graph, hybridBayesNet_); +#ifdef DEBUG_SMOOTHER // print size of graph, updatedGraph, hybridBayesNet_ std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() << std::endl; std::cout << "total size: " << updatedGraph.size() + hybridBayesNet_.size() << std::endl; +#endif Ordering ordering; // If no ordering provided, then we compute one @@ -87,7 +91,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, // Eliminate. HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering); -#ifdef DEBUG_SMOOTHER +#ifdef DEBUG_SMOOTHER_DETAIL for (auto conditional : bayesNetFragment) { auto e = std::dynamic_pointer_cast( conditional); @@ -95,11 +99,13 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, } #endif +#ifdef DEBUG_SMOOTHER // Print discrete keys in the bayesNetFragment: std::cout << "Discrete keys in bayesNetFragment: "; for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) { std::cout << DefaultKeyFormatter(key) << " "; } +#endif /// Prune if (maxNrLeaves) { @@ -111,14 +117,16 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, fixedValues_.insert(newlyFixedValues); } +#ifdef DEBUG_SMOOTHER // Print discrete keys in the bayesNetFragment: std::cout << "\nAfter pruning: "; for (auto &key : HybridFactorGraph(bayesNetFragment).discreteKeySet()) { std::cout << DefaultKeyFormatter(key) << " "; } std::cout << std::endl << std::endl; +#endif -#ifdef DEBUG_SMOOTHER +#ifdef DEBUG_SMOOTHER_DETAIL for (auto conditional : bayesNetFragment) { auto c = std::dynamic_pointer_cast( conditional); @@ -159,8 +167,6 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, auto conditional = hybridBayesNet.at(i); for (auto &key : conditional->frontals()) { - // GTSAM_PRINT(*std::dynamic_pointer_cast(conditional)); - // GTSAM_PRINT(*conditional); if (std::find(factorKeys.begin(), factorKeys.end(), key) != factorKeys.end()) { // Add the conditional parents to factorKeys @@ -173,7 +179,9 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, } } } +#ifdef DEBUG_SMOOTHER PrintKeySet(factorKeys); +#endif for (size_t i = 0; i < hybridBayesNet.size(); i++) { auto conditional = hybridBayesNet.at(i); From 3d4d7501515f345d15d3ce33230103dbda07e4a3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 10:57:41 -0500 Subject: [PATCH 23/30] throw in optimize --- gtsam/hybrid/HybridSmoother.cpp | 64 +++++++++++++++++++++------------ gtsam/hybrid/HybridSmoother.h | 11 +----- 2 files changed, 42 insertions(+), 33 deletions(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 89e091c42..b66b63ea6 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -26,48 +26,48 @@ namespace gtsam { /* ************************************************************************* */ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, - const KeySet &newFactorKeys) { + const KeySet &continuousKeys) { // Get all the discrete keys from the factors KeySet allDiscrete = factors.discreteKeySet(); // Create KeyVector with continuous keys followed by discrete keys. - KeyVector newKeysDiscreteLast; + KeyVector lastKeys; // Insert continuous keys first. - for (auto &k : newFactorKeys) { + for (auto &k : continuousKeys) { if (!allDiscrete.exists(k)) { - newKeysDiscreteLast.push_back(k); + lastKeys.push_back(k); } } // Insert discrete keys at the end std::copy(allDiscrete.begin(), allDiscrete.end(), - std::back_inserter(newKeysDiscreteLast)); + std::back_inserter(lastKeys)); const VariableIndex index(factors); // Get an ordering where the new keys are eliminated last Ordering ordering = Ordering::ColamdConstrainedLast( - index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), - true); + index, KeyVector(lastKeys.begin(), lastKeys.end()), true); return ordering; } /* ************************************************************************* */ -void HybridSmoother::update(const HybridGaussianFactorGraph &graph, +void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, std::optional maxNrLeaves, const std::optional given_ordering) { + const KeySet originalNewFactorKeys = newFactors.keys(); #ifdef DEBUG_SMOOTHER std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() << std::endl; - std::cout << "newFactors size: " << graph.size() << std::endl; + std::cout << "newFactors size: " << newFactors.size() << std::endl; #endif HybridGaussianFactorGraph updatedGraph; // Add the necessary conditionals from the previous timestep(s). std::tie(updatedGraph, hybridBayesNet_) = - addConditionals(graph, hybridBayesNet_); + addConditionals(newFactors, hybridBayesNet_); #ifdef DEBUG_SMOOTHER - // print size of graph, updatedGraph, hybridBayesNet_ + // print size of newFactors, updatedGraph, hybridBayesNet_ std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; std::cout << "hybridBayesNet_ size after: " << hybridBayesNet_.size() << std::endl; @@ -79,11 +79,11 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, // If no ordering provided, then we compute one if (!given_ordering.has_value()) { // Get the keys from the new factors - const KeySet newFactorKeys = graph.keys(); + const KeySet continuousKeysToInclude;// = newFactors.keys(); // Since updatedGraph now has all the connected conditionals, // we can get the correct ordering. - ordering = this->getOrdering(updatedGraph, newFactorKeys); + ordering = this->getOrdering(updatedGraph, continuousKeysToInclude); } else { ordering = *given_ordering; } @@ -140,12 +140,15 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph, /* ************************************************************************* */ std::pair -HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, +HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors, const HybridBayesNet &hybridBayesNet) const { - HybridGaussianFactorGraph graph(originalGraph); + HybridGaussianFactorGraph graph(newFactors); HybridBayesNet updatedHybridBayesNet(hybridBayesNet); - KeySet factorKeys = graph.keys(); + KeySet involvedKeys = newFactors.keys(); + auto involved = [&involvedKeys](const Key &key) { + return involvedKeys.find(key) != involvedKeys.end(); + }; // If hybridBayesNet is not empty, // it means we have conditionals to add to the factor graph. @@ -167,12 +170,11 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, auto conditional = hybridBayesNet.at(i); for (auto &key : conditional->frontals()) { - if (std::find(factorKeys.begin(), factorKeys.end(), key) != - factorKeys.end()) { - // Add the conditional parents to factorKeys + if (involved(key)) { + // Add the conditional parents to involvedKeys // so we add those conditionals too. for (auto &&parentKey : conditional->parents()) { - factorKeys.insert(parentKey); + involvedKeys.insert(parentKey); } // Break so we don't add parents twice. break; @@ -180,15 +182,14 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, } } #ifdef DEBUG_SMOOTHER - PrintKeySet(factorKeys); + PrintKeySet(involvedKeys); #endif for (size_t i = 0; i < hybridBayesNet.size(); i++) { auto conditional = hybridBayesNet.at(i); for (auto &key : conditional->frontals()) { - if (std::find(factorKeys.begin(), factorKeys.end(), key) != - factorKeys.end()) { + if (involved(key)) { newConditionals.push_back(conditional); // Remove the conditional from the updated Bayes net @@ -218,4 +219,21 @@ const HybridBayesNet &HybridSmoother::hybridBayesNet() const { return hybridBayesNet_; } +/* ************************************************************************* */ +HybridValues HybridSmoother::optimize() const { + // Solve for the MPE + DiscreteValues mpe = hybridBayesNet_.mpe(); + + // Add fixed values to the MPE. + mpe.insert(fixedValues_); + + // Given the MPE, compute the optimal continuous values. + GaussianBayesNet gbn = hybridBayesNet_.choose(mpe); + const VectorValues continuous = gbn.optimize(); + if (std::find(gbn.begin(), gbn.end(), nullptr) != gbn.end()) { + throw std::runtime_error("At least one nullptr factor in hybridBayesNet_"); + } + return HybridValues(continuous, mpe); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 653df3957..12da3b0af 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -108,16 +108,7 @@ class GTSAM_EXPORT HybridSmoother { const HybridBayesNet& hybridBayesNet() const; /// Optimize the hybrid Bayes Net, taking into accound fixed values. - HybridValues optimize() const { - // Solve for the MPE - DiscreteValues mpe = hybridBayesNet_.mpe(); - - // Add fixed values to the MPE. - mpe.insert(fixedValues_); - - // Given the MPE, compute the optimal continuous values. - return HybridValues(hybridBayesNet_.optimize(mpe), mpe); - } + HybridValues optimize() const; }; } // namespace gtsam From 39e46100778fbe966a637b1c08ff59930f9e02b8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 10:57:56 -0500 Subject: [PATCH 24/30] Small fixes, 10 hypotheses --- examples/Hybrid_City10000.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index 8136b255d..86f32d303 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -44,6 +44,7 @@ using symbol_shorthand::M; using symbol_shorthand::X; const size_t kMaxLoopCount = 2000; // Example default value +const size_t kMaxNrHypotheses = 10; auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); @@ -122,9 +123,14 @@ class Experiment { /// @brief Perform smoother update and optimize the graph. void smootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, const Values& initial, - size_t maxNrHypotheses, Values* result) { + size_t kMaxNrHypotheses, Values* result) { HybridGaussianFactorGraph linearized = *graph.linearize(initial); - smoother.update(linearized, maxNrHypotheses); + 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())); @@ -147,13 +153,9 @@ class Experiment { // Initialize local variables size_t discreteCount = 0, index = 0; size_t loopCount = 0; - size_t nrHybridFactors = 0; // for demonstration; never incremented below 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; @@ -165,7 +167,7 @@ class Experiment { // Initial update clock_t beforeUpdate = clock(); - smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); + smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); clock_t afterUpdate = clock(); std::vector> smootherUpdateTimes; smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); @@ -226,7 +228,7 @@ class Experiment { if (doSmootherUpdate) { gttic_(SmootherUpdate); beforeUpdate = clock(); - smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); + smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); afterUpdate = clock(); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); gttoc_(SmootherUpdate); @@ -256,14 +258,15 @@ class Experiment { // Final update beforeUpdate = clock(); - smootherUpdate(smoother_, graph_, initial_, maxNrHypotheses, &result_); + smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); afterUpdate = clock(); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); // Final optimize gttic_(HybridSmootherOptimize); - HybridValues delta = smoother_.hybridBayesNet().optimize(); + HybridValues delta = smoother_.optimize(); gttoc_(HybridSmootherOptimize); + result_.insert_or_assign(initial_.retract(delta.continuous())); std::cout << "Final error: " << smoother_.hybridBayesNet().error(delta) @@ -293,9 +296,6 @@ class Experiment { } outfileTime.close(); std::cout << "Output " << timeFileName << " file." << std::endl; - - // Just to show usage of nrHybridFactors - std::cout << nrHybridFactors << std::endl; } }; From c7864d32b58cfc178fae96a3ccfe0795118f6d99 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 15:54:11 -0500 Subject: [PATCH 25/30] New scalar operators --- gtsam/discrete/DecisionTreeFactor.h | 9 +++++++++ gtsam/discrete/DiscreteFactor.cpp | 5 +---- gtsam/discrete/DiscreteFactor.h | 6 ++++++ gtsam/discrete/TableDistribution.cpp | 5 +++++ gtsam/discrete/TableDistribution.h | 7 +++++++ gtsam/discrete/TableFactor.cpp | 30 ++++++++++++++++++++++++++++ gtsam/discrete/TableFactor.h | 25 ++++++++++++----------- 7 files changed, 71 insertions(+), 16 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 4da5a7c17..716c43b63 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -164,6 +164,12 @@ namespace gtsam { virtual DiscreteFactor::shared_ptr multiply( const DiscreteFactor::shared_ptr& f) const override; + /// multiply with a scalar + DiscreteFactor::shared_ptr operator*(double s) const override { + return std::make_shared( + apply([s](const double& a) { return Ring::mul(a, s); })); + } + /// multiply two factors DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { return apply(f, Ring::mul); @@ -201,6 +207,9 @@ namespace gtsam { return combine(keys, Ring::add); } + /// Find the maximum value in the factor. + double max() const override { return ADT::max(); }; + /// Create new factor by maximizing over all values with the same separator. DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { return combine(nrFrontals, Ring::max); diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index faae02af2..61b4c135c 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -73,10 +73,7 @@ AlgebraicDecisionTree DiscreteFactor::errorTree() const { /* ************************************************************************ */ DiscreteFactor::shared_ptr DiscreteFactor::scale() const { - // Max over all the potentials by pretending all keys are frontal: - shared_ptr denominator = this->max(this->size()); - // Normalize the product factor to prevent underflow. - return this->operator/(denominator); + return this->operator*(1.0 / max()); } } // namespace gtsam diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index fafb4dbf5..6fa074379 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -126,6 +126,9 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// Compute error for each assignment and return as a tree virtual AlgebraicDecisionTree errorTree() const; + /// Multiply with a scalar + virtual DiscreteFactor::shared_ptr operator*(double s) const = 0; + /// Multiply in a DecisionTreeFactor and return the result as /// DecisionTreeFactor virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; @@ -152,6 +155,9 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// Create new factor by summing all values with the same separator values virtual DiscreteFactor::shared_ptr sum(const Ordering& keys) const = 0; + /// Find the maximum value in the factor. + virtual double max() const = 0; + /// Create new factor by maximizing over all values with the same separator. virtual DiscreteFactor::shared_ptr max(size_t nrFrontals) const = 0; diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp index e8696c5b1..ce0d92bff 100644 --- a/gtsam/discrete/TableDistribution.cpp +++ b/gtsam/discrete/TableDistribution.cpp @@ -110,6 +110,11 @@ DiscreteFactor::shared_ptr TableDistribution::max(const Ordering& keys) const { return table_.max(keys); } +/* ****************************************************************************/ +DiscreteFactor::shared_ptr TableDistribution::operator*(double s) const { + return table_ * s; +} + /* ****************************************************************************/ DiscreteFactor::shared_ptr TableDistribution::operator/( const DiscreteFactor::shared_ptr& f) const { diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h index 72786a515..8e28bed5f 100644 --- a/gtsam/discrete/TableDistribution.h +++ b/gtsam/discrete/TableDistribution.h @@ -116,12 +116,19 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional { /// Create new factor by summing all values with the same separator values DiscreteFactor::shared_ptr sum(const Ordering& keys) const override; + /// Find the maximum value in the factor. + double max() const override { return table_.max(); } + /// Create new factor by maximizing over all values with the same separator. DiscreteFactor::shared_ptr max(size_t nrFrontals) const override; /// Create new factor by maximizing over all values with the same separator. DiscreteFactor::shared_ptr max(const Ordering& keys) const override; + + /// Multiply by scalar s + DiscreteFactor::shared_ptr operator*(double s) const override; + /// divide by DiscreteFactor::shared_ptr f (safely) DiscreteFactor::shared_ptr operator/( const DiscreteFactor::shared_ptr& f) const override; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index d1cedc9ef..25acae06e 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -389,6 +389,36 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const { cout << "number of nnzs: " << sparse_table_.nonZeros() << endl; } +/* ************************************************************************ */ +DiscreteFactor::shared_ptr TableFactor::sum(size_t nrFrontals) const { +return combine(nrFrontals, Ring::add); +} + +/* ************************************************************************ */ +DiscreteFactor::shared_ptr TableFactor::sum(const Ordering& keys) const { +return combine(keys, Ring::add); +} + +/* ************************************************************************ */ +double TableFactor::max() const { + double max_value = std::numeric_limits::lowest(); + for (Eigen::SparseVector::InnerIterator it(sparse_table_); it; ++it) { + max_value = std::max(max_value, it.value()); + } + return max_value; +} + +/* ************************************************************************ */ +DiscreteFactor::shared_ptr TableFactor::max(size_t nrFrontals) const { + return combine(nrFrontals, Ring::max); +} + +/* ************************************************************************ */ +DiscreteFactor::shared_ptr TableFactor::max(const Ordering& keys) const { + return combine(keys, Ring::max); +} + + /* ************************************************************************ */ TableFactor TableFactor::apply(Unary op) const { // Initialize new factor. diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 1cb9eda8b..ce58d14bc 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -171,6 +171,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; + /// multiply with a scalar + DiscreteFactor::shared_ptr operator*(double s) const override { + return std::make_shared( + apply([s](const double& a) { return Ring::mul(a, s); })); + } + /// multiply two TableFactors TableFactor operator*(const TableFactor& f) const { return apply(f, Ring::mul); @@ -215,24 +221,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { DiscreteKeys parent_keys) const; /// Create new factor by summing all values with the same separator values - DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override { - return combine(nrFrontals, Ring::add); - } + DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override; /// Create new factor by summing all values with the same separator values - DiscreteFactor::shared_ptr sum(const Ordering& keys) const override { - return combine(keys, Ring::add); - } + DiscreteFactor::shared_ptr sum(const Ordering& keys) const override; + + /// Find the maximum value in the factor. + double max() const override; /// Create new factor by maximizing over all values with the same separator. - DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { - return combine(nrFrontals, Ring::max); - } + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override; /// Create new factor by maximizing over all values with the same separator. - DiscreteFactor::shared_ptr max(const Ordering& keys) const override { - return combine(keys, Ring::max); - } + DiscreteFactor::shared_ptr max(const Ordering& keys) const override; /// @} /// @name Advanced Interface From 16b2710c3f7919ab9c6e3f302fe76420ce838076 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 15:55:33 -0500 Subject: [PATCH 26/30] Refactor scaledProduct --- gtsam/discrete/DiscreteFactorGraph.cpp | 35 +++++--------------------- 1 file changed, 6 insertions(+), 29 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 6f1f2d592..aec239d83 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -65,15 +65,10 @@ namespace gtsam { /* ************************************************************************ */ DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const { - DiscreteFactor::shared_ptr result; - for (auto it = this->begin(); it != this->end(); ++it) { - if (*it) { - if (result) { - result = result->multiply(*it); - } else { - // Assign to the first non-null factor - result = *it; - } + DiscreteFactor::shared_ptr result = nullptr; + for (const auto& factor : *this) { + if (factor) { + result = result ? result->multiply(factor) : factor; } } return result; @@ -120,15 +115,7 @@ namespace gtsam { /* ************************************************************************ */ DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const { - // PRODUCT: multiply all factors - gttic(product); - DiscreteFactor::shared_ptr product = this->product(); - gttoc(product); - - // Normalize the product factor to prevent underflow. - product = product->scale(); - - return product; + return product()->scale(); } /* ************************************************************************ */ @@ -216,7 +203,7 @@ namespace gtsam { const Ordering& frontalKeys) { gttic(product); // `product` is scaled later to prevent underflow. - DiscreteFactor::shared_ptr product = factors.product(); + DiscreteFactor::shared_ptr product = factors.scaledProduct(); gttoc(product); // sum out frontals, this is the factor on the separator @@ -224,16 +211,6 @@ namespace gtsam { DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); gttoc(sum); - // Normalize/scale to prevent underflow. - // We divide both `product` and `sum` by `max(sum)` - // since it is faster to compute and when the conditional - // is formed by `product/sum`, the scaling term cancels out. - // gttic(scale); - // DiscreteFactor::shared_ptr denominator = sum->max(sum->size()); - // product = product->operator/(denominator); - // sum = sum->operator/(denominator); - // gttoc(scale); - // Ordering keys for the conditional so that frontalKeys are really in front Ordering orderedKeys; orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), From a24246ab632c446a4b75ae8ac119668698805677 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 16:55:50 -0500 Subject: [PATCH 27/30] Play with different elimination schemes --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/HybridSmoother.cpp | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index cfe5c4b59..b0bd17131 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -50,7 +50,7 @@ #include #include -#define GTSAM_HYBRID_WITH_TABLEFACTOR 0 +#define GTSAM_HYBRID_WITH_TABLEFACTOR 1 namespace gtsam { diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index b66b63ea6..51fd1f1db 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -26,7 +26,7 @@ namespace gtsam { /* ************************************************************************* */ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, - const KeySet &continuousKeys) { + const KeySet &lastKeysToEliminate) { // Get all the discrete keys from the factors KeySet allDiscrete = factors.discreteKeySet(); @@ -34,7 +34,7 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, KeyVector lastKeys; // Insert continuous keys first. - for (auto &k : continuousKeys) { + for (auto &k : lastKeysToEliminate) { if (!allDiscrete.exists(k)) { lastKeys.push_back(k); } @@ -44,11 +44,10 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, std::copy(allDiscrete.begin(), allDiscrete.end(), std::back_inserter(lastKeys)); - const VariableIndex index(factors); - // Get an ordering where the new keys are eliminated last Ordering ordering = Ordering::ColamdConstrainedLast( - index, KeyVector(lastKeys.begin(), lastKeys.end()), true); + factors, KeyVector(lastKeys.begin(), lastKeys.end()), true); + return ordering; } @@ -79,7 +78,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, // If no ordering provided, then we compute one if (!given_ordering.has_value()) { // Get the keys from the new factors - const KeySet continuousKeysToInclude;// = newFactors.keys(); + KeySet continuousKeysToInclude; // Scheme 1: empty, 15sec/2000, 64sec/3000 (69s without TF) + // continuousKeysToInclude = newFactors.keys(); // Scheme 2: all, 8sec/2000, 160sec/3000 + // continuousKeysToInclude = updatedGraph.keys(); // Scheme 3: all, stopped after 80sec/2000 // Since updatedGraph now has all the connected conditionals, // we can get the correct ordering. From 2b6545233a626d802fb30eaacb0cccc3f3978334 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 21:13:40 -0500 Subject: [PATCH 28/30] Fix constraints --- gtsam/discrete/DiscreteFactor.h | 5 +---- gtsam_unstable/discrete/AllDiff.h | 5 +++++ gtsam_unstable/discrete/BinaryAllDiff.h | 5 +++++ gtsam_unstable/discrete/Constraint.h | 16 +++++++++++++++- gtsam_unstable/discrete/Domain.h | 5 +++++ gtsam_unstable/discrete/SingleValue.h | 5 +++++ 6 files changed, 36 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 6fa074379..62f1e064b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -136,9 +136,6 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /** * @brief Multiply in a DiscreteFactor and return the result as * DiscreteFactor, both via shared pointers. - * - * @param df DiscreteFactor shared_ptr - * @return DiscreteFactor::shared_ptr */ virtual DiscreteFactor::shared_ptr multiply( const DiscreteFactor::shared_ptr& df) const = 0; @@ -170,7 +167,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { * * @return DiscreteFactor::shared_ptr */ - DiscreteFactor::shared_ptr scale() const; + virtual DiscreteFactor::shared_ptr scale() const; /** * Get the number of non-zero values contained in this factor. diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 1180abad4..67a6ffd18 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -72,6 +72,11 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( const Domains&) const override; + + // Scale just returns the same constraint. + DiscreteFactor::shared_ptr scale() const override { + return std::make_shared(*this); + } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index e96bfdfde..21d5f1642 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -96,6 +96,11 @@ class BinaryAllDiff : public Constraint { AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("BinaryAllDiff::error not implemented"); } + + // Scale just returns the same constraint. + DiscreteFactor::shared_ptr scale() const override { + return std::make_shared(*this); + } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 328fabbea..91bcdbc20 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -29,7 +29,9 @@ class Domain; using Domains = std::map; /** - * Base class for constraint factors + * Base class for constraint factors. + * This class is used to represent constraints on discrete variables. + * The values are always either 0 or 1, with at least one 1. * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. */ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { @@ -80,6 +82,16 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { /// Partially apply known values, domain version virtual shared_ptr partiallyApply(const Domains&) const = 0; + /// Multiply in a DecisionTreeFactor. + DecisionTreeFactor operator*(const DecisionTreeFactor& df) const override { + throw std::logic_error("Constraint::operator* not implemented"); + } + + /// Multiply with scalar just returns the constraint. + DiscreteFactor::shared_ptr operator*(double /* s*/) const override { + throw std::logic_error("Constraint::operator* not implemented"); + } + /// Multiply factors, DiscreteFactor::shared_ptr edition DiscreteFactor::shared_ptr multiply( const DiscreteFactor::shared_ptr& df) const override { @@ -104,6 +116,8 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { return toDecisionTreeFactor().sum(keys); } + double max() const override { return 1.0; } + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { return toDecisionTreeFactor().max(nrFrontals); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 6ce846201..b45b1f23e 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -114,6 +114,11 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply(const Domains& domains) const override; + + // Scale just returns the same constraint. + DiscreteFactor::shared_ptr scale() const override { + return std::make_shared(*this); + } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 3df1209b8..152a8f51b 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -77,6 +77,11 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( const Domains& domains) const override; + + // Scale just returns the same constraint. + DiscreteFactor::shared_ptr scale() const override { + return std::make_shared(*this); + } }; } // namespace gtsam From 0d5b03d609dd04f2bc4ccfadeed8750103e889d7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Jan 2025 22:10:50 -0500 Subject: [PATCH 29/30] Revert "Fix constraints" This reverts commit 2b6545233a626d802fb30eaacb0cccc3f3978334. --- gtsam/discrete/DiscreteFactor.h | 5 ++++- gtsam_unstable/discrete/AllDiff.h | 5 ----- gtsam_unstable/discrete/BinaryAllDiff.h | 5 ----- gtsam_unstable/discrete/Constraint.h | 16 +--------------- gtsam_unstable/discrete/Domain.h | 5 ----- gtsam_unstable/discrete/SingleValue.h | 5 ----- 6 files changed, 5 insertions(+), 36 deletions(-) diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 62f1e064b..6fa074379 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -136,6 +136,9 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /** * @brief Multiply in a DiscreteFactor and return the result as * DiscreteFactor, both via shared pointers. + * + * @param df DiscreteFactor shared_ptr + * @return DiscreteFactor::shared_ptr */ virtual DiscreteFactor::shared_ptr multiply( const DiscreteFactor::shared_ptr& df) const = 0; @@ -167,7 +170,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { * * @return DiscreteFactor::shared_ptr */ - virtual DiscreteFactor::shared_ptr scale() const; + DiscreteFactor::shared_ptr scale() const; /** * Get the number of non-zero values contained in this factor. diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 67a6ffd18..1180abad4 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -72,11 +72,6 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( const Domains&) const override; - - // Scale just returns the same constraint. - DiscreteFactor::shared_ptr scale() const override { - return std::make_shared(*this); - } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 21d5f1642..e96bfdfde 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -96,11 +96,6 @@ class BinaryAllDiff : public Constraint { AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("BinaryAllDiff::error not implemented"); } - - // Scale just returns the same constraint. - DiscreteFactor::shared_ptr scale() const override { - return std::make_shared(*this); - } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 91bcdbc20..328fabbea 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -29,9 +29,7 @@ class Domain; using Domains = std::map; /** - * Base class for constraint factors. - * This class is used to represent constraints on discrete variables. - * The values are always either 0 or 1, with at least one 1. + * Base class for constraint factors * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. */ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { @@ -82,16 +80,6 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { /// Partially apply known values, domain version virtual shared_ptr partiallyApply(const Domains&) const = 0; - /// Multiply in a DecisionTreeFactor. - DecisionTreeFactor operator*(const DecisionTreeFactor& df) const override { - throw std::logic_error("Constraint::operator* not implemented"); - } - - /// Multiply with scalar just returns the constraint. - DiscreteFactor::shared_ptr operator*(double /* s*/) const override { - throw std::logic_error("Constraint::operator* not implemented"); - } - /// Multiply factors, DiscreteFactor::shared_ptr edition DiscreteFactor::shared_ptr multiply( const DiscreteFactor::shared_ptr& df) const override { @@ -116,8 +104,6 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { return toDecisionTreeFactor().sum(keys); } - double max() const override { return 1.0; } - DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { return toDecisionTreeFactor().max(nrFrontals); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index b45b1f23e..6ce846201 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -114,11 +114,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply(const Domains& domains) const override; - - // Scale just returns the same constraint. - DiscreteFactor::shared_ptr scale() const override { - return std::make_shared(*this); - } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 152a8f51b..3df1209b8 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -77,11 +77,6 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( const Domains& domains) const override; - - // Scale just returns the same constraint. - DiscreteFactor::shared_ptr scale() const override { - return std::make_shared(*this); - } }; } // namespace gtsam From 64aa0520edd7cb7b7bb58e976bc77516d3312321 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Jan 2025 21:52:49 -0500 Subject: [PATCH 30/30] implement DiscreteFactor methods in Constraint --- gtsam_unstable/discrete/Constraint.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 328fabbea..637cf404e 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -87,6 +87,16 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { this->operator*(df->toDecisionTreeFactor())); } + /// Multiply by a scalar + virtual DiscreteFactor::shared_ptr operator*(double s) const override { + return this->toDecisionTreeFactor() * s; + } + + /// Multiply by a DecisionTreeFactor and return a DecisionTreeFactor + DecisionTreeFactor operator*(const DecisionTreeFactor& dtf) const override { + return this->toDecisionTreeFactor() * dtf; + } + /// divide by DiscreteFactor::shared_ptr f (safely) DiscreteFactor::shared_ptr operator/( const DiscreteFactor::shared_ptr& df) const override { @@ -104,6 +114,9 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { return toDecisionTreeFactor().sum(keys); } + /// Find the max value + double max() const override { return toDecisionTreeFactor().max(); } + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { return toDecisionTreeFactor().max(nrFrontals); }