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 {
        if (this->size() != rhs.size()) return false;
        if (this->size() == 0) return true;
        return std::equal(begin(), end(), rhs.begin(), rhs.end());
      }
    };

    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