Main MRPT website > C++ reference for MRPT 1.4.0
ops_matrices.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef mrpt_math_matrix_ops_H
10#define mrpt_math_matrix_ops_H
11
12#include <mrpt/math/math_frwds.h> // forward declarations
13#include <mrpt/math/eigen_frwds.h> // forward declarations
14
17
18#include <mrpt/math/ops_containers.h> // Many generic operations
19
20/** \file ops_matrices.h
21 * This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt::math::detail
22 */
23namespace mrpt
24{
25 namespace math
26 {
27 /** \addtogroup container_ops_grp
28 * @{ */
29
30 /** Transpose operator for matrices */
31 template <class Derived>
32 inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator ~(const Eigen::MatrixBase<Derived> &m) {
33 return m.adjoint();
34 }
35
36 /** Unary inversion operator. */
37 template <class Derived>
38 inline typename Eigen::MatrixBase<Derived>::PlainObject operator !(const Eigen::MatrixBase<Derived> &m) {
39 return m.inv();
40 }
41
42 /** @} */ // end MRPT matrices operators
43
44
45 /** R = H * C * H^t (with C symmetric) */
46 template <typename MAT_H, typename MAT_C, typename MAT_R>
47 inline void multiply_HCHt(
48 const MAT_H &H,
49 const MAT_C &C,
50 MAT_R &R,
51 bool accumResultInOutput ) // bool allow_submatrix_mult)
52 {
53 if (accumResultInOutput)
54 R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
55 else
56 R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
57 }
58
59 /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
60 template <typename VECTOR_H, typename MAT_C>
61 typename MAT_C::Scalar
62 multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
63 {
64 return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
65 }
66
67 /** R = H^t * C * H (with C symmetric) */
68 template <typename MAT_H, typename MAT_C, typename MAT_R>
70 const MAT_H &H,
71 const MAT_C &C,
72 MAT_R &R,
73 bool accumResultInOutput) // bool allow_submatrix_mult)
74 {
75 if (accumResultInOutput)
76 R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
77 else
78 R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
79 }
80
81
82 /** Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
83 * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
84 * \param out_mean The output M-vector for the estimated mean.
85 * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
86 * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
87 */
88 template<class MAT_IN,class VECTOR, class MAT_OUT>
90 const MAT_IN & v,
91 VECTOR & out_mean,
92 MAT_OUT & out_cov
93 )
94 {
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;
98
99 const size_t M = v.cols();
100 ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
101
102 // First: Compute the mean
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);
107 out_mean*=N_inv;
108
109 // Second: Compute the covariance
110 // Save only the above-diagonal part, then after averaging
111 // duplicate that part to the other half.
112 out_cov.zeros(M,M);
113 for (size_t i=0;i<N;i++)
114 {
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]);
117
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]);
121 }
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);
125 out_cov*=N_inv;
126 }
127
128 /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
129 * \param v The set of data, as a NxM matrix.
130 * \param out_cov The output MxM matrix for the estimated covariance matrix.
131 * \sa math::mean,math::stddev, math::cov
132 */
133 template<class MATRIX>
134 inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
135 cov( const MATRIX &v )
136 {
137 Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
138 Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
139 meanAndCovMat(v,m,C);
140 return C;
141 }
142
143 /** A useful macro for saving matrixes to a file while debugging. */
144 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
145
146
147 /** Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetric matric generated from \a v (see mrpt::math::skew_symmetric3)
148 */
149 template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT>
150 void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out)
151 {
153 ASSERT_EQUAL_(size(A,2),3)
154 ASSERT_EQUAL_(v.size(),3)
155 const size_t N = size(A,1);
156 out.setSize(N,3);
157 for (size_t i=0;i<N;i++)
158 {
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] );
162 }
164 }
165
166 /** Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetric matric generated from \a v (see mrpt::math::skew_symmetric3)
167 */
168 template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT>
169 void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A,MAT_OUT &out)
170 {
172 ASSERT_EQUAL_(size(A,1),3)
173 ASSERT_EQUAL_(v.size(),3)
174 const size_t N = size(A,2);
175 out.setSize(3,N);
176 for (size_t i=0;i<N;i++)
177 {
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] );
181 }
183 }
184
185
186 // ------ Implementatin of detail functions -------------
187 namespace detail
188 {
189 /** Extract a submatrix - The output matrix must be set to the required size before call. */
190 template <class MATORG, class MATDEST>
192 const MATORG &M,
193 const size_t first_row,
194 const size_t first_col,
195 MATDEST &outMat)
196 {
197 const size_t NR = outMat.getRowCount();
198 const size_t NC = outMat.getColCount();
199 ASSERT_BELOWEQ_( first_row+NR, M.getRowCount() )
200 ASSERT_BELOWEQ_( first_col+NC, M.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);
204 }
205
206 } // end of detail namespace
207
208 /** @} */ // end of grouping
209
210 } // End of math namespace
211} // End of mrpt namespace
212
213
214#endif
Eigen::MatrixBase< Derived >::PlainObject operator!(const Eigen::MatrixBase< Derived > &m)
Unary inversion operator.
Definition: ops_matrices.h:38
const Eigen::MatrixBase< Derived >::AdjointReturnType operator~(const Eigen::MatrixBase< Derived > &m)
Transpose operator for matrices.
Definition: ops_matrices.h:32
#define ASSERT_EQUAL_(__A, __B)
Definition: mrpt_macros.h:264
#define MRPT_START
Definition: mrpt_macros.h:349
#define MRPT_END
Definition: mrpt_macros.h:353
#define ASSERT_BELOWEQ_(__A, __B)
Definition: mrpt_macros.h:268
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
void extractMatrix(const MATORG &M, const size_t first_row, const size_t first_col, MATDEST &outMat)
Extract a submatrix - The output matrix must be set to the required size before call.
Definition: ops_matrices.h:191
void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A, MAT_OUT &out)
Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetri...
Definition: ops_matrices.h:169
void multiply_HtCH(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput)
R = H^t * C * H (with C symmetric)
Definition: ops_matrices.h:69
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
Definition: ops_matrices.h:135
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:62
void multiply_A_skew3(const MAT_A &A, const SKEW_3VECTOR &v, MAT_OUT &out)
Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetri...
Definition: ops_matrices.h:150
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput)
R = H * C * H^t (with C symmetric)
Definition: ops_matrices.h:47
void meanAndCovMat(const MAT_IN &v, VECTOR &out_mean, MAT_OUT &out_cov)
Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a ...
Definition: ops_matrices.h:89
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This file implements several operations that operate element-wise on individual or pairs of container...



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:36:32 UTC 2021