HybridConditional.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/DiscreteConditional.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianConditional.h>
 
#include <memory>
#include <stdexcept>
#include <string>
#include <typeinfo>
#include <vector>
 
namespace gtsam {

class GTSAM_EXPORT HybridConditional
    : public HybridFactor,
      public Conditional<HybridFactor, HybridConditional> {
 public:
  // typedefs needed to play nice with gtsam
  typedef HybridConditional This;            
  typedef std::shared_ptr<This> shared_ptr;  
  typedef HybridFactor BaseFactor;  
  typedef Conditional<BaseFactor, This>
      BaseConditional;  
 
 protected:
  std::shared_ptr<Factor> inner_;
 
 public:

  HybridConditional() = default;

  HybridConditional(const KeyVector& continuousKeys,
                    const DiscreteKeys& discreteKeys, size_t nFrontals)
      : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}

  HybridConditional(const KeyVector& continuousFrontals,
                    const DiscreteKeys& discreteFrontals,
                    const KeyVector& continuousParents,
                    const DiscreteKeys& discreteParents);

  HybridConditional(
      const std::shared_ptr<GaussianConditional>& continuousConditional);

  HybridConditional(
      const std::shared_ptr<DiscreteConditional>& discreteConditional);

  HybridConditional(
      const std::shared_ptr<HybridGaussianConditional>& hybridGaussianCond);


  void print(
      const std::string& s = "Hybrid Conditional: ",
      const KeyFormatter& formatter = DefaultKeyFormatter) const override;

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


  HybridGaussianConditional::shared_ptr asHybrid() const {
    if (!isHybrid()) return nullptr;
    return std::static_pointer_cast<HybridGaussianConditional>(inner_);
  }

  GaussianConditional::shared_ptr asGaussian() const {
    if (!isContinuous()) return nullptr;
    return std::static_pointer_cast<GaussianConditional>(inner_);
  }

  template <typename T = DiscreteConditional>
  typename T::shared_ptr asDiscrete() const {
    if (!isDiscrete()) return nullptr;
    return std::static_pointer_cast<T>(inner_);
  }

  std::shared_ptr<Factor> inner() const { return inner_; }

  double error(const HybridValues& hybridValues) const override;

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

  double logProbability(const HybridValues& values) const override;

  double negLogConstant() const override;

  double evaluate(const HybridValues& values) const override;

  bool frontalsIn(const VectorValues& measurements) const {
    for (Key key : frontals()) {
      if (!measurements.exists(key)) {
        return false;
      }
    }
    return true;
  }

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

 
 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(BaseFactor);
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
    ar& BOOST_SERIALIZATION_NVP(inner_);
 
    // register the various casts based on the type of inner_
    // https://www.boost.org/doc/libs/1_80_0/libs/serialization/doc/serialization.html#runtimecasting
    if (isDiscrete()) {
      boost::serialization::void_cast_register<DiscreteConditional, Factor>(
          static_cast<DiscreteConditional*>(NULL), static_cast<Factor*>(NULL));
    } else if (isContinuous()) {
      boost::serialization::void_cast_register<GaussianConditional, Factor>(
          static_cast<GaussianConditional*>(NULL), static_cast<Factor*>(NULL));
    } else {
      boost::serialization::void_cast_register<HybridGaussianConditional,
                                               Factor>(
          static_cast<HybridGaussianConditional*>(NULL),
          static_cast<Factor*>(NULL));
    }
  }
#endif
 
};  // HybridConditional
 
// traits
template <>
struct traits<HybridConditional> : public Testable<HybridConditional> {};
 
}  // namespace gtsam