Matrix.cpp
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 
18 #include <gtsam/global_includes.h>
19 #include <gtsam/base/Matrix.h>
20 #include <gtsam/base/timing.h>
21 #include <gtsam/base/Vector.h>
22 #include <gtsam/base/FastList.h>
23 #include <Eigen/SVD>
24 #include <Eigen/LU>
25 
26 #include <cstdarg>
27 #include <cstring>
28 #include <iomanip>
29 #include <list>
30 #include <fstream>
31 #include <limits>
32 #include <iostream>
33 #include <iterator>
34 
35 using namespace std;
36 
37 namespace gtsam {
38 
39 /* ************************************************************************* */
40 bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) {
41 
42  if (equal_with_abs_tol(expected,actual,tol)) return true;
43 
44  size_t n1 = expected.cols(), m1 = expected.rows();
45  size_t n2 = actual.cols(), m2 = actual.rows();
46 
47  cout << "not equal:" << endl;
48  print(expected,"expected = ");
49  print(actual,"actual = ");
50  if(m1!=m2 || n1!=n2)
51  cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl;
52  else {
53  Matrix diff = actual-expected;
54  print(diff, "actual - expected = ");
55  }
56  return false;
57 }
58 
59 /* ************************************************************************* */
60 bool assert_inequal(const Matrix& A, const Matrix& B, double tol) {
61  if (!equal_with_abs_tol(A,B,tol)) return true;
62  cout << "Erroneously equal:" << endl;
63  print(A, "A = ");
64  print(B, "B = ");
65  return false;
66 }
67 
68 /* ************************************************************************* */
69 bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol) {
70  if (As.size() != Bs.size()) return false;
71 
72  list<Matrix>::const_iterator itA, itB;
73  itA = As.begin(); itB = Bs.begin();
74  for (; itA != As.end(); itA++, itB++)
75  if (!assert_equal(*itB, *itA, tol))
76  return false;
77 
78  return true;
79 }
80 
81 /* ************************************************************************* */
82 static bool is_linear_dependent(const Matrix& A, const Matrix& B, double tol) {
83  // This local static function is used by linear_independent and
84  // linear_dependent just below.
85  size_t n1 = A.cols(), m1 = A.rows();
86  size_t n2 = B.cols(), m2 = B.rows();
87 
88  bool dependent = true;
89  if(m1!=m2 || n1!=n2) dependent = false;
90 
91  for(size_t i=0; dependent && i<m1; i++) {
92  if (!gtsam::linear_dependent(Vector(row(A,i)), Vector(row(B,i)), tol))
93  dependent = false;
94  }
95 
96  return dependent;
97 }
98 
99 /* ************************************************************************* */
100 bool linear_independent(const Matrix& A, const Matrix& B, double tol) {
101  if(!is_linear_dependent(A, B, tol))
102  return true;
103  else {
104  cout << "not linearly dependent:" << endl;
105  print(A,"A = ");
106  print(B,"B = ");
107  if(A.rows()!=B.rows() || A.cols()!=B.cols())
108  cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
109  return false;
110  }
111 }
112 
113 /* ************************************************************************* */
114 bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
115  if(is_linear_dependent(A, B, tol))
116  return true;
117  else {
118  cout << "not linearly dependent:" << endl;
119  print(A,"A = ");
120  print(B,"B = ");
121  if(A.rows()!=B.rows() || A.cols()!=B.cols())
122  cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
123  return false;
124  }
125 }
126 
127 /* ************************************************************************* */
128 Vector operator^(const Matrix& A, const Vector & v) {
129  if (A.rows()!=v.size()) {
130  throw std::invalid_argument("Matrix operator^ : A.m(" + std::to_string(A.rows()) + ")!=v.size(" +
131  std::to_string(v.size()) + ")");
132  }
133 // Vector vt = v.transpose();
134 // Vector vtA = vt * A;
135 // return vtA.transpose();
136  return A.transpose() * v;
137 }
138 
140  static const Eigen::IOFormat matlab(
141  Eigen::StreamPrecision, // precision
142  Eigen::DontAlignCols, // flags set such that rowSpacers are not added
143  ", ", // coeffSeparator
144  ";\n", // rowSeparator
145  "\t", // rowPrefix
146  "", // rowSuffix
147  "[\n", // matPrefix
148  "\n]" // matSuffix
149  );
150  return matlab;
151 }
152 
153 /* ************************************************************************* */
154 //3 argument call
155 void print(const Matrix& A, const string &s, ostream& stream) {
156  stream << s << A.format(matlabFormat()) << endl;
157 }
158 
159 /* ************************************************************************* */
160 //1 or 2 argument call
161 void print(const Matrix& A, const string &s){
162  print(A, s, cout);
163 }
164 
165 /* ************************************************************************* */
166 void save(const Matrix& A, const string &s, const string& filename) {
167  fstream stream(filename.c_str(), fstream::out | fstream::app);
168  print(A, s + "=", stream);
169  stream.close();
170 }
171 
172 /* ************************************************************************* */
173 istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
174  string line;
175  FastList<vector<double> > coeffs;
176  bool first = true;
177  size_t width = 0;
178  size_t height = 0;
179  while(getline(inputStream, line)) {
180  // Read coefficients from file
181  coeffs.push_back(vector<double>());
182  if(!first)
183  coeffs.back().reserve(width);
184  stringstream lineStream(line);
185  std::copy(istream_iterator<double>(lineStream), istream_iterator<double>(),
186  back_insert_iterator<vector<double> >(coeffs.back()));
187  if(first)
188  width = coeffs.back().size();
189  if(coeffs.back().size() != width)
190  throw runtime_error("Error reading matrix from input stream, inconsistent numbers of elements in rows");
191  ++ height;
192  }
193 
194  // Copy coefficients to matrix
195  destinationMatrix.resize(height, width);
196  int row = 0;
197  for(const vector<double>& rowVec: coeffs) {
198  destinationMatrix.row(row) = Eigen::Map<const Eigen::RowVectorXd>(&rowVec[0], width);
199  ++ row;
200  }
201 
202  return inputStream;
203 }
204 
205 /* ************************************************************************* */
206 Matrix diag(const std::vector<Matrix>& Hs) {
207  size_t rows = 0, cols = 0;
208  for (size_t i = 0; i<Hs.size(); ++i) {
209  rows+= Hs[i].rows();
210  cols+= Hs[i].cols();
211  }
212  Matrix results = Matrix::Zero(rows,cols);
213  size_t r = 0, c = 0;
214  for (size_t i = 0; i<Hs.size(); ++i) {
215  insertSub(results, Hs[i], r, c);
216  r+=Hs[i].rows();
217  c+=Hs[i].cols();
218  }
219  return results;
220 }
221 
222 /* ************************************************************************* */
224  Vector v (A.cols()) ;
225  for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) {
226  v[i] = A.col(i).dot(A.col(i));
227  }
228  return v ;
229 }
230 
231 /* ************************************************************************* */
233 /* ************************************************************************* */
234 pair<Matrix,Matrix> qr(const Matrix& A) {
235  const size_t m = A.rows(), n = A.cols(), kprime = min(m,n);
236 
237  Matrix Q=Matrix::Identity(m,m),R(A);
238  Vector v(m);
239 
240  // loop over the kprime first columns
241  for(size_t j=0; j < kprime; j++){
242 
243  // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end)
244  const size_t mm=m-j;
245 
246  // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j)
247  Vector xjm(mm);
248  for(size_t k = 0 ; k < mm; k++)
249  xjm(k) = R(j+k, j);
250 
251  // calculate the Householder vector v
252  const auto [beta, vjm] = house(xjm);
253 
254  // pad with zeros to get m-dimensional vector v
255  for(size_t k = 0 ; k < m; k++)
256  v(k) = k<j ? 0.0 : vjm(k-j);
257 
258  // create Householder reflection matrix Qj = I-beta*v*v'
259  Matrix Qj = Matrix::Identity(m,m) - beta * v * v.transpose();
260 
261  R = Qj * R; // update R
262  Q = Q * Qj; // update Q
263 
264  } // column j
265 
266  return make_pair(Q,R);
267 }
268 
269 /* ************************************************************************* */
270 list<std::tuple<Vector, double, double> >
272  size_t m = A.rows(), n = A.cols(); // get size(A)
273  size_t maxRank = min(m,n);
274 
275  // create list
276  list<std::tuple<Vector, double, double> > results;
277 
278  Vector pseudo(m); // allocate storage for pseudo-inverse
279  Vector weights = sigmas.array().square().inverse(); // calculate weights once
280 
281  // We loop over all columns, because the columns that can be eliminated
282  // are not necessarily contiguous. For each one, estimate the corresponding
283  // scalar variable x as d-rS, with S the separator (remaining columns).
284  // Then update A and b by substituting x with d-rS, zero-ing out x's column.
285  for (size_t j=0; j<n; ++j) {
286  // extract the first column of A
287  Vector a(column(A, j));
288 
289  // Calculate weighted pseudo-inverse and corresponding precision
290  double precision = weightedPseudoinverse(a, weights, pseudo);
291 
292  // if precision is zero, no information on this column
293  if (precision < 1e-8) continue;
294 
295  // create solution and copy into r
296  Vector r(Vector::Unit(n,j));
297  for (size_t j2=j+1; j2<n; ++j2)
298  r(j2) = pseudo.dot(A.col(j2));
299 
300  // create the rhs
301  double d = pseudo.dot(b);
302 
303  // construct solution (r, d, sigma)
304  // TODO: avoid sqrt, store precision or at least variance
305  results.push_back(std::make_tuple(r, d, 1./sqrt(precision)));
306 
307  // exit after rank exhausted
308  if (results.size()>=maxRank) break;
309 
310  // update A, b, expensive, using outer product
311  // A' \define A_{S}-a*r and b'\define b-d*a
312  A -= a * r.transpose();
313  b -= d * a;
314  }
315 
316  return results;
317 }
318 
319 /* ************************************************************************* */
324 /* ************************************************************************* */
325 void householder_(Matrix& A, size_t k, bool copy_vectors) {
326  const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
327  // loop over the kprime first columns
328  for(size_t j=0; j < kprime; j++) {
329  // copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j)
330  Vector vjm = A.col(j).segment(j, m-j);
331 
332  // calculate the Householder vector, in place
333  double beta = houseInPlace(vjm);
334 
335  // do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
336  gttic(householder_update); // bottleneck for system
337  // don't touch old columns
338  Vector w = beta * A.block(j,j,m-j,n-j).transpose() * vjm;
339  A.block(j,j,m-j,n-j) -= vjm * w.transpose();
340  gttoc(householder_update);
341 
342  // the Householder vector is copied in the zeroed out part
343  if (copy_vectors) {
344  gttic(householder_vector_copy);
345  A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1));
346  gttoc(householder_vector_copy);
347  }
348  } // column j
349 }
350 
351 /* ************************************************************************* */
352 void householder(Matrix& A, size_t k) {
353  // version with zeros below diagonal
355  householder_(A,k,false);
357 // gttic(householder_zero_fill);
358 // const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
359 // for(size_t j=0; j < kprime; j++)
360 // A.col(j).segment(j+1, m-(j+1)).setZero();
361 // gttoc(householder_zero_fill);
362 }
363 
364 /* ************************************************************************* */
365 Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
366  // @return the solution x of L*x=b
367  assert(L.rows() == L.cols());
368  if (unit)
369  return L.triangularView<Eigen::UnitLower>().solve(b);
370  else
371  return L.triangularView<Eigen::Lower>().solve(b);
372 }
373 
374 /* ************************************************************************* */
375 Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
376  // @return the solution x of U*x=b
377  assert(U.rows() == U.cols());
378  if (unit)
379  return U.triangularView<Eigen::UnitUpper>().solve(b);
380  else
381  return U.triangularView<Eigen::Upper>().solve(b);
382 }
383 
384 /* ************************************************************************* */
385 Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
386  // @return the solution x of x'*U=b'
387  assert(U.rows() == U.cols());
388  if (unit)
389  return U.triangularView<Eigen::UnitUpper>().transpose().solve<Eigen::OnTheLeft>(b);
390  else
391  return U.triangularView<Eigen::Upper>().transpose().solve<Eigen::OnTheLeft>(b);
392 }
393 
394 /* ************************************************************************* */
395 Matrix stack(size_t nrMatrices, ...)
396 {
397  size_t dimA1 = 0;
398  size_t dimA2 = 0;
399  va_list ap;
400  va_start(ap, nrMatrices);
401  for(size_t i = 0 ; i < nrMatrices ; i++) {
402  Matrix *M = va_arg(ap, Matrix *);
403  dimA1 += M->rows();
404  dimA2 = M->cols(); // TODO: should check if all the same !
405  }
406  va_end(ap);
407  va_start(ap, nrMatrices);
408  Matrix A(dimA1, dimA2);
409  size_t vindex = 0;
410  for( size_t i = 0 ; i < nrMatrices ; i++) {
411  Matrix *M = va_arg(ap, Matrix *);
412  for(size_t d1 = 0; d1 < (size_t) M->rows(); d1++)
413  for(size_t d2 = 0; d2 < (size_t) M->cols(); d2++)
414  A(vindex+d1, d2) = (*M)(d1, d2);
415  vindex += M->rows();
416  }
417 
418  return A;
419 }
420 
421 /* ************************************************************************* */
422 Matrix stack(const std::vector<Matrix>& blocks) {
423  if (blocks.size() == 1) return blocks.at(0);
424  DenseIndex nrows = 0, ncols = blocks.at(0).cols();
425  for(const Matrix& mat: blocks) {
426  nrows += mat.rows();
427  if (ncols != mat.cols())
428  throw invalid_argument("Matrix::stack(): column size mismatch!");
429  }
430  Matrix result(nrows, ncols);
431 
432  DenseIndex cur_row = 0;
433  for(const Matrix& mat: blocks) {
434  result.middleRows(cur_row, mat.rows()) = mat;
435  cur_row += mat.rows();
436  }
437  return result;
438 }
439 
440 /* ************************************************************************* */
441 Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
442 {
443  // if we have known and constant dimensions, use them
444  size_t dimA1 = m;
445  size_t dimA2 = n*matrices.size();
446  if (!m && !n) {
447  for(const Matrix* M: matrices) {
448  dimA1 = M->rows(); // TODO: should check if all the same !
449  dimA2 += M->cols();
450  }
451  }
452 
453  // stl::copy version
454  Matrix A(dimA1, dimA2);
455  size_t hindex = 0;
456  for(const Matrix* M: matrices) {
457  size_t row_len = M->cols();
458  A.block(0, hindex, dimA1, row_len) = *M;
459  hindex += row_len;
460  }
461 
462  return A;
463 }
464 
465 /* ************************************************************************* */
466 Matrix collect(size_t nrMatrices, ...)
467 {
468  vector<const Matrix *> matrices;
469  va_list ap;
470  va_start(ap, nrMatrices);
471  for( size_t i = 0 ; i < nrMatrices ; i++) {
472  Matrix *M = va_arg(ap, Matrix *);
473  matrices.push_back(M);
474  }
475  return collect(matrices);
476 }
477 
478 /* ************************************************************************* */
479 // row scaling, in-place
480 void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
481  const DenseIndex m = A.rows();
482  if (inf_mask) {
483  for (DenseIndex i=0; i<m; ++i) {
484  const double& vi = v(i);
485  if (std::isfinite(vi))
486  A.row(i) *= vi;
487  }
488  } else {
489  for (DenseIndex i=0; i<m; ++i)
490  A.row(i) *= v(i);
491  }
492 }
493 
494 /* ************************************************************************* */
495 // row scaling
496 Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask) {
497  Matrix M(A);
498  vector_scale_inplace(v, M, inf_mask);
499  return M;
500 }
501 
502 /* ************************************************************************* */
503 // column scaling
504 Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
505  Matrix M(A);
506  const size_t n = A.cols();
507  if (inf_mask) {
508  for (size_t j=0; j<n; ++j) {
509  const double& vj = v(j);
510  if (std::isfinite(vj))
511  M.col(j) *= vj;
512  }
513  } else {
514  for (size_t j=0; j<n; ++j)
515  M.col(j) *= v(j);
516  }
517  return M;
518 }
519 
520 /* ************************************************************************* */
521 Matrix LLt(const Matrix& A)
522 {
524  return llt.matrixL();
525 }
526 
527 /* ************************************************************************* */
528 Matrix RtR(const Matrix &A)
529 {
531  return llt.matrixU();
532 }
533 
534 /*
535  * This is not ultra efficient, but not terrible, either.
536  */
538 {
540  Matrix inv = Matrix::Identity(A.rows(),A.rows());
541  llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
542  return inv*inv.transpose();
543 }
544 
545 /* ************************************************************************* */
546 // Semantics:
547 // if B = inverse_square_root(A), then all of the following are true:
548 // inv(B) * inv(B)' == A
549 // inv(B' * B) == A
552  Matrix inv = Matrix::Identity(A.rows(),A.rows());
553  llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
554  return inv.transpose();
555 }
556 
557 /* ************************************************************************* */
558 void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) {
560  U = svd.matrixU();
561  S = svd.singularValues();
562  V = svd.matrixV();
563 }
564 
565 /* ************************************************************************* */
566 std::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
567 
568  // Check size of A
569  size_t n = A.rows(), p = A.cols(), m = min(n,p);
570 
571  // Do SVD on A
573  Vector s = svd.singularValues();
574  Matrix V = svd.matrixV();
575 
576  // Find rank
577  size_t rank = 0;
578  for (size_t j = 0; j < m; j++)
579  if (s(j) > rank_tol) rank++;
580 
581  // Return rank, error, and corresponding column of V
582  double error = m<p ? 0 : s(m-1);
583  return std::tuple<int, double, Vector>((int)rank, error, Vector(column(V, p-1)));
584 }
585 
586 /* ************************************************************************* */
587 Matrix expm(const Matrix& A, size_t K) {
588  Matrix E = Matrix::Identity(A.rows(),A.rows()), A_k = Matrix::Identity(A.rows(),A.rows());
589  for(size_t k=1;k<=K;k++) {
590  A_k = A_k*A/double(k);
591  E = E + A_k;
592  }
593  return E;
594 }
595 
596 /* ************************************************************************* */
597 std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal)
598 {
599  stringstream ss;
600  const string firstline = label;
601  ss << firstline;
602  const string padding(firstline.size(), ' ');
603  const bool transposeMatrix = makeVectorHorizontal && matrix.cols() == 1 && matrix.rows() > 1;
604  const DenseIndex effectiveRows = transposeMatrix ? matrix.cols() : matrix.rows();
605 
606  if(matrix.rows() > 0 && matrix.cols() > 0)
607  {
608  stringstream matrixPrinted;
609  if(transposeMatrix)
610  matrixPrinted << matrix.transpose();
611  else
612  matrixPrinted << matrix;
613  const std::string matrixStr = matrixPrinted.str();
614 
615  // Split the matrix string into lines and indent them
616  std::string line;
617  std::istringstream iss(matrixStr);
618  DenseIndex row = 0;
619  while (std::getline(iss, line)) {
620  assert(row < effectiveRows);
621  if(row > 0)
622  ss << padding;
623  ss << "[ " << line << " ]";
624  if(row < effectiveRows - 1)
625  ss << "\n";
626  ++ row;
627  }
628 
629  } else {
630  ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")";
631  }
632  return ss.str();
633 }
634 
635 /* ************************************************************************* */
637  size_t rows = A.rows();
638  size_t cols = A.cols();
639  size_t size = std::min(rows,cols);
640 
642  typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
643  HCoeffsType hCoeffs(size);
644  RowVectorType temp(cols);
645 
646 #if !EIGEN_VERSION_AT_LEAST(3,2,5)
648 #else
650 #endif
651 
653 }
654 
655 } // namespace gtsam
Matrix3f m
std::string formatMatrixIndented(const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
Definition: Matrix.cpp:597
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
Definition: base/Matrix.h:231
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
void householder_(Matrix &A, size_t k, bool copy_vectors)
Definition: Matrix.cpp:325
static void run(MatrixQR &mat, HCoeffs &hCoeffs, Index maxBlockSize=32, typename MatrixQR::Scalar *tempData=0)
void save(const Matrix &A, const string &s, const string &filename)
Definition: Matrix.cpp:166
double weightedPseudoinverse(const Vector &a, const Vector &weights, Vector &pseudo)
Definition: Vector.cpp:245
A thin wrapper around std::list that uses boost&#39;s fast_pool_allocator.
void inplace_QR(Matrix &A)
Definition: Matrix.cpp:636
Scalar * b
Definition: benchVecAdd.cpp:17
pair< Matrix, Matrix > qr(const Matrix &A)
Definition: Matrix.cpp:234
EIGEN_DEVICE_FUNC const CwiseBinaryOp< internal::scalar_boolean_xor_op, const Derived, const OtherDerived > operator^(const EIGEN_CURRENT_STORAGE_BASE_CLASS< OtherDerived > &other) const
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
tuple make_tuple()
Definition: cast.h:1209
std::ofstream out("Result.txt")
#define min(a, b)
Definition: datatypes.h:19
Matrix expected
Definition: testMatrix.cpp:971
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
MatrixType m2(n_dims)
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
std::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Definition: Matrix.cpp:566
Rot2 R(Rot2::fromAngle(0.1))
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
MatrixXd L
Definition: LLT_example.cpp:6
Definition: BFloat16.h:88
#define isfinite(X)
Definition: main.h:95
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
DiscreteKey S(1, 2)
void print(const Matrix &A, const string &s)
Definition: Matrix.cpp:161
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
Definition: Matrix.cpp:365
Matrix RtR(const Matrix &A)
Definition: Matrix.cpp:528
bool assert_equal(const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol)
Definition: Matrix.cpp:69
Included from all GTSAM files.
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:114
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
Matrix LLt(const Matrix &A)
Definition: Matrix.cpp:521
#define gttic(label)
Definition: timing.h:295
const Eigen::IOFormat & matlabFormat()
Definition: Matrix.cpp:139
pair< double, Vector > house(const Vector &x)
Definition: Vector.cpp:236
EIGEN_DONT_INLINE void llt(const Mat &A, const Mat &B, Mat &C)
Definition: llt.cpp:5
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
void householder(const MatrixType &m)
Definition: householder.cpp:13
Vector backSubstituteUpper(const Vector &b, const Matrix &U, bool unit)
Definition: Matrix.cpp:385
Eigen::VectorXd Vector
Definition: Vector.h:38
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Definition: Matrix.cpp:173
Values result
Matrix3d m1
Definition: IOFormat.cpp:2
m row(1)
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
Definition: LLT.h:66
Array< int, Dynamic, 1 > v
static bool is_linear_dependent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:82
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Matrix collect(size_t nrMatrices,...)
Definition: Matrix.cpp:466
RealScalar s
Matrix vector_scale(const Matrix &A, const Vector &v, bool inf_mask)
Definition: Matrix.cpp:504
RowVector3d w
static std::stringstream ss
Definition: testBTree.cpp:31
Matrix stack(const std::vector< Matrix > &blocks)
Definition: Matrix.cpp:422
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
Traits::MatrixL matrixL() const
Definition: LLT.h:135
DiscreteKey E(5, 2)
Vector columnNormSquare(const Matrix &A)
Definition: Matrix.cpp:223
#define gttoc(label)
Definition: timing.h:296
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
Definition: base/Matrix.h:194
double houseInPlace(Vector &v)
Definition: Vector.cpp:211
The quaternion class used to represent 3D orientations and rotations.
std::map< std::string, Array< float, 1, 8, DontAlign|RowMajor > > results
cout precision(2)
Matrix inverse_square_root(const Matrix &A)
Definition: Matrix.cpp:550
Traits::MatrixU matrixU() const
Definition: LLT.h:128
Two-sided Jacobi SVD decomposition of a rectangular matrix.
float * p
const SingularValuesType & singularValues() const
Definition: SVDBase.h:129
static double error
Definition: testRot3.cpp:37
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
Definition: Matrix.cpp:480
const G double tol
Definition: Group.h:86
Generic expression where a coefficient-wise unary operator is applied to an expression.
Definition: CwiseUnaryOp.h:55
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
bool linear_independent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:100
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: base/Matrix.h:210
Matrix cholesky_inverse(const Matrix &A)
Definition: Matrix.cpp:537
Matrix expm(const Matrix &A, size_t K)
Definition: Matrix.cpp:587
Stores a set of parameters controlling the way matrices are printed.
Definition: IO.h:51
std::ptrdiff_t j
Timing utilities.
list< std::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, Vector &b, const Vector &sigmas)
Definition: Matrix.cpp:271


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:46