12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update the HybridCity10000 script

release/4.3a0
Varun Agrawal 2025-03-17 16:55:34 -04:00
parent d2f13710d5
commit e1f5a5155a
4 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/hybrid/HybridSmoother.cpp
View File

@ -307,4 +307,9 @@ Values HybridSmoother::linearizationPoint() const {
return linearizationPoint_;
}
/* ************************************************************************* */
HybridNonlinearFactorGraph HybridSmoother::allFactors() const {
return allFactors_;
}
} // namespace gtsam

3
gtsam/hybrid/HybridSmoother.h
View File

@ -133,6 +133,9 @@ class GTSAM_EXPORT HybridSmoother {
/// Return the current linearization point.
Values linearizationPoint() const;
/// Return all the recorded nonlinear factors
HybridNonlinearFactorGraph allFactors() const;
private:
/// Helper to compute the ordering if ordering is not given.
Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph,

1
gtsam/hybrid/hybrid.i
View File

@ -291,6 +291,7 @@ class HybridSmoother {
void relinearize();
gtsam::Values linearizationPoint() const;
gtsam::HybridNonlinearFactorGraph allFactors() const;
gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors,
const gtsam::KeySet& newFactorKeys);

17
python/gtsam/examples/HybridCity10000.py
View File

@ -194,7 +194,6 @@ class Experiment:
self.smoother_ = HybridSmoother(marginal_threshold)
self.new_factors_ = HybridNonlinearFactorGraph()
self.all_factors_ = HybridNonlinearFactorGraph()
self.initial_ = Values()
self.plot_hypotheses = plot_hypotheses
@ -231,24 +230,18 @@ class Experiment:
"""Perform smoother update and optimize the graph."""
print(f"Smoother update: {self.new_factors_.size()}")
before_update = time.time()
linearized = self.new_factors_.linearize(self.initial_)
self.smoother_.update(linearized, max_num_hypotheses)
self.all_factors_.push_back(self.new_factors_)
self.smoother_.update(self.new_factors_, self.initial_,
max_num_hypotheses)
self.new_factors_.resize(0)
after_update = time.time()
return after_update - before_update
def reinitialize(self) -> float:
"""Re-linearize, solve ALL, and re-initialize smoother."""
print(f"================= Re-Initialize: {self.all_factors_.size()}")
print(f"================= Re-Initialize: {self.smoother_.allFactors().size()}")
before_update = time.time()
self.all_factors_ = self.all_factors_.restrict(
self.smoother_.fixedValues())
linearized = self.all_factors_.linearize(self.initial_)
bayesNet = linearized.eliminateSequential()
delta: HybridValues = bayesNet.optimize()
self.initial_ = self.initial_.retract(delta.continuous())
self.smoother_.reInitialize(bayesNet)
self.smoother_.relinearize()
self.initial_ = self.smoother_.linearizationPoint()
after_update = time.time()
print(f"Took {after_update - before_update} seconds.")
return after_update - before_update