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/Vector.h>
#include <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace gtsam {

typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;

typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,6,6> Matrix6;

// Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;

// Matlab-like syntax

GTSAM_EXPORT Matrix zeros(size_t m, size_t n);

GTSAM_EXPORT Matrix ones(size_t m, size_t n);

GTSAM_EXPORT Matrix eye(size_t m, size_t n);

inline Matrix eye( size_t m ) { return eye(m,m); }
GTSAM_EXPORT Matrix diag(const Vector& v);

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(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
        return false;
      else if(fabs(A(i,j) - B(i,j)) > tol)
        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 multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e);

GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e);

GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);

GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x);

GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x);

GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x);

template<class MATRIX>
inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
  MATRIX result = A * B;
  return result;
}

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

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

template<class MATRIX>
Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) {
  size_t m=i2-i1, n=j2-j1;
  return A.block(i1,j1,m,n);
}

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

GTSAM_EXPORT Matrix diag(const std::vector<Matrix>& Hs);

template<class MATRIX>
const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) {
  return A.col(j);
}

template<class MATRIX>
const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
  return A.row(j);
}

GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j);
GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j);

GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);

template<class MATRIX>
void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
  const size_t m = A.rows(), n = A.cols();
  const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n);
  for (size_t j=0; j<k; ++j)
    A.col(j).segment(j+1, m-(j+1)).setZero();
}

inline Matrix trans(const Matrix& A) { return A.transpose(); }

GTSAM_EXPORT void solve(Matrix& A, Matrix& B);

GTSAM_EXPORT Matrix inverse(const Matrix& A);

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

template <class MATRIX>
void inplace_QR(MATRIX& A) {
  size_t rows = A.rows();
  size_t cols = A.cols();
  size_t size = std::min(rows,cols);

  typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
  typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
  HCoeffsType hCoeffs(size);
  RowVectorType temp(cols);

  Eigen::internal::householder_qr_inplace_blocked<MATRIX, HCoeffsType>::run(A, hCoeffs, 48, temp.data());

  zeroBelowDiagonal(A);
}

GTSAM_EXPORT std::list<boost::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);

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

//TODO: is this function necessary? it isn't used
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);

GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...);
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);
GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);

GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // column

GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz);
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);

GTSAM_EXPORT Matrix LLt(const Matrix& A);

GTSAM_EXPORT Matrix RtR(const Matrix& A);

GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);

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

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

GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);

GTSAM_EXPORT Matrix Cayley(const Matrix& A);

template<int N>
Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
  typedef Eigen::Matrix<double, N, N> FMat;
  return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
}

std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);

} // namespace gtsam

#include <boost/serialization/nvp.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/split_free.hpp>

namespace boost {
  namespace serialization {

    // split version - sends sizes ahead
    template<class Archive>
    void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) {
      const size_t rows = m.rows(), cols = m.cols();
      ar << BOOST_SERIALIZATION_NVP(rows);
      ar << BOOST_SERIALIZATION_NVP(cols);
      ar << make_nvp("data", make_array(m.data(), m.size()));
    }

    template<class Archive>
    void load(Archive & ar, gtsam::Matrix & m, unsigned int version) {
      size_t rows, cols;
      ar >> BOOST_SERIALIZATION_NVP(rows);
      ar >> BOOST_SERIALIZATION_NVP(cols);
      m.resize(rows, cols);
      ar >> make_nvp("data", make_array(m.data(), m.size()));
    }

  } // namespace serialization
} // namespace boost

BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix)