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

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_);
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);
}

View File

@ -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<Ordering> givenOrdering = {});
/// Return the current linearization point.
Values linearizationPoint() const;

View File

@ -288,7 +288,7 @@ class HybridSmoother {
std::optional<size_t> maxNrLeaves = 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::HybridNonlinearFactorGraph allFactors() const;