Conditional.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
 
 * -------------------------------------------------------------------------- */
 
// \callgraph
#pragma once
 
#include <gtsam/inference/Key.h>
 
namespace gtsam {
 
  class HybridValues;  // forward declaration.
 
  template<class FACTOR, class DERIVEDCONDITIONAL>
  class Conditional
  {
  protected:
    size_t nrFrontals_;
 
  private:
    typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
 
 
  public:
    typedef std::pair<typename FACTOR::const_iterator, typename FACTOR::const_iterator> ConstFactorRange;
    struct ConstFactorRangeIterator {
      ConstFactorRange range_;
      // Delete default constructor
      ConstFactorRangeIterator() = delete;
      ConstFactorRangeIterator(ConstFactorRange const& x) : range_(x) {}
      // Implement begin and end for iteration
      typename FACTOR::const_iterator begin() const { return range_.first; }
      typename FACTOR::const_iterator end() const { return range_.second; }
      size_t size() const { return std::distance(range_.first, range_.second); }
      const auto& front() const { return *begin(); }
      // == operator overload for comparison with another iterator
      template<class OTHER>
      bool operator==(const OTHER& rhs) const {
        return std::equal(begin(), end(), rhs.begin());
      }
    };
 
    typedef ConstFactorRangeIterator Frontals;
 
    typedef ConstFactorRangeIterator Parents;
 
  protected:
 
    Conditional() : nrFrontals_(0) {}
 
    Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
 
 
  public:
 
    void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
    bool equals(const This& c, double tol = 1e-9) const;
 
 
 
    virtual ~Conditional() {}
 
    size_t nrFrontals() const { return nrFrontals_; }
 
    size_t nrParents() const { return asFactor().size() - nrFrontals_; }
 
    Key firstFrontalKey() const {
      if(nrFrontals_ > 0)
        return asFactor().front();
      else
        throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
    }
 
    Frontals frontals() const { return ConstFactorRangeIterator({beginFrontals(), endFrontals()});}
 
    Parents parents() const { return ConstFactorRangeIterator({beginParents(), endParents()}); }
 
    virtual double logProbability(const HybridValues& c) const;
 
    virtual double evaluate(const HybridValues& c) const;
 
    double operator()(const HybridValues& x) const {
      return evaluate(x);
    }
 
    virtual double negLogConstant() const;
 
 
    typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
 
    typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
 
    typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
 
    typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
 
    size_t& nrFrontals() { return nrFrontals_; }
 
    typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
 
    typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
 
    typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
 
    typename FACTOR::iterator endParents() { return asFactor().end(); }
 
    template <class VALUES>
    static bool CheckInvariants(const DERIVEDCONDITIONAL& conditional,
                                const VALUES& x);
 
 
  private:
 
 
    // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
    FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
 
    // Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
    const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
 
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
    }
#endif
 
 
  };
 
} // gtsam