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