Loading...
Searching...
No Matches
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 <boost/tuple/tuple.hpp>
#include <vector>
namespace gtsam {
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
// 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)
#define I_1x1 Matrix1::Identity()
#define Z_1x1 Matrix1::Constant(0.0)
#define I_2x2 Matrix2::Identity()
#define Z_2x2 Matrix2::Constant(0.0)
#define I_3x3 Matrix3::Identity()
#define Z_3x3 Matrix3::Constant(0.0)
#define I_4x4 Matrix4::Identity()
#define Z_4x4 Matrix4::Constant(0.0)
#define I_5x5 Matrix5::Identity()
#define Z_5x5 Matrix5::Constant(0.0)
#define I_6x6 Matrix6::Identity()
#define Z_6x6 Matrix6::Constant(0.0)
#define I_7x7 Matrix7::Identity()
#define Z_7x7 Matrix7::Constant(0.0)
#define I_8x8 Matrix8::Identity()
#define Z_8x8 Matrix8::Constant(0.0)
#define I_9x9 Matrix9::Identity()
#define Z_9x9 Matrix9::Constant(0.0)
// 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 Vector operator^(const Matrix& A, const Vector & v);
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);
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);
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);
}
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(); }
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
struct Reshape {
//TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
return in.data();
}
};
template <int M, int InOptions>
struct Reshape<M, M, InOptions, M, M, InOptions> {
typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
return in;
}
};
template <int M, int N, int InOptions>
struct Reshape<M, N, InOptions, M, N, InOptions> {
typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in;
}
};
template <int M, int N, int InOptions>
struct Reshape<N, M, InOptions, M, N, InOptions> {
typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in.transpose();
}
};
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
}
GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
GTSAM_EXPORT void inplace_QR(Matrix& 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
inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
}
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 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);
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 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) 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 {
enum { 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 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) const {
MatrixN A;
phi_(a, b, boost::none, 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, boost::none); // get derivative H of forward mapping
*H1 = -invA* H;
}
if (H2) *H2 = invA;
return c;
}
private:
const Operator phi_;
};
GTSAM_EXPORT Matrix LLt(const Matrix& A);
GTSAM_EXPORT Matrix RtR(const Matrix& A);
GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
} // namespace gtsam
OptionalJacobian.h
Special class for optional Jacobian arguments.
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::backSubstituteLower
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition Matrix.cpp:367
gtsam::operator^
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition Matrix.cpp:131
gtsam::vector_scale_inplace
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns...
Definition Matrix.cpp:482
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:249
gtsam::expm
T expm(const Vector &x, int K=7)
Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from...
Definition Lie.h:317
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear dependent
Definition Matrix.cpp:117
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition Matrix.h:238
gtsam::zeroBelowDiagonal
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
Zeros all of the elements below the diagonal of a matrix, in place.
Definition Matrix.h:259
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition Matrix.cpp:397
gtsam::weighted_eliminate
list< boost::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, Vector &b, const Vector &sigmas)
Imperative algorithm for in-place full elimination with weights and constraint handling.
Definition Matrix.cpp:273
gtsam::backSubstituteUpper
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition Matrix.cpp:377
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
inequals with an tolerance, prints out message if within tolerance
Definition Matrix.cpp:63
gtsam::householder
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition Matrix.cpp:354
gtsam::operator>>
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition Matrix.cpp:174
gtsam::inplace_QR
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition Matrix.cpp:636
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
Definition Matrix.h:428
gtsam::sub
Eigen::Block< const MATRIX > sub(const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
extract submatrix, slice semantics, i.e.
Definition Matrix.h:208
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:269
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:134
gtsam::DLT
boost::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic erro...
Definition Matrix.cpp:568
gtsam::cholesky_inverse
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition Matrix.cpp:539
gtsam::prod
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition Matrix.h:171
gtsam::householder_
void householder_(Matrix &A, size_t k, bool copy_vectors)
Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder v...
Definition Matrix.cpp:327
gtsam::insertSub
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checkin...
Definition Matrix.h:222
gtsam::collect
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same...
Definition Matrix.cpp:443
gtsam::linear_independent
bool linear_independent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear independent
Definition Matrix.cpp:103
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Ensure we are not including a different version of Eigen in user code than while compiling gtsam,...
Definition Vector.cpp:42
gtsam::qr
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version.
Definition Matrix.cpp:235
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition Matrix.cpp:207
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:108
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:127
gtsam::inverse_square_root
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition Matrix.cpp:552
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Reshape
Reshape functor.
Definition Matrix.h:273
gtsam::MultiplyWithInverse
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition Matrix.h:483
gtsam::MultiplyWithInverse::operator()
VectorN operator()(const MatrixN &A, const VectorN &b, OptionalJacobian< N, N *N > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const
A.inverse() * b, with optional derivatives.
Definition Matrix.h:488
gtsam::MultiplyWithInverseFunction::operator()
VectorN operator()(const T &a, const VectorN &b, OptionalJacobian< N, M > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const
f(a).inverse() * b, with optional derivatives
Definition Matrix.h:524
gtsam::MultiplyWithInverseFunction::MultiplyWithInverseFunction
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition Matrix.h:521
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41