27 template <
typename MAT_H,
typename MAT_C,
typename MAT_R>
29 const MAT_H& H,
const MAT_C& C, MAT_R&
R,
bool accumResultInOutput =
false)
32 if (accumResultInOutput)
36 R.resize(res.rows(), res.cols());
42 template <std::
size_t H_ROWS, std::
size_t H_COLS,
typename Scalar>
53 template <
typename VECTOR_H,
typename MAT_C>
62 template <
typename VECTOR_H,
typename MAT_C>
79 template <
class MAT_IN,
class VECTOR,
class MAT_OUT>
82 const size_t N = v.rows();
83 ASSERTMSG_(N > 0,
"The input matrix contains no elements");
84 const double N_inv = 1.0 / N;
86 const size_t M = v.cols();
87 ASSERTMSG_(M > 0,
"The input matrix contains rows of length 0");
90 out_mean.assign(M, 0);
91 for (
size_t i = 0; i < N; i++)
92 for (
size_t j = 0; j < M; j++) out_mean[j] += v.coeff(i, j);
98 out_cov.setZero(M, M);
99 for (
size_t i = 0; i < N; i++)
101 for (
size_t j = 0; j < M; j++)
102 out_cov(j, j) +=
square(v(i, j) - out_mean[j]);
104 for (
size_t j = 0; j < M; j++)
105 for (
size_t k = j + 1; k < M; k++)
107 (v(i, j) - out_mean[j]) * (v(i, k) - out_mean[k]);
109 for (
size_t j = 0; j < M; j++)
110 for (
size_t k = j + 1; k < M; k++) out_cov(k, j) = out_cov(j, k);
121 template <
class MAT_IN,
class VEC>
123 const MAT_IN& m, VEC& outMeanVector, VEC& outStdVector,
124 const bool unbiased_variance =
true)
126 const auto N = m.rows(), M = m.cols();
127 if (N == 0)
throw std::runtime_error(
"meanAndStdColumns: Empty container.");
128 const double N_inv = 1.0 / N;
130 unbiased_variance ? (N > 1 ? 1.0 / (N - 1) : 1.0) : 1.0 / N;
131 outMeanVector.resize(M);
132 outStdVector.resize(M);
133 for (decltype(m.cols()) i = 0; i < M; i++)
135 outMeanVector[i] = m.asEigen().col(i).array().sum() * N_inv;
136 outStdVector[i] = std::sqrt(
137 (m.asEigen().col(i).array() - outMeanVector[i]).square().sum() *
148 template <
class MATRIX>
158 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
164 template <
class MAT_A,
class SKEW_3VECTOR,
class MAT_OUT>
170 const size_t N =
A.rows();
172 for (
size_t i = 0; i < N; i++)
174 out(i, 0) =
A(i, 1) * v[2] -
A(i, 2) * v[1];
175 out(i, 1) = -
A(i, 0) * v[2] +
A(i, 2) * v[0];
176 out(i, 2) =
A(i, 0) * v[1] -
A(i, 1) * v[0];
185 template <
class SKEW_3VECTOR,
class MAT_A,
class MAT_OUT>
191 const size_t N =
A.cols();
193 for (
size_t i = 0; i < N; i++)
195 out(0, i) = -
A(1, i) * v[2] +
A(2, i) * v[1];
196 out(1, i) =
A(0, i) * v[2] -
A(2, i) * v[0];
197 out(2, i) = -
A(0, i) * v[1] +
A(1, i) * v[0];
206 template <
typename MATIN,
typename MATOUT>
209 if (g.rows() != g.cols())
210 throw std::runtime_error(
"laplacian: Defined for square matrixes only");
211 const auto N = g.rows();
214 for (
typename MATIN::Index i = 0; i < N; i++)
217 for (
typename MATIN::Index j = 0; j < N; j++) deg += g(j, i);
229 template <std::
size_t BLOCKSIZE,
typename MAT,
typename MATRIX>
231 const MAT& m,
const std::vector<size_t>& block_indices, MATRIX&
out)
234 throw std::runtime_error(
235 "extractSubmatrixSymmetricalBlocks: BLOCKSIZE must be >=1");
236 if (m.cols() != m.rows())
237 throw std::runtime_error(
238 "extractSubmatrixSymmetricalBlocks: Matrix is not square.");
240 const size_t N = block_indices.size();
242 out.resize(nrows_out, nrows_out);
244 for (
size_t dst_row_blk = 0; dst_row_blk < N; ++dst_row_blk)
246 for (
size_t dst_col_blk = 0; dst_col_blk < N; ++dst_col_blk)
251 throw std::runtime_error(
252 "extractSubmatrixSymmetricalBlocks: Indices out of "
255 out.asEigen().template block<BLOCKSIZE, BLOCKSIZE>(
257 m.asEigen().template block<BLOCKSIZE, BLOCKSIZE>(
265 template <
typename MAT,
typename MATRIX>
267 const MAT& m,
const std::size_t
BLOCKSIZE,
268 const std::vector<size_t>& block_indices, MATRIX&
out)
271 throw std::runtime_error(
272 "extractSubmatrixSymmetricalBlocks: BLOCKSIZE must be >=1");
273 if (m.cols() != m.rows())
274 throw std::runtime_error(
275 "extractSubmatrixSymmetricalBlocks: Matrix is not square.");
277 const size_t N = block_indices.size();
279 out.resize(nrows_out, nrows_out);
281 for (
size_t dst_row_blk = 0; dst_row_blk < N; ++dst_row_blk)
283 for (
size_t dst_col_blk = 0; dst_col_blk < N; ++dst_col_blk)
288 throw std::runtime_error(
289 "extractSubmatrixSymmetricalBlocks: Indices out of "
309 template <
typename MAT,
typename MATRIX>
311 const MAT& m,
const std::vector<size_t>& indices, MATRIX&
out)
313 if (m.cols() != m.rows())
314 throw std::runtime_error(
315 "extractSubmatrixSymmetrical: Matrix is not square.");
317 const size_t N = indices.size();
318 const size_t nrows_out = N;
319 out.resize(nrows_out, nrows_out);
321 for (
size_t dst_row_blk = 0; dst_row_blk < N; ++dst_row_blk)
322 for (
size_t dst_col_blk = 0; dst_col_blk < N; ++dst_col_blk)
323 out(dst_row_blk, dst_col_blk) =
324 m(indices[dst_row_blk], indices[dst_col_blk]);