9 #ifndef mrpt_math_matrix_ops_H
10 #define mrpt_math_matrix_ops_H
32 template <
class Derived>
40 template <
class Derived>
50 template <
typename MAT_H,
typename MAT_C,
typename MAT_R>
52 const MAT_H& H,
const MAT_C& C, MAT_R&
R,
53 bool accumResultInOutput)
55 if (accumResultInOutput)
56 R += ((H * C.template selfadjointView<Eigen::Lower>()).eval() *
59 .template selfadjointView<Eigen::Lower>();
61 R = ((H * C.template selfadjointView<Eigen::Lower>()).eval() *
64 .template selfadjointView<Eigen::Lower>();
68 template <
typename VECTOR_H,
typename MAT_C>
71 return (H.matrix().adjoint() * C * H.matrix()).eval()(0, 0);
75 template <
typename MAT_H,
typename MAT_C,
typename MAT_R>
77 const MAT_H& H,
const MAT_C& C, MAT_R&
R,
78 bool accumResultInOutput)
80 if (accumResultInOutput)
82 ((H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() *
85 .template selfadjointView<Eigen::Lower>();
87 R = ((H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() *
90 .template selfadjointView<Eigen::Lower>();
102 template <
class MAT_IN,
class VECTOR,
class MAT_OUT>
105 const size_t N =
v.rows();
106 ASSERTMSG_(N > 0,
"The input matrix contains no elements");
107 const double N_inv = 1.0 / N;
109 const size_t M =
v.cols();
110 ASSERTMSG_(M > 0,
"The input matrix contains rows of length 0");
113 out_mean.assign(M, 0);
114 for (
size_t i = 0; i < N; i++)
115 for (
size_t j = 0; j < M; j++) out_mean[j] +=
v.coeff(i, j);
122 for (
size_t i = 0; i < N; i++)
124 for (
size_t j = 0; j < M; j++)
125 out_cov.get_unsafe(j, j) +=
126 square(
v.get_unsafe(i, j) - out_mean[j]);
128 for (
size_t j = 0; j < M; j++)
129 for (
size_t k = j + 1; k < M; k++)
130 out_cov.get_unsafe(j, k) += (
v.get_unsafe(i, j) - out_mean[j]) *
131 (
v.get_unsafe(i, k) - out_mean[k]);
133 for (
size_t j = 0; j < M; j++)
134 for (
size_t k = j + 1; k < M; k++)
135 out_cov.get_unsafe(k, j) = out_cov.get_unsafe(j, k);
145 template <
class MATRIX>
146 inline Eigen::Matrix<
typename MATRIX::Scalar, MATRIX::ColsAtCompileTime,
147 MATRIX::ColsAtCompileTime>
150 Eigen::Matrix<double, MATRIX::ColsAtCompileTime, 1> m;
152 MATRIX::ColsAtCompileTime>
159 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
165 template <
class MAT_A,
class SKEW_3VECTOR,
class MAT_OUT>
171 const size_t N = A.rows();
173 for (
size_t i = 0; i < N; i++)
176 i, 0, A.get_unsafe(i, 1) *
v[2] - A.get_unsafe(i, 2) *
v[1]);
178 i, 1, -A.get_unsafe(i, 0) *
v[2] + A.get_unsafe(i, 2) *
v[0]);
180 i, 2, A.get_unsafe(i, 0) *
v[1] - A.get_unsafe(i, 1) *
v[0]);
189 template <
class SKEW_3VECTOR,
class MAT_A,
class MAT_OUT>
195 const size_t N = A.cols();
197 for (
size_t i = 0; i < N; i++)
200 0, i, -A.get_unsafe(1, i) *
v[2] + A.get_unsafe(2, i) *
v[1]);
202 1, i, A.get_unsafe(0, i) *
v[2] - A.get_unsafe(2, i) *
v[0]);
204 2, i, -A.get_unsafe(0, i) *
v[1] + A.get_unsafe(1, i) *
v[0]);
214 template <
class MATORG,
class MATDEST>
216 const MATORG& M,
const size_t first_row,
const size_t first_col,
219 const size_t NR = outMat.rows();
220 const size_t NC = outMat.cols();
223 for (
size_t r = 0;
r < NR;
r++)
224 for (
size_t c = 0;
c < NC;
c++)
225 outMat.get_unsafe(
r,
c) =
226 M.get_unsafe(first_row +
r, first_col +
c);