HybridGaussianFactor.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/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
 
namespace gtsam {
 
class HybridValues;
class DiscreteValues;
class VectorValues;

using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;

class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 public:
  using Base = HybridFactor;
  using This = HybridGaussianFactor;
  using shared_ptr = std::shared_ptr<This>;
 
  using sharedFactor = std::shared_ptr<GaussianFactor>;

  using FactorValuePairs = DecisionTree<Key, GaussianFactorValuePair>;
 
 private:
  FactorValuePairs factors_;
 
 public:

  HybridGaussianFactor() = default;

  HybridGaussianFactor(const DiscreteKey &discreteKey,
                       const std::vector<GaussianFactor::shared_ptr> &factors);

  HybridGaussianFactor(const DiscreteKey &discreteKey,
                       const std::vector<GaussianFactorValuePair> &factorPairs);

  HybridGaussianFactor(const DiscreteKeys &discreteKeys,
                       const FactorValuePairs &factorPairs);

 
  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
  void print(const std::string &s = "", const KeyFormatter &formatter =
                                            DefaultKeyFormatter) const override;


  GaussianFactorValuePair operator()(const DiscreteValues &assignment) const;

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

  double error(const HybridValues &hybridValues) const override;

  const FactorValuePairs &factors() const { return factors_; }

  virtual HybridGaussianProductFactor asProductFactor() const;

  std::shared_ptr<Factor> restrict(
      const DiscreteValues &discreteValues) const override;

 
 private:
  static FactorValuePairs augment(const FactorValuePairs &factors);

  struct ConstructorHelper;
 
  // Private constructor using ConstructorHelper above.
  HybridGaussianFactor(const ConstructorHelper &helper);
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    ar &BOOST_SERIALIZATION_NVP(factors_);
  }
#endif
};
 
// traits
template <>
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
 
}  // namespace gtsam