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());
 
    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());
 
    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));
    }
 
    // 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; 
 
    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;
 
    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>