GaussianConditional.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/global_includes.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/VectorValues.h>

#include <random> // for std::mt19937_64 

namespace gtsam {

  class GTSAM_EXPORT GaussianConditional :
    public JacobianFactor,
    public Conditional<JacobianFactor, GaussianConditional>
  {
  public:
    typedef GaussianConditional This; 
    typedef std::shared_ptr<This> shared_ptr; 
    typedef JacobianFactor BaseFactor; 
    typedef Conditional<BaseFactor, This> BaseConditional; 


    GaussianConditional() {}

    GaussianConditional(Key key, const Vector& d, const Matrix& R,
      const SharedDiagonal& sigmas = SharedDiagonal());

    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
                        const Matrix& S,
                        const SharedDiagonal& sigmas = SharedDiagonal());

    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
                        const Matrix& S, Key parent2, const Matrix& T,
                        const SharedDiagonal& sigmas = SharedDiagonal());

    template<typename TERMS>
    GaussianConditional(const TERMS& terms,
      size_t nrFrontals, const Vector& d,
      const SharedDiagonal& sigmas = SharedDiagonal());

    template<typename KEYS>
    GaussianConditional(
      const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
      const SharedDiagonal& sigmas = SharedDiagonal());

    static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu,
                                                 double sigma);

    static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
                                                 Key parent, const Vector& b,
                                                 double sigma);

    static GaussianConditional FromMeanAndStddev(Key key,  //
                                                 const Matrix& A1, Key parent1,
                                                 const Matrix& A2, Key parent2,
                                                 const Vector& b, double sigma);

    template<typename... Args>
    static shared_ptr sharedMeanAndStddev(Args&&... args) {
      return std::make_shared<This>(FromMeanAndStddev(std::forward<Args>(args)...));
    }

    template<typename ITERATOR>
    static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);


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

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


    double logNormalizationConstant() const override;

    double logProbability(const VectorValues& x) const;

    double evaluate(const VectorValues& x) const;

    double operator()(const VectorValues& x) const {
      return evaluate(x);
    }

    VectorValues solve(const VectorValues& parents) const;

    VectorValues solveOtherRHS(const VectorValues& parents, const VectorValues& rhs) const;

    void solveTransposeInPlace(VectorValues& gy) const;

    JacobianFactor::shared_ptr likelihood(
        const VectorValues& frontalValues) const;

    JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;

    VectorValues sample(std::mt19937_64* rng) const;

    VectorValues sample(const VectorValues& parentsValues,
                        std::mt19937_64* rng) const;

    VectorValues sample() const;

    VectorValues sample(const VectorValues& parentsValues) const;


    constABlock R() const { return Ab_.range(0, nrFrontals()); }

    constABlock S() const { return Ab_.range(nrFrontals(), size()); }

    constABlock S(const_iterator it) const { return BaseFactor::getA(it); }

    const constBVector d() const { return BaseFactor::getb(); }

    inline double determinant() const { return exp(logDeterminant()); }

    double logDeterminant() const;


    double logProbability(const HybridValues& x) const override;

    double evaluate(const HybridValues& x) const override;

    using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
    using JacobianFactor::error; // Expose error(const HybridValues&) method..


   private:
#ifdef 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);
    }
#endif
  }; // GaussianConditional

template<>
struct traits<GaussianConditional> : public Testable<GaussianConditional> {};

} // \ namespace gtsam

#include <gtsam/linear/GaussianConditional-inl.h>