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

add new methods for HybridSmoother relinearization

release/4.3a0
Varun Agrawal 2025-03-17 16:51:06 -04:00
parent d27583ba92
commit d2f13710d5
3 changed files with 26 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/hybrid/HybridSmoother.cpp
View File

@ -291,4 +291,20 @@ HybridValues HybridSmoother::optimize() const {
return HybridValues(continuous, mpe); return HybridValues(continuous, mpe);
} }
/* ************************************************************************* */
void HybridSmoother::relinearize() {
allFactors_ = allFactors_.restrict(fixedValues_);
HybridGaussianFactorGraph::shared_ptr linearized =
allFactors_.linearize(linearizationPoint_);
HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential();
HybridValues delta = bayesNet->optimize();
linearizationPoint_ = linearizationPoint_.retract(delta.continuous());
reInitialize(*bayesNet);
}
/* ************************************************************************* */
Values HybridSmoother::linearizationPoint() const {
return linearizationPoint_;
}
} // namespace gtsam } // namespace gtsam

15
gtsam/hybrid/HybridSmoother.h
View File

@ -126,15 +126,12 @@ class GTSAM_EXPORT HybridSmoother {
/// Optimize the hybrid Bayes Net, taking into accound fixed values. /// Optimize the hybrid Bayes Net, taking into accound fixed values.
HybridValues optimize() const; HybridValues optimize() const;
void relinearize() { /// Relinearize the nonlinear factor graph
allFactors_ = allFactors_.restrict(fixedValues_); /// with the latest linearization point.
HybridGaussianFactorGraph::shared_ptr linearized = void relinearize();
allFactors_.linearize(linearizationPoint_);
HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential(); /// Return the current linearization point.
HybridValues delta = bayesNet->optimize(); Values linearizationPoint() const;
linearizationPoint_ = linearizationPoint_.retract(delta.continuous());
reInitialize(*bayesNet);
}
private: private:
/// Helper to compute the ordering if ordering is not given. /// Helper to compute the ordering if ordering is not given.

5
gtsam/hybrid/hybrid.i
View File

@ -283,12 +283,15 @@ class HybridSmoother {
void reInitialize(gtsam::HybridBayesNet& hybridBayesNet); void reInitialize(gtsam::HybridBayesNet& hybridBayesNet);
void update( void update(
const gtsam::HybridNonlinearFactorGraph& graph, const Values& initial, const gtsam::HybridNonlinearFactorGraph& graph,
const gtsam::Values& initial,
std::optional<size_t> maxNrLeaves = std::nullopt, std::optional<size_t> maxNrLeaves = std::nullopt,
const std::optional<gtsam::Ordering> given_ordering = std::nullopt); const std::optional<gtsam::Ordering> given_ordering = std::nullopt);
void relinearize(); void relinearize();
gtsam::Values linearizationPoint() const;
gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors, gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors,
const gtsam::KeySet& newFactorKeys); const gtsam::KeySet& newFactorKeys);