From 47d370903ced65848d4245eb3e744e68ff2f5c08 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Mar 2025 09:45:16 -0400 Subject: [PATCH 1/5] correctly fix script and record timing information --- examples/Hybrid_City10000.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index fc5d55d44..f37b65ff8 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -109,11 +109,10 @@ class Experiment { std::cout << "Smoother update: " << newFactors_.size() << std::endl; gttic_(SmootherUpdate); clock_t beforeUpdate = clock(); - auto linearized = newFactors_.linearize(initial_); - smoother_.update(*linearized, initial_); + smoother_.update(newFactors_, initial_, maxNrHypotheses); + clock_t afterUpdate = clock(); allFactors_.push_back(newFactors_); newFactors_.resize(0); - clock_t afterUpdate = clock(); return afterUpdate - beforeUpdate; } @@ -262,10 +261,20 @@ class Experiment { std::string timeFileName = "Hybrid_City10000_time.txt"; outfileTime.open(timeFileName); for (auto accTime : timeList) { - outfileTime << accTime << std::endl; + outfileTime << accTime / CLOCKS_PER_SEC << std::endl; } outfileTime.close(); std::cout << "Output " << timeFileName << " file." << std::endl; + + std::ofstream timingFile; + std::string timingFileName = "Hybrid_City10000_timing.txt"; + timingFile.open(timingFileName); + for (size_t i = 0; i < smootherUpdateTimes.size(); i++) { + auto p = smootherUpdateTimes.at(i); + timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl; + } + timingFile.close(); + std::cout << "Wrote timing information to " << timingFileName << std::endl; } }; From 5c2e8d6746f18f3439612517ced7507b470a13ee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Mar 2025 09:45:34 -0400 Subject: [PATCH 2/5] record timing information --- examples/ISAM2_City10000.cpp | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index 2c210ab44..526883b82 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -74,6 +74,8 @@ class Experiment { // Initialize local variables size_t index = 0; + std::vector> smootherUpdateTimes; + std::list timeList; // Set up initial prior @@ -82,10 +84,15 @@ class Experiment { graph_.addPrior(X(0), priorPose, kPriorNoiseModel); // Initial update + clock_t beforeUpdate = clock(); isam2_.update(graph_, initial_); + results = isam2_.calculateBestEstimate(); + clock_t afterUpdate = clock(); + smootherUpdateTimes.push_back( + std::make_pair(index, afterUpdate - beforeUpdate)); graph_.resize(0); initial_.clear(); - results = isam2_.calculateBestEstimate(); + index += 1; // Start main loop size_t keyS = 0; @@ -127,10 +134,15 @@ class Experiment { index++; } + clock_t beforeUpdate = clock(); isam2_.update(graph_, initial_); + results = isam2_.calculateBestEstimate(); + clock_t afterUpdate = clock(); + smootherUpdateTimes.push_back( + std::make_pair(index, afterUpdate - beforeUpdate)); graph_.resize(0); initial_.clear(); - results = isam2_.calculateBestEstimate(); + index += 1; // Print loop index and time taken in processor clock ticks if (index % 50 == 0 && keyS != keyT - 1) { @@ -175,6 +187,16 @@ class Experiment { outfileTime.close(); std::cout << "Written cumulative time to: " << timeFileName << " file." << std::endl; + + std::ofstream timingFile; + std::string timingFileName = "ISAM2_City10000_timing.txt"; + timingFile.open(timingFileName); + for (size_t i = 0; i < smootherUpdateTimes.size(); i++) { + auto p = smootherUpdateTimes.at(i); + timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl; + } + timingFile.close(); + std::cout << "Wrote timing information to " << timingFileName << std::endl; } }; From d01bfba7639c0f4e8122592812ae87c0711805d7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Mar 2025 09:47:29 -0400 Subject: [PATCH 3/5] implement DecisionTreeFactor::restrict --- gtsam/discrete/DecisionTreeFactor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index e39339dd8..ebc02c1b5 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -547,7 +547,9 @@ namespace gtsam { /* ************************************************************************ */ DiscreteFactor::shared_ptr DecisionTreeFactor::restrict( const DiscreteValues& assignment) const { - throw std::runtime_error("DecisionTreeFactor::restrict not implemented"); + ADT restricted_tree = ADT::restrict(assignment); + return std::make_shared(this->discreteKeys(), + restricted_tree); } /* ************************************************************************ */ From bbceb7a3057e1f41f706d81a1dd8df2329916b2f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Mar 2025 09:48:16 -0400 Subject: [PATCH 4/5] optionally provide ordering for HybridSmoother::relinearize --- gtsam/hybrid/HybridSmoother.cpp | 10 ++++++++-- gtsam/hybrid/HybridSmoother.h | 10 +++++++--- gtsam/hybrid/hybrid.i | 2 +- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 4beaf6474..1133f645e 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -292,13 +292,19 @@ HybridValues HybridSmoother::optimize() const { } /* ************************************************************************* */ -void HybridSmoother::relinearize() { +void HybridSmoother::relinearize(const std::optional givenOrdering) { allFactors_ = allFactors_.restrict(fixedValues_); HybridGaussianFactorGraph::shared_ptr linearized = allFactors_.linearize(linearizationPoint_); - HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential(); + + // Compute ordering if not given + Ordering ordering = this->maybeComputeOrdering(*linearized, givenOrdering); + + HybridBayesNet::shared_ptr bayesNet = + linearized->eliminateSequential(ordering); HybridValues delta = bayesNet->optimize(); linearizationPoint_ = linearizationPoint_.retract(delta.continuous()); + reInitialize(*bayesNet); } diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index b45327258..77b809a44 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -126,9 +126,13 @@ class GTSAM_EXPORT HybridSmoother { /// Optimize the hybrid Bayes Net, taking into accound fixed values. HybridValues optimize() const; - /// Relinearize the nonlinear factor graph - /// with the latest linearization point. - void relinearize(); + /** + * @brief Relinearize the nonlinear factor graph with + * the latest stored linearization point. + * + * @param givenOrdering An optional elimination ordering. + */ + void relinearize(const std::optional givenOrdering = {}); /// Return the current linearization point. Values linearizationPoint() const; diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index f76de20be..e231714cb 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -288,7 +288,7 @@ class HybridSmoother { std::optional maxNrLeaves = std::nullopt, const std::optional given_ordering = std::nullopt); - void relinearize(); + void relinearize(const std::optional givenOrdering); gtsam::Values linearizationPoint() const; gtsam::HybridNonlinearFactorGraph allFactors() const; From 2869a703d6e675c5a73fad97e58d8e4dcdfc5386 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Mar 2025 09:50:48 -0400 Subject: [PATCH 5/5] default argument for ordering in wrapper --- gtsam/hybrid/hybrid.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index e231714cb..a5902d769 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -288,7 +288,8 @@ class HybridSmoother { std::optional maxNrLeaves = std::nullopt, const std::optional given_ordering = std::nullopt); - void relinearize(const std::optional givenOrdering); + void relinearize( + const std::optional givenOrdering = std::nullopt); gtsam::Values linearizationPoint() const; gtsam::HybridNonlinearFactorGraph allFactors() const;