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

optionally provide ordering for HybridSmoother::relinearize

release/4.3a0
Varun Agrawal 2025-03-25 09:48:16 -04:00
parent d01bfba763
commit bbceb7a305
3 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/hybrid/HybridSmoother.cpp
View File

@ -292,13 +292,19 @@ HybridValues HybridSmoother::optimize() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HybridSmoother::relinearize() { void HybridSmoother::relinearize(const std::optional<Ordering> givenOrdering) {
allFactors_ = allFactors_.restrict(fixedValues_); allFactors_ = allFactors_.restrict(fixedValues_);
HybridGaussianFactorGraph::shared_ptr linearized = HybridGaussianFactorGraph::shared_ptr linearized =
allFactors_.linearize(linearizationPoint_); 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(); HybridValues delta = bayesNet->optimize();
linearizationPoint_ = linearizationPoint_.retract(delta.continuous()); linearizationPoint_ = linearizationPoint_.retract(delta.continuous());
reInitialize(*bayesNet); reInitialize(*bayesNet);
} }

10
gtsam/hybrid/HybridSmoother.h
View File

@ -126,9 +126,13 @@ 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;
/// Relinearize the nonlinear factor graph /**
/// with the latest linearization point. * @brief Relinearize the nonlinear factor graph with
void relinearize(); * the latest stored linearization point.
*
* @param givenOrdering An optional elimination ordering.
*/
void relinearize(const std::optional<Ordering> givenOrdering = {});
/// Return the current linearization point. /// Return the current linearization point.
Values linearizationPoint() const; Values linearizationPoint() const;

2
gtsam/hybrid/hybrid.i
View File

@ -288,7 +288,7 @@ class HybridSmoother {
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(const std::optional<gtsam::Ordering> givenOrdering);
gtsam::Values linearizationPoint() const; gtsam::Values linearizationPoint() const;
gtsam::HybridNonlinearFactorGraph allFactors() const; gtsam::HybridNonlinearFactorGraph allFactors() const;