9 #ifndef mrpt_math_matrix_ops_H
10 #define mrpt_math_matrix_ops_H
31 template <
class Derived>
32 inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType
operator ~(
const Eigen::MatrixBase<Derived> &m) {
37 template <
class Derived>
38 inline typename Eigen::MatrixBase<Derived>::PlainObject
operator !(
const Eigen::MatrixBase<Derived> &m) {
46 template <
typename MAT_H,
typename MAT_C,
typename MAT_R>
51 bool accumResultInOutput )
53 if (accumResultInOutput)
54 R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
56 R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
60 template <
typename VECTOR_H,
typename MAT_C>
61 typename MAT_C::Scalar
64 return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
68 template <
typename MAT_H,
typename MAT_C,
typename MAT_R>
73 bool accumResultInOutput)
75 if (accumResultInOutput)
76 R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
78 R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
88 template<
class MAT_IN,
class VECTOR,
class MAT_OUT>
95 const size_t N = v.rows();
96 ASSERTMSG_(N>0,
"The input matrix contains no elements");
97 const double N_inv = 1.0/N;
99 const size_t M = v.cols();
100 ASSERTMSG_(M>0,
"The input matrix contains rows of length 0");
103 out_mean.assign(M,0);
104 for (
size_t i=0;i<N;i++)
105 for (
size_t j=0;j<M;j++)
106 out_mean[j]+=v.coeff(i,j);
113 for (
size_t i=0;i<N;i++)
115 for (
size_t j=0;j<M;j++)
116 out_cov.get_unsafe(j,j)+=
square(v.get_unsafe(i,j)-out_mean[j]);
118 for (
size_t j=0;j<M;j++)
119 for (
size_t k=j+1;k<M;k++)
120 out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
122 for (
size_t j=0;j<M;j++)
123 for (
size_t k=j+1;k<M;k++)
124 out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
133 template<
class MATRIX>
134 inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
137 Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
138 Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
144 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
149 template <
class MAT_A,
class SKEW_3VECTOR,
class MAT_OUT>
155 const size_t N =
size(A,1);
157 for (
size_t i=0;i<N;i++)
159 out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
160 out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
161 out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
168 template <
class SKEW_3VECTOR,
class MAT_A,
class MAT_OUT>
174 const size_t N =
size(A,2);
176 for (
size_t i=0;i<N;i++)
178 out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
179 out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
180 out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
190 template <
class MATORG,
class MATDEST>
193 const size_t first_row,
194 const size_t first_col,
197 const size_t NR = outMat.getRowCount();
198 const size_t NC = outMat.getColCount();
201 for (
size_t r=0;r<NR;r++)
202 for (
size_t c=0;c<NC;c++)
203 outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);