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 <gtsam/inference/VariableSlots.h>

#include <boost/make_shared.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/split_member.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);

    explicit JacobianFactor(
      const GaussianFactorGraph& graph,
      const VariableSlots& p_variableSlots);
    
    explicit JacobianFactor(
      const GaussianFactorGraph& graph,
      const Ordering& ordering);
    
    explicit JacobianFactor(
      const GaussianFactorGraph& graph,
      const Ordering& ordering,
      const VariableSlots& p_variableSlots);

    ~JacobianFactor() override {}

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

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

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

    Matrix information() const override;

    using Base::hessianDiagonal;

    void hessianDiagonalAdd(VectorValues& d) const override;

    void hessianDiagonal(double* d) const override;

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

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

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

    Matrix augmentedJacobian() const override;

    Matrix augmentedJacobianUnweighted() const;

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

    VerticalBlockMatrix& matrixObject() { return Ab_; }

    GaussianFactor::shared_ptr negate() const override;

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

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

    DenseIndex getDim(const_iterator variable) const override { 
      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 override;

    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 override;

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

    VectorValues gradientAtZero() const override;

    void gradientAtZero(double* d) const override;

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

    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:

    void JacobianFactorHelper(
      const GaussianFactorGraph& graph,
      const FastVector<VariableSlots::const_iterator>& orderedSlots);

    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 save(ARCHIVE & ar, const unsigned int version) const {
      // TODO(fan): This is a hack for Boost < 1.66
      // We really need to introduce proper versioning in the archives
      // As otherwise this will not read objects serialized by older
      // versions of GTSAM
      ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
      ar << BOOST_SERIALIZATION_NVP(Ab_);
      bool model_null = false;
      if(model_.get() == nullptr) {
        model_null = true;
        ar << boost::serialization::make_nvp("model_null", model_null);
      } else {
        ar << boost::serialization::make_nvp("model_null", model_null);
        ar << BOOST_SERIALIZATION_NVP(model_);
      }
    }

    template<class ARCHIVE>
    void load(ARCHIVE & ar, const unsigned int version) {
      // invoke serialization of the base class
      ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
      ar >> BOOST_SERIALIZATION_NVP(Ab_);
      if (version < 1) {
        ar >> BOOST_SERIALIZATION_NVP(model_);
      } else {
        bool model_null;
        ar >> BOOST_SERIALIZATION_NVP(model_null);
        if (!model_null) {
          ar >> BOOST_SERIALIZATION_NVP(model_);
        }
      }
    }

    BOOST_SERIALIZATION_SPLIT_MEMBER()
  }; // JacobianFactor
template<>
struct traits<JacobianFactor> : public Testable<JacobianFactor> {
};

} // \ namespace gtsam

BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)

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