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;

  double error(const HybridValues& x) const;

  double error(const VectorValues& x) const;
 
 private:
  Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph,
                                const std::optional<Ordering> givenOrdering);

  HybridGaussianFactorGraph removeFixedValues(
      const HybridGaussianFactorGraph& graph,
      const HybridGaussianFactorGraph& newFactors);
};
 
}  // namespace gtsam