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>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/version.hpp>
#include <boost/serialization/split_member.hpp>
#endif
 
namespace gtsam {
 
  // Forward declarations
  class HessianFactor;
  class VariableSlots;
  class GaussianFactorGraph;
  class GaussianConditional;
  class HessianFactor;
  class VectorValues;
  class Ordering;
  class JacobianFactor;

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

  class GTSAM_EXPORT JacobianFactor : public GaussianFactor
  {
  public:
 
    typedef JacobianFactor This; 
    typedef GaussianFactor Base; 
    typedef std::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& operator=(const JacobianFactor& jf) = default;

    JacobianFactor();

    explicit JacobianFactor(const Vector& b_in);

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

    template <int M, int N1,
              typename = std::enable_if_t<(M != Eigen::Dynamic &&
                                           N1 != Eigen::Dynamic)>>
    JacobianFactor(Key i1, const Eigen::Matrix<double, M, N1>& A1,
        const Eigen::Matrix<double, M, 1>& b,
        const SharedDiagonal& model = SharedDiagonal());

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

    template <int M, int N1, int N2,
              typename = std::enable_if_t<(M != Eigen::Dynamic &&
                                           N1 != Eigen::Dynamic &&
                                           N2 != Eigen::Dynamic)>>
    JacobianFactor(Key i1, const Eigen::Matrix<double, M, N1>& A1,
        Key i2, const Eigen::Matrix<double, M, N2>& A2,
        const Eigen::Matrix<double, M, 1>& 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 <int M, int N1, int N2, int N3,
              typename = std::enable_if_t<(M != Eigen::Dynamic &&
                                           N1 != Eigen::Dynamic &&
                                           N2 != Eigen::Dynamic &&
                                           N3 != Eigen::Dynamic)>>
    JacobianFactor(Key i1, const Eigen::Matrix<double, M, N1>& A1,
        Key i2, const Eigen::Matrix<double, M, N2>& A2,
        Key i3, const Eigen::Matrix<double, M, N3>& A3,
        const Eigen::Matrix<double, M, 1>& 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());

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

    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 std::static_pointer_cast<GaussianFactor>(
          std::make_shared<JacobianFactor>(*this));
    }

    bool isJacobian() const override { return true; }
 
    // 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; 

    using GaussianFactor::error;

    double error(const VectorValues& c) const override; 

    double deltaError(const VectorValues& c, double* oldError = nullptr,
                      double* newError = nullptr) 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 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()); }

    ABlock getA(const Key& key) { return Ab_(find(key) - begin()); }

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

    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info,
                       DenseIndex beginCol, DenseIndex endCol) 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<std::shared_ptr<GaussianConditional>, shared_ptr>
      eliminate(const Ordering& keys);

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

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

    std::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
 
  protected:

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

    void checkAb(const SharedDiagonal& model,
                 const VerticalBlockMatrix& augmentedMatrix) const;
 
   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;
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
    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()
#endif
  }; // JacobianFactor

template<>
struct traits<JacobianFactor> : public Testable<JacobianFactor> {
};
 
} // \ namespace gtsam
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
#endif
 
#include <gtsam/linear/JacobianFactor-inl.h>