HybridGaussianFactorGraph.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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorErrorPredicate.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
 
#include <optional>
 
namespace gtsam {
 
// Forward declarations
class HybridGaussianFactorGraph;
class HybridConditional;
class HybridBayesNet;
class HybridEliminationTree;
class HybridBayesTree;
class HybridJunctionTree;
class DecisionTreeFactor;
class TableFactor;
class JacobianFactor;
class HybridValues;

GTSAM_EXPORT
std::pair<std::shared_ptr<HybridConditional>, std::shared_ptr<Factor>>
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);

GTSAM_EXPORT const Ordering
HybridOrdering(const HybridGaussianFactorGraph& graph);
 
/* ************************************************************************* */
template <>
struct EliminationTraits<HybridGaussianFactorGraph> {
  typedef Factor FactorType;  
  typedef HybridGaussianFactorGraph
      FactorGraphType;  
  typedef HybridConditional
      ConditionalType;  
  typedef HybridBayesNet
      BayesNetType;  
  typedef HybridEliminationTree
      EliminationTreeType;                      
  typedef HybridBayesTree BayesTreeType;        
  typedef HybridJunctionTree JunctionTreeType;  
  static std::pair<std::shared_ptr<ConditionalType>,
                   std::shared_ptr<FactorType>>
  DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
    return EliminateHybrid(factors, keys);
  }

  static Ordering DefaultOrderingFunc(
      const FactorGraphType& graph,
      std::optional<std::reference_wrapper<const VariableIndex>>) {
    return HybridOrdering(graph);
  }
};

class GTSAM_EXPORT HybridGaussianFactorGraph
    : public HybridFactorGraph,
      public EliminateableFactorGraph<HybridGaussianFactorGraph> {
 protected:
  template <typename FACTOR>
  using IsGaussian = typename std::enable_if<
      std::is_base_of<GaussianFactor, FACTOR>::value>::type;
 
 public:
  using Base = HybridFactorGraph;
  using This = HybridGaussianFactorGraph;  
  using BaseEliminateable = EliminateableFactorGraph<This>;
  using shared_ptr = std::shared_ptr<This>;  
 
  using Values = gtsam::Values;  
  using Indices = KeyVector;     


  HybridGaussianFactorGraph() = default;

  template <class CONTAINER>
  explicit HybridGaussianFactorGraph(const CONTAINER& factors)
      : Base(factors) {}

  HybridGaussianFactorGraph(std::initializer_list<sharedFactor> factors)
      : Base(factors) {}

  template <class DERIVEDFACTOR>
  HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
      : Base(graph) {}

 
  void print(
      const std::string& s = "HybridGaussianFactorGraph",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

  void printErrors(
      const HybridValues& values,
      const std::string& str = "HybridGaussianFactorGraph: ",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter,
      const FactorErrorPredicate&
          printCondition = FactorErrorPredicate{
              [](const Factor*, double, size_t) { return true; }}) const;
 
  // bool equals(const This& fg, double tol = 1e-9) const override;


  using Base::error;

  AlgebraicDecisionTree<Key> errorTree(
      const VectorValues& continuousValues) const;

  double probPrime(const HybridValues& values) const;

  AlgebraicDecisionTree<Key> discretePosterior(
      const VectorValues& continuousValues) const;

  HybridGaussianProductFactor collectProductFactor() const;

  std::pair<std::shared_ptr<HybridConditional>, std::shared_ptr<Factor>>
  eliminate(const Ordering& keys) const;

  GaussianFactorGraph choose(const DiscreteValues& assignment) const;

  GaussianFactorGraph operator()(const DiscreteValues& assignment) const {
    return choose(assignment);
  }

  DiscreteFactorGraph discreteFactors() const;
};
 
// traits
template <>
struct traits<HybridGaussianFactorGraph>
    : public Testable<HybridGaussianFactorGraph> {};
 
}  // namespace gtsam