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/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
 
#include <functional>
#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 DERIVEDFACTOR>
  HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
      : Base(graph) {}
 
 
  // TODO(dellaert):  customize print and equals.
  // 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 std::function<bool(const Factor* /*factor*/,
                               double /*whitenedError*/, size_t /*index*/)>&
          printCondition =
              [](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;
 
  AlgebraicDecisionTree<Key> probPrime(
      const VectorValues& continuousValues) const;
 
  double probPrime(const HybridValues& values) const;
 
  GaussianFactorGraphTree assembleGraphTree() const;
 
};
 
}  // namespace gtsam