gtsam  4.1.0
gtsam
Matrix.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
23 // \callgraph
24 
25 #pragma once
26 
28 #include <gtsam/base/Vector.h>
29 #include <gtsam/config.h>
30 
31 #include <boost/format.hpp>
32 #include <boost/function.hpp>
33 #include <boost/tuple/tuple.hpp>
34 #include <boost/math/special_functions/fpclassify.hpp>
35 
41 namespace gtsam {
42 
43 typedef Eigen::MatrixXd Matrix;
44 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
45 
46 // Create handy typedefs and constants for square-size matrices
47 // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
48 #define GTSAM_MAKE_MATRIX_DEFS(N) \
49 typedef Eigen::Matrix<double, N, N> Matrix##N; \
50 typedef Eigen::Matrix<double, 1, N> Matrix1##N; \
51 typedef Eigen::Matrix<double, 2, N> Matrix2##N; \
52 typedef Eigen::Matrix<double, 3, N> Matrix3##N; \
53 typedef Eigen::Matrix<double, 4, N> Matrix4##N; \
54 typedef Eigen::Matrix<double, 5, N> Matrix5##N; \
55 typedef Eigen::Matrix<double, 6, N> Matrix6##N; \
56 typedef Eigen::Matrix<double, 7, N> Matrix7##N; \
57 typedef Eigen::Matrix<double, 8, N> Matrix8##N; \
58 typedef Eigen::Matrix<double, 9, N> Matrix9##N; \
59 static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
60 static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
61 
62 GTSAM_MAKE_MATRIX_DEFS(1);
63 GTSAM_MAKE_MATRIX_DEFS(2);
64 GTSAM_MAKE_MATRIX_DEFS(3);
65 GTSAM_MAKE_MATRIX_DEFS(4);
66 GTSAM_MAKE_MATRIX_DEFS(5);
67 GTSAM_MAKE_MATRIX_DEFS(6);
68 GTSAM_MAKE_MATRIX_DEFS(7);
69 GTSAM_MAKE_MATRIX_DEFS(8);
70 GTSAM_MAKE_MATRIX_DEFS(9);
71 
72 // Matrix expressions for accessing parts of matrices
73 typedef Eigen::Block<Matrix> SubMatrix;
74 typedef Eigen::Block<const Matrix> ConstSubMatrix;
75 
76 // Matrix formatting arguments when printing.
77 // Akin to Matlab style.
78 const Eigen::IOFormat& matlabFormat();
79 
83 template <class MATRIX>
84 bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
85 
86  const size_t n1 = A.cols(), m1 = A.rows();
87  const size_t n2 = B.cols(), m2 = B.rows();
88 
89  if(m1!=m2 || n1!=n2) return false;
90 
91  for(size_t i=0; i<m1; i++)
92  for(size_t j=0; j<n1; j++) {
93  if(!fpEqual(A(i,j), B(i,j), tol, false)) {
94  return false;
95  }
96  }
97  return true;
98 }
99 
103 inline bool operator==(const Matrix& A, const Matrix& B) {
104  return equal_with_abs_tol(A,B,1e-9);
105 }
106 
110 inline bool operator!=(const Matrix& A, const Matrix& B) {
111  return !(A==B);
112  }
113 
117 GTSAM_EXPORT bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);
118 
122 GTSAM_EXPORT bool assert_inequal(const Matrix& A, const Matrix& B, double tol = 1e-9);
123 
127 GTSAM_EXPORT bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol = 1e-9);
128 
132 GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double tol = 1e-9);
133 
137 GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
138 
143 GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);
144 
146 template<class MATRIX>
147 inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
148  MATRIX result = A * B;
149  return result;
150 }
151 
155 GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
156 
160 GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
161 
165 GTSAM_EXPORT void save(const Matrix& A, const std::string &s, const std::string& filename);
166 
172 GTSAM_EXPORT std::istream& operator>>(std::istream& inputStream, Matrix& destinationMatrix);
173 
183 template<class MATRIX>
184 Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) {
185  size_t m=i2-i1, n=j2-j1;
186  return A.block(i1,j1,m,n);
187 }
188 
197 template <typename Derived1, typename Derived2>
198 void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
199  fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
200 }
201 
205 GTSAM_EXPORT Matrix diag(const std::vector<Matrix>& Hs);
206 
213 template<class MATRIX>
214 const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) {
215  return A.col(j);
216 }
217 
224 template<class MATRIX>
225 const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
226  return A.row(j);
227 }
228 
234 template<class MATRIX>
235 void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
236  const size_t m = A.rows(), n = A.cols();
237  const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n);
238  for (size_t j=0; j<k; ++j)
239  A.col(j).segment(j+1, m-(j+1)).setZero();
240 }
241 
245 inline Matrix trans(const Matrix& A) { return A.transpose(); }
246 
248 template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
249 struct Reshape {
250  //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)
251  typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
252  static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
253  return in.data();
254  }
255 };
256 
258 template <int M, int InOptions>
259 struct Reshape<M, M, InOptions, M, M, InOptions> {
260  typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
261  static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
262  return in;
263  }
264 };
265 
267 template <int M, int N, int InOptions>
268 struct Reshape<M, N, InOptions, M, N, InOptions> {
269  typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
270  static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
271  return in;
272  }
273 };
274 
276 template <int M, int N, int InOptions>
277 struct Reshape<N, M, InOptions, M, N, InOptions> {
278  typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
279  static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
280  return in.transpose();
281  }
282 };
283 
284 template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
285 inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
286  BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
288 }
289 
296 GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
297 
303 GTSAM_EXPORT void inplace_QR(Matrix& A);
304 
313 GTSAM_EXPORT std::list<boost::tuple<Vector, double, double> >
314 weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
315 
323 GTSAM_EXPORT void householder_(Matrix& A, size_t k, bool copy_vectors=true);
324 
331 GTSAM_EXPORT void householder(Matrix& A, size_t k);
332 
340 GTSAM_EXPORT Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
341 
349 //TODO: is this function necessary? it isn't used
350 GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
351 
359 GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false);
360 
367 GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...);
368 GTSAM_EXPORT Matrix stack(const std::vector<Matrix>& blocks);
369 
380 GTSAM_EXPORT Matrix collect(const std::vector<const Matrix *>& matrices, size_t m = 0, size_t n = 0);
381 GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
382 
389 GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
390 GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
391 GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // column
392 
404 inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
405  return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
406 }
407 
408 template <class Derived>
409 inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
410  return skewSymmetric(w(0), w(1), w(2));
411 }
412 
414 GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);
415 
417 GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);
418 
431 GTSAM_EXPORT void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
432 
440 GTSAM_EXPORT boost::tuple<int, double, Vector>
441 DLT(const Matrix& A, double rank_tol = 1e-9);
442 
448 GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
449 
450 std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
451 
458 template <int N>
460  typedef Eigen::Matrix<double, N, 1> VectorN;
461  typedef Eigen::Matrix<double, N, N> MatrixN;
462 
464  VectorN operator()(const MatrixN& A, const VectorN& b,
465  OptionalJacobian<N, N* N> H1 = boost::none,
466  OptionalJacobian<N, N> H2 = boost::none) const {
467  const MatrixN invA = A.inverse();
468  const VectorN c = invA * b;
469  // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
470  if (H1)
471  for (size_t j = 0; j < N; j++)
472  H1->template middleCols<N>(N * j) = -c[j] * invA;
473  // The derivative in b is easy, as invA*b is just a linear map:
474  if (H2) *H2 = invA;
475  return c;
476  }
477 };
478 
484 template <typename T, int N>
486  enum { M = traits<T>::dimension };
487  typedef Eigen::Matrix<double, N, 1> VectorN;
488  typedef Eigen::Matrix<double, N, N> MatrixN;
489 
490  // The function phi should calculate f(a)*b, with derivatives in a and b.
491  // Naturally, the derivative in b is f(a).
492  typedef boost::function<VectorN(
493  const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
494  Operator;
495 
497  MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}
498 
500  VectorN operator()(const T& a, const VectorN& b,
501  OptionalJacobian<N, M> H1 = boost::none,
502  OptionalJacobian<N, N> H2 = boost::none) const {
503  MatrixN A;
504  phi_(a, b, boost::none, A); // get A = f(a) by calling f once
505  const MatrixN invA = A.inverse();
506  const VectorN c = invA * b;
507 
508  if (H1) {
509  Eigen::Matrix<double, N, M> H;
510  phi_(a, c, H, boost::none); // get derivative H of forward mapping
511  *H1 = -invA* H;
512  }
513  if (H2) *H2 = invA;
514  return c;
515  }
516 
517  private:
518  const Operator phi_;
519 };
520 
521 GTSAM_EXPORT Matrix LLt(const Matrix& A);
522 
523 GTSAM_EXPORT Matrix RtR(const Matrix& A);
524 
525 GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
526 } // namespace gtsam
527 
528 #include <boost/serialization/nvp.hpp>
529 #include <boost/serialization/array.hpp>
530 #include <boost/serialization/split_free.hpp>
531 
532 namespace boost {
533  namespace serialization {
534 
549  // split version - sends sizes ahead
550  template<class Archive,
551  typename Scalar_,
552  int Rows_,
553  int Cols_,
554  int Ops_,
555  int MaxRows_,
556  int MaxCols_>
557  void save(Archive & ar,
558  const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
559  const unsigned int /*version*/) {
560  const size_t rows = m.rows(), cols = m.cols();
561  ar << BOOST_SERIALIZATION_NVP(rows);
562  ar << BOOST_SERIALIZATION_NVP(cols);
563  ar << make_nvp("data", make_array(m.data(), m.size()));
564  }
565 
566  template<class Archive,
567  typename Scalar_,
568  int Rows_,
569  int Cols_,
570  int Ops_,
571  int MaxRows_,
572  int MaxCols_>
573  void load(Archive & ar,
574  Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
575  const unsigned int /*version*/) {
576  size_t rows, cols;
577  ar >> BOOST_SERIALIZATION_NVP(rows);
578  ar >> BOOST_SERIALIZATION_NVP(cols);
579  m.resize(rows, cols);
580  ar >> make_nvp("data", make_array(m.data(), m.size()));
581  }
582 
583  // templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
584  template<class Archive,
585  typename Scalar_,
586  int Rows_,
587  int Cols_,
588  int Ops_,
589  int MaxRows_,
590  int MaxCols_>
591  void serialize(Archive & ar,
592  Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
593  const unsigned int version) {
594  split_free(ar, m, version);
595  }
596 
597  // specialized to Matrix for MATLAB wrapper
598  template <class Archive>
599  void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
600  split_free(ar, m, version);
601  }
602 
603  } // namespace serialization
604 } // namespace boost
gtsam::backSubstituteUpper
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition: Matrix.cpp:376
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:198
gtsam::MultiplyWithInverseFunction::MultiplyWithInverseFunction
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition: Matrix.h:497
gtsam::inverse_square_root
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition: Matrix.cpp:551
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:84
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
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::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:102
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:326
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:272
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:500
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam::MultiplyWithInverseFunction
Functor that implements multiplication with the inverse of a matrix, itself the result of a function ...
Definition: Matrix.h:485
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::backSubstituteLower
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition: Matrix.cpp:366
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:62
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...
Definition: Matrix.cpp:442
Vector.h
typedef and functions to augment Eigen's VectorXd
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:567
gtsam::householder
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition: Matrix.cpp:353
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition: Matrix.cpp:559
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:184
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:225
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:116
gtsam::prod
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition: Matrix.h:147
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:316
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition: Matrix.h:245
gtsam::operator>>
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition: Matrix.cpp:173
gtsam::qr
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version
Definition: Matrix.cpp:234
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:464
gtsam::Reshape
Reshape functor.
Definition: Matrix.h:249
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:404
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:166
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition: Matrix.h:110
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::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:235
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:42
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:481
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:214
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition: Matrix.h:103
gtsam::inplace_QR
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition: Matrix.cpp:635
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition: Matrix.cpp:206
gtsam::operator^
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition: Matrix.cpp:130
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:396
gtsam::MultiplyWithInverse
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition: Matrix.h:459
gtsam::cholesky_inverse
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition: Matrix.cpp:538