gtsam/gtsam/linear/GaussianFactor.h

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

/**
 * @file    GaussianFactor.h
 * @brief   A factor with a quadratic error function - a Gaussian
 * @brief   GaussianFactor
 * @author  Richard Roberts, Christian Potthast
 */

// \callgraph

#pragma once

#include <gtsam/inference/Factor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>

namespace gtsam {

  // Forward declarations
  class VectorValues;
  class Scatter;
  class SymmetricBlockMatrix;

  /**
   * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a
   * quadratic error function. GaussianFactor is non-mutable (all methods const!). The factor value
   * is exp(-0.5*||Ax-b||^2) */
  class GTSAM_EXPORT GaussianFactor : public Factor
  {
  public:
    typedef GaussianFactor This; ///< This class
    typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
    typedef Factor Base; ///< Our base class

    /** Default constructor creates empty factor */
    GaussianFactor() {}
    
    /** Construct from container of keys.  This constructor is used internally from derived factor
     *  constructors, either from a container of keys or from a boost::assign::list_of. */
    template<typename CONTAINER>
    GaussianFactor(const CONTAINER& keys) : Base(keys) {}

    /** Destructor */
    virtual ~GaussianFactor() {}

    // Implementing Testable interface
    virtual void print(const std::string& s = "",
        const KeyFormatter& formatter = DefaultKeyFormatter) const = 0;

    /** Equals for testable */
    virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;

    /** Print for testable */
    virtual double error(const VectorValues& c) const = 0; /**  0.5*(A*x-b)'*D*(A*x-b) */

    /** Return the dimension of the variable pointed to by the given key iterator */
    virtual DenseIndex getDim(const_iterator variable) const = 0;

    /**
     * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
     * Jacobian matrix, augmented with b with the noise models baked
     * into A and b.  The negative log-likelihood is
     * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
     * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
     */
    virtual Matrix augmentedJacobian() const = 0;

    /**
     * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
     * with the noise models baked into A and b. The negative log-likelihood
     * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
     * GaussianFactorGraph::augmentedJacobian and
     * GaussianFactorGraph::sparseJacobian.
     */
    virtual std::pair<Matrix,Vector> jacobian() const = 0;

    /** Return the augmented information matrix represented by this GaussianFactor.
     * The augmented information matrix contains the information matrix with an
     * additional column holding the information vector, and an additional row
     * holding the transpose of the information vector.  The lower-right entry
     * contains the constant error term (when \f$ \delta x = 0 \f$).  The
     * augmented information matrix is described in more detail in HessianFactor,
     * which in fact stores an augmented information matrix.
     */
    virtual Matrix augmentedInformation() const = 0;

    /** Return the non-augmented information matrix represented by this
     * GaussianFactor.
     */
    virtual Matrix information() const = 0;

    /// Return the diagonal of the Hessian for this factor
    virtual VectorValues hessianDiagonal() const = 0;

    /// Raw memory access version of hessianDiagonal
    virtual void hessianDiagonal(double* d) const = 0;

    /// Return the block diagonal of the Hessian for this factor
    virtual std::map<Key,Matrix> hessianBlockDiagonal() const = 0;

    /** Clone a factor (make a deep copy) */
    virtual GaussianFactor::shared_ptr clone() const = 0;

    /** Test whether the factor is empty */
    virtual bool empty() const = 0;

    /**
     * Construct the corresponding anti-factor to negate information
     * stored stored in this factor.
     * @return a HessianFactor with negated Hessian matrices
     */
    virtual GaussianFactor::shared_ptr negate() const = 0;

    /** Update an information matrix by adding the information corresponding to this factor
     * (used internally during elimination).
     * @param scatter A mapping from variable index to slot index in this HessianFactor
     * @param info The information matrix to be updated
     */
    virtual void updateHessian(const KeyVector& keys,
                           SymmetricBlockMatrix* info) const = 0;

    /// y += alpha * A'*A*x
    virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;

    /// A'*b for Jacobian, eta for Hessian
    virtual VectorValues gradientAtZero() const = 0;

    /// Raw memory access version of gradientAtZero
    virtual void gradientAtZero(double* d) const = 0;

    /// Gradient wrt a key at any values
    virtual Vector gradient(Key key, const VectorValues& x) const = 0;

    // Determine position of a given key
    template <typename CONTAINER>
    static DenseIndex Slot(const CONTAINER& keys, Key key) {
      return std::find(keys.begin(), keys.end(), key) - keys.begin();
    }

  private:
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    }

  }; // GaussianFactor

/// traits
template<>
struct traits<GaussianFactor> : public Testable<GaussianFactor> {
};

} // \ namespace gtsam