Go to the documentation of this file.
45 size_t n2 = actual.cols(),
m2 = actual.rows();
47 cout <<
"not equal:" << endl;
49 print(actual,
"actual = ");
51 cout <<
m1 <<
"," <<
n1 <<
" != " <<
m2 <<
"," <<
n2 << endl;
54 print(diff,
"actual - expected = ");
62 cout <<
"Erroneously equal:" << endl;
69 bool assert_equal(
const std::list<Matrix>& As,
const std::list<Matrix>& Bs,
double tol) {
70 if (As.size() != Bs.size())
return false;
72 list<Matrix>::const_iterator itA, itB;
73 itA = As.begin(); itB = Bs.begin();
74 for (; itA != As.end(); itA++, itB++)
85 size_t n1 =
A.cols(),
m1 =
A.rows();
86 size_t n2 =
B.cols(),
m2 =
B.rows();
88 bool dependent =
true;
89 if(
m1!=
m2 ||
n1!=
n2) dependent =
false;
91 for(
size_t i=0; dependent &&
i<
m1;
i++) {
104 cout <<
"not linearly dependent:" << endl;
107 if(
A.rows()!=
B.rows() ||
A.cols()!=
B.cols())
108 cout <<
A.rows() <<
"x" <<
A.cols() <<
" != " <<
B.rows() <<
"x" <<
B.cols() << endl;
118 cout <<
"not linearly dependent:" << endl;
121 if(
A.rows()!=
B.rows() ||
A.cols()!=
B.cols())
122 cout <<
A.rows() <<
"x" <<
A.cols() <<
" != " <<
B.rows() <<
"x" <<
B.cols() << endl;
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()) +
")");
136 return A.transpose() *
v;
179 while(getline(inputStream, line)) {
181 coeffs.push_back(vector<double>());
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()));
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");
195 destinationMatrix.resize(height, width);
197 for(
const vector<double>& rowVec: coeffs) {
208 for (
size_t i = 0;
i<Hs.size(); ++
i) {
214 for (
size_t i = 0;
i<Hs.size(); ++
i) {
225 for (
size_t i = 0 ;
i < (
size_t)
A.cols() ; ++
i ) {
226 v[
i] =
A.col(
i).dot(
A.col(
i));
235 const size_t m =
A.rows(),
n =
A.cols(), kprime =
min(
m,
n);
241 for(
size_t j=0;
j < kprime;
j++){
248 for(
size_t k = 0 ; k < mm; k++)
255 for(
size_t k = 0 ; k <
m; k++)
256 v(k) = k<
j ? 0.0 : vjm(k-
j);
266 return make_pair(
Q,
R);
270 list<std::tuple<Vector, double, double> >
272 size_t m =
A.rows(),
n =
A.cols();
273 size_t maxRank =
min(
m,
n);
276 list<std::tuple<Vector, double, double> >
results;
285 for (
size_t j=0;
j<
n; ++
j) {
297 for (
size_t j2=
j+1; j2<
n; ++j2)
298 r(j2) = pseudo.dot(
A.col(j2));
301 double d = pseudo.dot(
b);
308 if (
results.size()>=maxRank)
break;
312 A -=
a * r.transpose();
326 const size_t m =
A.rows(),
n =
A.cols(), kprime =
min(k,
min(
m,
n));
328 for(
size_t j=0;
j < kprime;
j++) {
336 gttic(householder_update);
339 A.block(
j,
j,
m-
j,
n-
j) -= vjm *
w.transpose();
340 gttoc(householder_update);
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);
367 assert(
L.rows() ==
L.cols());
377 assert(
U.rows() ==
U.cols());
387 assert(
U.rows() ==
U.cols());
400 va_start(ap, nrMatrices);
401 for(
size_t i = 0 ;
i < nrMatrices ;
i++) {
407 va_start(ap, nrMatrices);
410 for(
size_t i = 0 ;
i < nrMatrices ;
i++) {
412 for(
size_t d1 = 0; d1 < (
size_t)
M->rows(); d1++)
414 A(vindex+d1,
d2) = (*M)(d1,
d2);
423 if (blocks.size() == 1)
return blocks.at(0);
424 DenseIndex nrows = 0, ncols = blocks.at(0).cols();
427 if (ncols !=
mat.cols())
428 throw invalid_argument(
"Matrix::stack(): column size mismatch!");
435 cur_row +=
mat.rows();
445 size_t dimA2 =
n*matrices.size();
447 for(
const Matrix*
M: matrices) {
456 for(
const Matrix*
M: matrices) {
457 size_t row_len =
M->cols();
458 A.block(0, hindex, dimA1, row_len) = *
M;
468 vector<const Matrix *> matrices;
470 va_start(ap, nrMatrices);
471 for(
size_t i = 0 ;
i < nrMatrices ;
i++) {
473 matrices.push_back(
M);
484 const double& vi =
v(
i);
506 const size_t n =
A.cols();
508 for (
size_t j=0;
j<
n; ++
j) {
509 const double& vj =
v(
j);
514 for (
size_t j=0;
j<
n; ++
j)
524 return llt.matrixL();
531 return llt.matrixU();
540 Matrix inv = Matrix::Identity(
A.rows(),
A.rows());
542 return inv*inv.transpose();
552 Matrix inv = Matrix::Identity(
A.rows(),
A.rows());
554 return inv.transpose();
561 S =
svd.singularValues();
566 std::tuple<int, double, Vector>
DLT(
const Matrix& A,
double rank_tol) {
569 size_t n =
A.rows(),
p =
A.cols(),
m =
min(
n,
p);
578 for (
size_t j = 0;
j <
m;
j++)
579 if (
s(
j) > rank_tol) rank++;
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);
600 const string firstline = label;
602 const string padding(firstline.size(),
' ');
603 const bool transposeMatrix = makeVectorHorizontal &&
matrix.cols() == 1 &&
matrix.rows() > 1;
608 stringstream matrixPrinted;
610 matrixPrinted <<
matrix.transpose();
613 const std::string matrixStr = matrixPrinted.str();
617 std::istringstream iss(matrixStr);
619 while (std::getline(iss, line)) {
620 assert(
row < effectiveRows);
623 ss <<
"[ " << line <<
" ]";
624 if(
row < effectiveRows - 1)
637 size_t rows =
A.rows();
638 size_t cols =
A.cols();
643 HCoeffsType hCoeffs(
size);
644 RowVectorType temp(
cols);
646 #if !EIGEN_VERSION_AT_LEAST(3,2,5)
list< std::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, Vector &b, const Vector &sigmas)
std::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
const EIGEN_DEVICE_FUNC CwiseBinaryOp< internal::scalar_boolean_xor_op, const Derived, const OtherDerived > operator^(const EIGEN_CURRENT_STORAGE_BASE_CLASS< OtherDerived > &other) const
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
EIGEN_DONT_INLINE void llt(const Mat &A, const Mat &B, Mat &C)
typedef and functions to augment Eigen's VectorXd
Included from all GTSAM files.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
double weightedPseudoinverse(const Vector &a, const Vector &weights, Vector &pseudo)
double houseInPlace(Vector &v)
Matrix inverse_square_root(const Matrix &A)
typedef and functions to augment Eigen's MatrixXd
Matrix vector_scale(const Matrix &A, const Vector &v, bool inf_mask)
Matrix diag(const std::vector< Matrix > &Hs)
void householder_(Matrix &A, size_t k, bool copy_vectors)
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Matrix stack(const std::vector< Matrix > &blocks)
const Eigen::IOFormat & matlabFormat()
pair< Matrix, Matrix > qr(const Matrix &A)
double beta(double a, double b)
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
static std::stringstream ss
void householder(const MatrixType &m)
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
Matrix RtR(const Matrix &A)
Matrix cholesky_inverse(const Matrix &A)
Matrix expm(const Matrix &A, size_t K)
Vector columnNormSquare(const Matrix &A)
A matrix or vector expression mapping an existing array of data.
std::ofstream out("Result.txt")
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
void save(const Matrix &A, const string &s, const string &filename)
Two-sided Jacobi SVD decomposition of a rectangular matrix.
The quaternion class used to represent 3D orientations and rotations.
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
void inplace_QR(Matrix &A)
static void run(MatrixQR &mat, HCoeffs &hCoeffs, Index maxBlockSize=32, typename MatrixQR::Scalar *tempData=0)
ptrdiff_t DenseIndex
The index type for Eigen objects.
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
bool assert_equal(const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol)
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
Array< int, Dynamic, 1 > v
std::map< std::string, Array< float, 1, 8, DontAlign|RowMajor > > results
A thin wrapper around std::list that uses boost's fast_pool_allocator.
pair< double, Vector > house(const Vector &x)
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Vector backSubstituteUpper(const Vector &b, const Matrix &U, bool unit)
bool linear_independent(const Matrix &A, const Matrix &B, double tol)
void print(const Matrix &A, const string &s)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
static bool is_linear_dependent(const Matrix &A, const Matrix &B, double tol)
Matrix collect(size_t nrMatrices,...)
std::string formatMatrixIndented(const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
Matrix LLt(const Matrix &A)
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:34