optionally provide ordering for HybridSmoother::relinearize
parent
d01bfba763
commit
bbceb7a305
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue