JacobianFactor.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/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/global_includes.h>

#include <boost/make_shared.hpp>

namespace gtsam {

  // Forward declarations
  class HessianFactor;
  class VariableSlots;
  class GaussianFactorGraph;
  class GaussianConditional;
  class HessianFactor;
  class VectorValues;
  class Ordering;
  class JacobianFactor;

  GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
    EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);

  class GTSAM_EXPORT JacobianFactor : public GaussianFactor
  {
  public:

    typedef JacobianFactor This; 
    typedef GaussianFactor Base; 
    typedef boost::shared_ptr<This> shared_ptr; 

    typedef VerticalBlockMatrix::Block ABlock;
    typedef VerticalBlockMatrix::constBlock constABlock;
    typedef ABlock::ColXpr BVector;
    typedef constABlock::ConstColXpr constBVector;

  protected:

    VerticalBlockMatrix Ab_;      // the block view of the full matrix
    noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix

  public:

    explicit JacobianFactor(const GaussianFactor& gf);

    JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}

    explicit JacobianFactor(const HessianFactor& hf);

    JacobianFactor();

    explicit JacobianFactor(const Vector& b_in);

    JacobianFactor(Key i1, const Matrix& A1,
        const Vector& b, const SharedDiagonal& model = SharedDiagonal());

    JacobianFactor(Key i1, const Matrix& A1,
        Key i2, const Matrix& A2,
        const Vector& b, const SharedDiagonal& model = SharedDiagonal());

    JacobianFactor(Key i1, const Matrix& A1, Key i2,
        const Matrix& A2, Key i3, const Matrix& A3,
        const Vector& b, const SharedDiagonal& model = SharedDiagonal());

    template<typename TERMS>
    JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());

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

    explicit JacobianFactor(
      const GaussianFactorGraph& graph,
      boost::optional<const Ordering&> ordering = boost::none,
      boost::optional<const VariableSlots&> p_variableSlots = boost::none);

    virtual ~JacobianFactor() {}

    virtual GaussianFactor::shared_ptr clone() const {
      return boost::static_pointer_cast<GaussianFactor>(
          boost::make_shared<JacobianFactor>(*this));
    }

    // Implementing Testable interface
    virtual void print(const std::string& s = "",
      const KeyFormatter& formatter = DefaultKeyFormatter) const;
    virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;

    Vector unweighted_error(const VectorValues& c) const; 
    Vector error_vector(const VectorValues& c) const; 
    virtual double error(const VectorValues& c) const; 
    virtual Matrix augmentedInformation() const;

    virtual Matrix information() const;

    virtual VectorValues hessianDiagonal() const;

    virtual void hessianDiagonal(double* d) const;

    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;

    virtual std::pair<Matrix, Vector> jacobian() const;

    std::pair<Matrix, Vector> jacobianUnweighted() const;

    virtual Matrix augmentedJacobian() const;

    Matrix augmentedJacobianUnweighted() const;

    const VerticalBlockMatrix& matrixObject() const { return Ab_; }

    VerticalBlockMatrix& matrixObject() { return Ab_; }

    virtual GaussianFactor::shared_ptr negate() const;

    virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }

    bool isConstrained() const {
      return model_ && model_->isConstrained();
    }

    virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }

    size_t rows() const { return Ab_.rows(); }

    size_t cols() const { return Ab_.cols(); }

    const SharedDiagonal& get_model() const { return model_;  }

    SharedDiagonal& get_model() { return model_;  }

    const constBVector getb() const { return Ab_(size()).col(0); }

    constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }

    constABlock getA() const { return Ab_.range(0, size()); }

    BVector getb() { return Ab_(size()).col(0); }

    ABlock getA(iterator variable) { return Ab_(variable - begin()); }

    ABlock getA() { return Ab_.range(0, size()); }

    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;

    Vector operator*(const VectorValues& x) const;

    void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;

    void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;

    void multiplyHessianAdd(double alpha, const double* x, double* y,
        const std::vector<size_t>& accumulatedDims) const;

    VectorValues gradientAtZero() const;

    virtual void gradientAtZero(double* d) const;

    Vector gradient(Key key, const VectorValues& x) const;

    JacobianFactor whiten() const;

    std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
      eliminate(const Ordering& keys);

    void setModel(bool anyConstrained, const Vector& sigmas);

    friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
      EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);

    boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);

  protected:

    template<typename TERMS>
    void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);

  private:

    template<class KEYS, class DIMENSIONS>
    JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
        const SharedDiagonal& model = SharedDiagonal()) :
        Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
    }

    // be very selective on who can access these private methods:
    template<typename T> friend class ExpressionFactor;

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

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

} // \ namespace gtsam

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