Matrix.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
 
 * -------------------------------------------------------------------------- */

 
// \callgraph
 
#pragma once
 
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
 
#include <vector>

namespace gtsam {
 
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
using ConstMatrixView =
    Eigen::Ref<const Matrix, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>;
 
// Create handy typedefs and constants for square-size matrices
// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
#define GTSAM_MAKE_MATRIX_DEFS(N)   \
using Matrix##N = Eigen::Matrix<double, N, N>;  \
using Matrix1##N = Eigen::Matrix<double, 1, N>;  \
using Matrix2##N = Eigen::Matrix<double, 2, N>;  \
using Matrix3##N = Eigen::Matrix<double, 3, N>;  \
using Matrix4##N = Eigen::Matrix<double, 4, N>;  \
using Matrix5##N = Eigen::Matrix<double, 5, N>;  \
using Matrix6##N = Eigen::Matrix<double, 6, N>;  \
using Matrix7##N = Eigen::Matrix<double, 7, N>;  \
using Matrix8##N = Eigen::Matrix<double, 8, N>;  \
using Matrix9##N = Eigen::Matrix<double, 9, N>;  \
 
GTSAM_MAKE_MATRIX_DEFS(1)
GTSAM_MAKE_MATRIX_DEFS(2)
GTSAM_MAKE_MATRIX_DEFS(3)
GTSAM_MAKE_MATRIX_DEFS(4)
GTSAM_MAKE_MATRIX_DEFS(5)
GTSAM_MAKE_MATRIX_DEFS(6)
GTSAM_MAKE_MATRIX_DEFS(7)
GTSAM_MAKE_MATRIX_DEFS(8)
GTSAM_MAKE_MATRIX_DEFS(9)
 
// Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
// Matrix formatting arguments when printing.
// Akin to Matlab style.
const Eigen::IOFormat& matlabFormat();

template <class MATRIX>
bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
 
  const size_t n1 = A.cols(), m1 = A.rows();
  const size_t n2 = B.cols(), m2 = B.rows();
 
  if(m1!=m2 || n1!=n2) return false;
 
  for(size_t i=0; i<m1; i++)
    for(size_t j=0; j<n1; j++) {
      if(!fpEqual(A(i,j), B(i,j), tol, false)) {
        return false;
      }
    }
  return true;
}

inline bool operator==(const Matrix& A, const Matrix& B) {
  return equal_with_abs_tol(A,B,1e-9);
}

inline bool operator!=(const Matrix& A, const Matrix& B) {
  return !(A==B);
 }

GTSAM_EXPORT bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);

GTSAM_EXPORT bool assert_inequal(const Matrix& A, const Matrix& B, double tol = 1e-9);

GTSAM_EXPORT bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol = 1e-9);

GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double tol = 1e-9);

GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);

GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);

GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");

GTSAM_EXPORT void save(const Matrix& A, const std::string &s, const std::string& filename);

GTSAM_EXPORT std::istream& operator>>(std::istream& inputStream, Matrix& destinationMatrix);
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
template <typename Derived1, typename Derived2>
void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix,
               const Eigen::MatrixBase<Derived2>& subMatrix, size_t i,
               size_t j) {
  fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}
#endif

GTSAM_EXPORT Matrix diag(const std::vector<Matrix>& Hs);
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
inline Matrix trans(const Matrix& A) { return A.transpose(); }
#endif

GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);

GTSAM_EXPORT void inplace_QR(Matrix& A);

GTSAM_EXPORT std::list<std::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);

GTSAM_EXPORT void householder_(Matrix& A, size_t k, bool copy_vectors=true);

GTSAM_EXPORT void householder(Matrix& A, size_t k);
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
GTSAM_EXPORT Vector backSubstituteUpper(const Matrix& U, const Vector& b,
                                        bool unit = false);

GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U,
                                        bool unit = false);

GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b,
                                        bool unit = false);
#endif
 
namespace internal {

template <class RDerived, class SDerived, class DDerived, class ParentsDerived>
void solveUpperConditional(const Eigen::MatrixBase<RDerived>& R,
                           const Eigen::MatrixBase<SDerived>& S,
                           const Eigen::MatrixBase<DDerived>& d,
                           const Eigen::MatrixBase<ParentsDerived>& parents,
                           Vector* result) {
  result->resize(d.rows());
  result->noalias() = d - S * parents;
  R.derived().template triangularView<Eigen::Upper>().solveInPlace(*result);
}
 
}  // namespace internal
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...);
#endif
GTSAM_EXPORT Matrix stack(const std::vector<Matrix>& blocks);

GTSAM_EXPORT Matrix collect(const std::vector<const Matrix*>& matrices,
                            size_t m = 0, size_t n = 0);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
#endif
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A,
                                       bool inf_mask = false);

GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A,
                                 bool inf_mask = false);

GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v,
                                 bool inf_mask = false);
#endif

 
inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
  return Matrix3{{0.0, -wz, +wy}, {+wz, 0.0, -wx}, {-wy, +wx, 0.0}};
}
 
template <class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
  return skewSymmetric(w(0), w(1), w(2));
}

GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);
#endif

GTSAM_EXPORT void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);

GTSAM_EXPORT std::tuple<int, double, Vector>
DLT(const Matrix& A, double rank_tol = 1e-9);

GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
 
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);

template <int N>
struct MultiplyWithInverse {
  typedef Eigen::Matrix<double, N, 1> VectorN;
  typedef Eigen::Matrix<double, N, N> MatrixN;

  VectorN operator()(const MatrixN& A, const VectorN& b,
                     OptionalJacobian<N, N* N> H1 = {},
                     OptionalJacobian<N, N> H2 = {}) const {
    const MatrixN invA = A.inverse();
    const VectorN c = invA * b;
    // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
    if (H1)
      for (size_t j = 0; j < N; j++)
        H1->template middleCols<N>(N * j) = -c[j] * invA;
    // The derivative in b is easy, as invA*b is just a linear map:
    if (H2) *H2 = invA;
    return c;
  }
};

template <typename T, int N>
struct MultiplyWithInverseFunction {
  inline constexpr static auto M = traits<T>::dimension;
  typedef Eigen::Matrix<double, N, 1> VectorN;
  typedef Eigen::Matrix<double, N, N> MatrixN;
 
  // The function phi should calculate f(a)*b, with derivatives in a and b.
  // Naturally, the derivative in b is f(a).
  typedef std::function<VectorN(
      const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
      Operator;

  MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}

  VectorN operator()(const T& a, const VectorN& b,
                     OptionalJacobian<N, M> H1 = {},
                     OptionalJacobian<N, N> H2 = {}) const {
    MatrixN A;
    phi_(a, b, {}, A);  // get A = f(a) by calling f once
    const MatrixN invA = A.inverse();
    const VectorN c = invA * b;
 
    if (H1) {
      Eigen::Matrix<double, N, M> H;
      phi_(a, c, H, {});  // get derivative H of forward mapping
      *H1 = -invA* H;
    }
    if (H2) *H2 = invA;
    return c;
  }
 
 private:
  const Operator phi_;
};
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
GTSAM_EXPORT Matrix LLt(const Matrix& A);

GTSAM_EXPORT Matrix RtR(const Matrix& A);

GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
#endif
}  // namespace gtsam