HybridSmoother.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
 
#include <optional>
 
namespace gtsam {
 
class GTSAM_EXPORT HybridSmoother {
 private:
  HybridNonlinearFactorGraph allFactors_;
  Values linearizationPoint_;
 
  HybridBayesNet hybridBayesNet_;
  std::optional<double> marginalThreshold_;
  DiscreteValues fixedValues_;
 
 public:
  HybridSmoother(const std::optional<double> marginalThreshold = {})
      : marginalThreshold_(marginalThreshold) {}
 
  const DiscreteValues& fixedValues() const { return fixedValues_; }
 
  void reInitialize(HybridBayesNet&& hybridBayesNet);
 
  void reInitialize(HybridBayesNet& hybridBayesNet);
 
  void update(const HybridNonlinearFactorGraph& graph, const Values& initial,
              std::optional<size_t> maxNrLeaves = {},
              const std::optional<Ordering> givenOrdering = {});
 
  Ordering getOrdering(const HybridGaussianFactorGraph& factors,
                       const KeySet& newFactorKeys);
 
  std::pair<HybridGaussianFactorGraph, HybridBayesNet> addConditionals(
      const HybridGaussianFactorGraph& graph,
      const HybridBayesNet& hybridBayesNet) const;
 
  HybridGaussianConditional::shared_ptr gaussianMixture(size_t index) const;
 
  const HybridBayesNet& hybridBayesNet() const;
 
  HybridValues optimize() const;
 
  void relinearize(const std::optional<Ordering> givenOrdering = {});
 
  Values linearizationPoint() const;
 
  HybridNonlinearFactorGraph allFactors() const;
 
 private:
  Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph,
                                const std::optional<Ordering> givenOrdering);
 
  void removeFixedValues(const HybridGaussianFactorGraph& newFactors);
};
 
}  // namespace gtsam