HybridFactor.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/base/Testable.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
 
#include <cstddef>
#include <string>
namespace gtsam {
 
class HybridValues;
 
KeyVector CollectKeys(const KeyVector &continuousKeys,
                      const DiscreteKeys &discreteKeys);
KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
                                 const DiscreteKeys &key2);

class GTSAM_EXPORT HybridFactor : public Factor {
 public:
  enum class Category { None, Discrete, Continuous, Hybrid };
 
 private:
  Category category_ = Category::None;
 
 protected:
  // Set of DiscreteKeys for this factor.
  DiscreteKeys discreteKeys_;
  KeyVector continuousKeys_;
 
 public:
  // typedefs needed to play nice with gtsam
  typedef HybridFactor This;  
  typedef std::shared_ptr<HybridFactor>
      shared_ptr;       
  typedef Factor Base;  


  HybridFactor() = default;

  explicit HybridFactor(const KeyVector &keys);

  explicit HybridFactor(const DiscreteKeys &discreteKeys);

  HybridFactor(const KeyVector &continuousKeys,
               const DiscreteKeys &discreteKeys);


  virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const;

  void print(
      const std::string &s = "HybridFactor\n",
      const KeyFormatter &formatter = DefaultKeyFormatter) const override;


  bool isDiscrete() const { return category_ == Category::Discrete; }

  bool isContinuous() const { return category_ == Category::Continuous; }

  bool isHybrid() const { return category_ == Category::Hybrid; }

  size_t nrContinuous() const { return continuousKeys_.size(); }

  const DiscreteKeys &discreteKeys() const { return discreteKeys_; }

  const KeyVector &continuousKeys() const { return continuousKeys_; }

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

  virtual std::shared_ptr<Factor> restrict(
      const DiscreteValues &discreteValues) const = 0;

 
 private:
#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(category_);
    ar &BOOST_SERIALIZATION_NVP(discreteKeys_);
    ar &BOOST_SERIALIZATION_NVP(continuousKeys_);
  }
#endif
};
// HybridFactor
 
// traits
template <>
struct traits<HybridFactor> : public Testable<HybridFactor> {};
 
// For wrapper:
using AlgebraicDecisionTreeKey = AlgebraicDecisionTree<Key>;
 
}  // namespace gtsam