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 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;
  // bool equals(const This& fg, double tol = 1e-9) const override;


  using Base::error; // Expose error(const HybridValues&) method..

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

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

  double probPrime(const HybridValues& values) const;

  GaussianFactorGraphTree assembleGraphTree() const;

};

}  // namespace gtsam