00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef mrpt_math_matrix_ops_H
00029 #define mrpt_math_matrix_ops_H
00030
00031 #include <mrpt/math/math_frwds.h>
00032
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 #include <mrpt/utils/CStream.h>
00036
00037 #include <mrpt/math/CMatrixTemplateNumeric.h>
00038 #include <mrpt/math/CMatrixFixedNumeric.h>
00039
00040 #include <mrpt/math/ops_containers.h>
00041
00042 #include <mrpt/utils/metaprogramming.h>
00043
00044
00045
00046
00047
00048
00049
00050 namespace mrpt
00051 {
00052 namespace math
00053 {
00054
00055
00056
00057
00058 template <size_t NROWS,size_t NCOLS>
00059 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
00060 CMatrix aux;
00061 in.ReadObject(&aux);
00062 ASSERTMSG_(M.cols()==aux.cols() && M.rows()==aux.rows(), format("Size mismatch: deserialized is %ux%u, expected is %ux%u",(unsigned)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(unsigned)NCOLS))
00063 M = aux;
00064 return in;
00065 }
00066
00067 template <size_t NROWS,size_t NCOLS>
00068 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
00069 CMatrixD aux;
00070 in.ReadObject(&aux);
00071 ASSERTMSG_(M.cols()==aux.cols() && M.rows()==aux.rows(), format("Size mismatch: deserialized is %ux%u, expected is %ux%u",(unsigned)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(unsigned)NCOLS))
00072 M = aux;
00073 return in;
00074 }
00075
00076
00077 template <size_t NROWS,size_t NCOLS>
00078 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
00079 CMatrix aux = CMatrixFloat(M);
00080 out.WriteObject(&aux);
00081 return out;
00082 }
00083
00084 template <size_t NROWS,size_t NCOLS>
00085 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
00086 CMatrixD aux = CMatrixDouble(M);
00087 out.WriteObject(&aux);
00088 return out;
00089 }
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 template <typename T,size_t NROWS,size_t NCOLS>
00100 inline std::ostream & operator << (std::ostream & s, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
00101 {
00102 Eigen::IOFormat fmt; fmt.matSuffix="\n";
00103 return s << m.format(fmt);
00104 }
00105
00106
00107 template<typename T>
00108 inline std::ostream & operator << (std::ostream & s, const CMatrixTemplateNumeric<T>& m)
00109 {
00110 Eigen::IOFormat fmt; fmt.matSuffix="\n";
00111 return s << m.format(fmt);
00112 }
00113
00114
00115 template<typename T>
00116 inline std::ostream & operator << (std::ostream & s, const mrpt::dynamicsize_vector<T>& m)
00117 {
00118 Eigen::IOFormat fmt; fmt.rowSeparator=" "; fmt.matPrefix="["; fmt.matSuffix="]";
00119 return s << m.format(fmt);
00120 }
00121
00122
00123
00124
00125
00126 template <class Derived>
00127 inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator ~(const Eigen::MatrixBase<Derived> &m) {
00128 return m.adjoint();
00129 }
00130
00131
00132 template <class Derived>
00133 inline typename Eigen::MatrixBase<Derived>::PlainObject operator !(const Eigen::MatrixBase<Derived> &m) {
00134 return m.inv();
00135 }
00136
00137
00138
00139
00140
00141 template <typename MAT_H, typename MAT_C, typename MAT_R>
00142 inline void multiply_HCHt(
00143 const MAT_H &H,
00144 const MAT_C &C,
00145 MAT_R &R,
00146 bool accumResultInOutput )
00147 {
00148 if (accumResultInOutput)
00149 R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
00150 else
00151 R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
00152 }
00153
00154
00155 template <typename VECTOR_H, typename MAT_C>
00156 typename MAT_C::value_type
00157 multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
00158 {
00159 return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
00160 }
00161
00162
00163 template <typename MAT_H, typename MAT_C, typename MAT_R>
00164 void multiply_HtCH(
00165 const MAT_H &H,
00166 const MAT_C &C,
00167 MAT_R &R,
00168 bool accumResultInOutput)
00169 {
00170 if (accumResultInOutput)
00171 R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
00172 else
00173 R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
00174 }
00175
00176
00177
00178
00179
00180
00181
00182
00183 template<class MAT_IN,class VECTOR, class MAT_OUT>
00184 void meanAndCovMat(
00185 const MAT_IN & v,
00186 VECTOR & out_mean,
00187 MAT_OUT & out_cov
00188 )
00189 {
00190 const size_t N = v.rows();
00191 ASSERTMSG_(N>0,"The input matrix contains no elements");
00192 const double N_inv = 1.0/N;
00193
00194 const size_t M = v.cols();
00195 ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
00196
00197
00198 out_mean.assign(M,0);
00199 for (size_t i=0;i<N;i++)
00200 for (size_t j=0;j<M;j++)
00201 out_mean[j]+=v.coeff(i,j);
00202 out_mean*=N_inv;
00203
00204
00205
00206
00207 out_cov.zeros(M,M);
00208 for (size_t i=0;i<N;i++)
00209 {
00210 for (size_t j=0;j<M;j++)
00211 out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);
00212
00213 for (size_t j=0;j<M;j++)
00214 for (size_t k=j+1;k<M;k++)
00215 out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
00216 }
00217 for (size_t j=0;j<M;j++)
00218 for (size_t k=j+1;k<M;k++)
00219 out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
00220 out_cov*=N_inv;
00221 }
00222
00223
00224
00225
00226
00227
00228 template<class MATRIX>
00229 inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
00230 cov( const MATRIX &v )
00231 {
00232 Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
00233 Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
00234 meanAndCovMat(v,m,C);
00235 return C;
00236 }
00237
00238
00239 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
00240
00241
00242
00243
00244 template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT>
00245 void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out)
00246 {
00247 MRPT_START
00248 ASSERT_EQUAL_(size(A,2),3)
00249 ASSERT_EQUAL_(v.size(),3)
00250 const size_t N = size(A,1);
00251 out.setSize(N,3);
00252 for (size_t i=0;i<N;i++)
00253 {
00254 out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
00255 out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
00256 out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
00257 }
00258 MRPT_END
00259 }
00260
00261
00262
00263 template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT>
00264 void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A,MAT_OUT &out)
00265 {
00266 MRPT_START
00267 ASSERT_EQUAL_(size(A,1),3)
00268 ASSERT_EQUAL_(v.size(),3)
00269 const size_t N = size(A,2);
00270 out.setSize(3,N);
00271 for (size_t i=0;i<N;i++)
00272 {
00273 out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
00274 out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
00275 out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
00276 }
00277 MRPT_END
00278 }
00279
00280
00281
00282 namespace detail
00283 {
00284
00285 template <class MATORG, class MATDEST>
00286 void extractMatrix(
00287 const MATORG &M,
00288 const size_t first_row,
00289 const size_t first_col,
00290 MATDEST &outMat)
00291 {
00292 const size_t NR = outMat.getRowCount();
00293 const size_t NC = outMat.getColCount();
00294 ASSERT_BELOWEQ_( first_row+NR, M.getRowCount() )
00295 ASSERT_BELOWEQ_( first_col+NC, M.getColCount() )
00296 for (size_t r=0;r<NR;r++)
00297 for (size_t c=0;c<NC;c++)
00298 outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
00299 }
00300
00301 }
00302
00303 }
00304 }
00305
00306
00307 #endif