29#include <boost/tuple/tuple.hpp>
40typedef Eigen::MatrixXd Matrix;
41typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
45#define GTSAM_MAKE_MATRIX_DEFS(N) \
46using Matrix##N = Eigen::Matrix<double, N, N>; \
47using Matrix1##N = Eigen::Matrix<double, 1, N>; \
48using Matrix2##N = Eigen::Matrix<double, 2, N>; \
49using Matrix3##N = Eigen::Matrix<double, 3, N>; \
50using Matrix4##N = Eigen::Matrix<double, 4, N>; \
51using Matrix5##N = Eigen::Matrix<double, 5, N>; \
52using Matrix6##N = Eigen::Matrix<double, 6, N>; \
53using Matrix7##N = Eigen::Matrix<double, 7, N>; \
54using Matrix8##N = Eigen::Matrix<double, 8, N>; \
55using Matrix9##N = Eigen::Matrix<double, 9, N>; \
56static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
57static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
59GTSAM_MAKE_MATRIX_DEFS(1)
60GTSAM_MAKE_MATRIX_DEFS(2)
61GTSAM_MAKE_MATRIX_DEFS(3)
62GTSAM_MAKE_MATRIX_DEFS(4)
63GTSAM_MAKE_MATRIX_DEFS(5)
64GTSAM_MAKE_MATRIX_DEFS(6)
65GTSAM_MAKE_MATRIX_DEFS(7)
66GTSAM_MAKE_MATRIX_DEFS(8)
67GTSAM_MAKE_MATRIX_DEFS(9)
70typedef Eigen::Block<Matrix> SubMatrix;
71typedef Eigen::Block<const Matrix> ConstSubMatrix;
75const Eigen::IOFormat& matlabFormat();
80template <class MATRIX>
81bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B,
double tol = 1e-9) {
83 const size_t n1 = A.cols(), m1 = A.rows();
84 const size_t n2 = B.cols(), m2 = B.rows();
86 if(m1!=m2 || n1!=n2)
return false;
88 for(
size_t i=0; i<m1; i++)
89 for(
size_t j=0; j<n1; j++) {
90 if(!
fpEqual(A(i,j), B(i,j), tol,
false)) {
114GTSAM_EXPORT
bool assert_equal(
const Matrix& A,
const Matrix& B,
double tol = 1e-9);
119GTSAM_EXPORT
bool assert_inequal(
const Matrix& A,
const Matrix& B,
double tol = 1e-9);
124GTSAM_EXPORT
bool assert_equal(
const std::list<Matrix>& As,
const std::list<Matrix>& Bs,
double tol = 1e-9);
129GTSAM_EXPORT
bool linear_independent(
const Matrix& A,
const Matrix& B,
double tol = 1e-9);
134GTSAM_EXPORT
bool linear_dependent(
const Matrix& A,
const Matrix& B,
double tol = 1e-9);
140GTSAM_EXPORT Vector
operator^(
const Matrix& A,
const Vector & v);
143template<
class MATRIX>
144inline MATRIX
prod(
const MATRIX& A,
const MATRIX&B) {
145 MATRIX result = A * B;
152GTSAM_EXPORT
void print(
const Matrix& A,
const std::string& s, std::ostream& stream);
157GTSAM_EXPORT
void print(
const Matrix& A,
const std::string& s =
"");
162GTSAM_EXPORT
void save(
const Matrix& A,
const std::string &s,
const std::string& filename);
169GTSAM_EXPORT std::istream&
operator>>(std::istream& inputStream, Matrix& destinationMatrix);
180template<
class MATRIX>
181Eigen::Block<const MATRIX>
sub(
const MATRIX& A,
size_t i1,
size_t i2,
size_t j1,
size_t j2) {
182 size_t m=i2-i1, n=j2-j1;
183 return A.block(i1,j1,m,n);
194template <
typename Derived1,
typename Derived2>
195void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix,
const Eigen::MatrixBase<Derived2>& subMatrix,
size_t i,
size_t j) {
196 fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
202GTSAM_EXPORT Matrix
diag(
const std::vector<Matrix>& Hs);
210template<
class MATRIX>
211const typename MATRIX::ConstColXpr
column(
const MATRIX& A,
size_t j) {
221template<
class MATRIX>
222const typename MATRIX::ConstRowXpr
row(
const MATRIX& A,
size_t j) {
231template<
class MATRIX>
233 const size_t m = A.rows(), n = A.cols();
234 const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n);
235 for (
size_t j=0; j<k; ++j)
236 A.col(j).segment(j+1, m-(j+1)).setZero();
242inline Matrix
trans(
const Matrix& A) {
return A.transpose(); }
245template <
int OutM,
int OutN,
int OutOptions,
int InM,
int InN,
int InOptions>
248 typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
249 static inline ReshapedType reshape(
const Eigen::Matrix<double, InM, InN, InOptions> & in) {
255template <
int M,
int InOptions>
256struct Reshape<M, M, InOptions, M, M, InOptions> {
257 typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
258 static inline ReshapedType reshape(
const Eigen::Matrix<double, M, M, InOptions> & in) {
264template <
int M,
int N,
int InOptions>
265struct Reshape<M, N, InOptions, M, N, InOptions> {
266 typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
267 static inline ReshapedType reshape(
const Eigen::Matrix<double, M, N, InOptions> & in) {
273template <
int M,
int N,
int InOptions>
274struct Reshape<N, M, InOptions, M, N, InOptions> {
275 typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
276 static inline ReshapedType reshape(
const Eigen::Matrix<double, M, N, InOptions> & in) {
277 return in.transpose();
281template <
int OutM,
int OutN,
int OutOptions,
int InM,
int InN,
int InOptions>
282inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(
const Eigen::Matrix<double, InM, InN, InOptions> & m){
283 BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
293GTSAM_EXPORT std::pair<Matrix,Matrix>
qr(
const Matrix& A);
310GTSAM_EXPORT std::list<boost::tuple<Vector, double, double> >
320GTSAM_EXPORT
void householder_(Matrix& A,
size_t k,
bool copy_vectors=
true);
364GTSAM_EXPORT Matrix
stack(
size_t nrMatrices, ...);
365GTSAM_EXPORT Matrix
stack(
const std::vector<Matrix>& blocks);
377GTSAM_EXPORT Matrix
collect(
const std::vector<const Matrix *>& matrices,
size_t m = 0,
size_t n = 0);
378GTSAM_EXPORT Matrix
collect(
size_t nrMatrices, ...);
387GTSAM_EXPORT Matrix vector_scale(
const Vector& v,
const Matrix& A,
bool inf_mask =
false);
388GTSAM_EXPORT Matrix vector_scale(
const Matrix& A,
const Vector& v,
bool inf_mask =
false);
402 return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
405template <
class Derived>
406inline Matrix3
skewSymmetric(
const Eigen::MatrixBase<Derived>& w) {
428GTSAM_EXPORT
void svd(
const Matrix& A, Matrix& U, Vector& S, Matrix& V);
437GTSAM_EXPORT boost::tuple<int, double, Vector>
438DLT(
const Matrix& A,
double rank_tol = 1e-9);
445GTSAM_EXPORT Matrix
expm(
const Matrix& A,
size_t K=7);
447std::string formatMatrixIndented(
const std::string& label,
const Matrix& matrix,
bool makeVectorHorizontal =
false);
457 typedef Eigen::Matrix<double, N, 1> VectorN;
458 typedef Eigen::Matrix<double, N, N> MatrixN;
464 const MatrixN invA = A.inverse();
465 const VectorN c = invA * b;
468 for (
size_t j = 0; j < N; j++)
469 H1->template middleCols<N>(N * j) = -c[j] * invA;
481template <
typename T,
int N>
484 typedef Eigen::Matrix<double, N, 1> VectorN;
485 typedef Eigen::Matrix<double, N, N> MatrixN;
489 typedef std::function<VectorN(
501 phi_(a, b, boost::none, A);
502 const MatrixN invA = A.inverse();
503 const VectorN c = invA * b;
506 Eigen::Matrix<double, N, M> H;
507 phi_(a, c, H, boost::none);
518GTSAM_EXPORT Matrix LLt(
const Matrix& A);
520GTSAM_EXPORT Matrix RtR(
const Matrix& A);
522GTSAM_EXPORT Vector columnNormSquare(
const Matrix &A);
Special class for optional Jacobian arguments.
typedef and functions to augment Eigen's VectorXd
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition: Matrix.cpp:367
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition: Matrix.cpp:131
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
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition: Matrix.h:222
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
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
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition: Matrix.cpp:43
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
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition: Matrix.h:211
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
Zeros all of the elements below the diagonal of a matrix, in place.
Definition: Matrix.h:232
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:397
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
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition: Matrix.cpp:377
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
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition: Matrix.cpp:354
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition: Matrix.cpp:174
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition: Matrix.cpp:636
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition: Matrix.cpp:560
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:401
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:181
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition: Matrix.h:242
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition: Matrix.h:107
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
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition: Matrix.cpp:539
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition: Matrix.h:144
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
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:195
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
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
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
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version
Definition: Matrix.cpp:235
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition: Matrix.cpp:207
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:81
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition: Matrix.h:100
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition: Matrix.cpp:552
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Reshape functor.
Definition: Matrix.h:246
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition: Matrix.h:456
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:461
Functor that implements multiplication with the inverse of a matrix, itself the result of a function ...
Definition: Matrix.h:482
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:497
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition: Matrix.h:494
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41