Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
DiagonalMatrix.h
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> 00005 // Copyright (C) 2007-2009 Benoit Jacob <jacob.benoit.1@gmail.com> 00006 // 00007 // This Source Code Form is subject to the terms of the Mozilla 00008 // Public License v. 2.0. If a copy of the MPL was not distributed 00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00010 00011 #ifndef EIGEN_DIAGONALMATRIX_H 00012 #define EIGEN_DIAGONALMATRIX_H 00013 00014 namespace Eigen { 00015 00016 #ifndef EIGEN_PARSED_BY_DOXYGEN 00017 template<typename Derived> 00018 class DiagonalBase : public EigenBase<Derived> 00019 { 00020 public: 00021 typedef typename internal::traits<Derived>::DiagonalVectorType DiagonalVectorType; 00022 typedef typename DiagonalVectorType::Scalar Scalar; 00023 typedef typename DiagonalVectorType::RealScalar RealScalar; 00024 typedef typename internal::traits<Derived>::StorageKind StorageKind; 00025 typedef typename internal::traits<Derived>::Index Index; 00026 00027 enum { 00028 RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, 00029 ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, 00030 MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, 00031 MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, 00032 IsVectorAtCompileTime = 0, 00033 Flags = 0 00034 }; 00035 00036 typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime, 0, MaxRowsAtCompileTime, MaxColsAtCompileTime> DenseMatrixType; 00037 typedef DenseMatrixType DenseType; 00038 typedef DiagonalMatrix<Scalar,DiagonalVectorType::SizeAtCompileTime,DiagonalVectorType::MaxSizeAtCompileTime> PlainObject; 00039 00040 inline const Derived& derived() const { return *static_cast<const Derived*>(this); } 00041 inline Derived& derived() { return *static_cast<Derived*>(this); } 00042 00043 DenseMatrixType toDenseMatrix() const { return derived(); } 00044 template<typename DenseDerived> 00045 void evalTo(MatrixBase<DenseDerived> &other) const; 00046 template<typename DenseDerived> 00047 void addTo(MatrixBase<DenseDerived> &other) const 00048 { other.diagonal() += diagonal(); } 00049 template<typename DenseDerived> 00050 void subTo(MatrixBase<DenseDerived> &other) const 00051 { other.diagonal() -= diagonal(); } 00052 00053 inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); } 00054 inline DiagonalVectorType& diagonal() { return derived().diagonal(); } 00055 00056 inline Index rows() const { return diagonal().size(); } 00057 inline Index cols() const { return diagonal().size(); } 00058 00059 /** \returns the diagonal matrix product of \c *this by the matrix \a matrix. 00060 */ 00061 template<typename MatrixDerived> 00062 const DiagonalProduct<MatrixDerived, Derived, OnTheLeft> 00063 operator* (const MatrixBase<MatrixDerived> &matrix) const 00064 { 00065 return DiagonalProduct<MatrixDerived, Derived, OnTheLeft>(matrix.derived(), derived()); 00066 } 00067 00068 inline const DiagonalWrapper<const CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const DiagonalVectorType> > 00069 inverse() const 00070 { 00071 return diagonal().cwiseInverse(); 00072 } 00073 00074 inline const DiagonalWrapper<const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DiagonalVectorType> > 00075 operator* (const Scalar& scalar) const 00076 { 00077 return diagonal() * scalar; 00078 } 00079 friend inline const DiagonalWrapper<const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DiagonalVectorType> > 00080 operator* (const Scalar& scalar, const DiagonalBase& other) 00081 { 00082 return other.diagonal() * scalar; 00083 } 00084 00085 #ifdef EIGEN2_SUPPORT 00086 template<typename OtherDerived> 00087 bool isApprox(const DiagonalBase<OtherDerived>& other, typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision()) const 00088 { 00089 return diagonal().isApprox(other.diagonal(), precision); 00090 } 00091 template<typename OtherDerived> 00092 bool isApprox(const MatrixBase<OtherDerived>& other, typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision()) const 00093 { 00094 return toDenseMatrix().isApprox(other, precision); 00095 } 00096 #endif 00097 }; 00098 00099 template<typename Derived> 00100 template<typename DenseDerived> 00101 void DiagonalBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const 00102 { 00103 other.setZero(); 00104 other.diagonal() = diagonal(); 00105 } 00106 #endif 00107 00108 /** \class DiagonalMatrix 00109 * \ingroup Core_Module 00110 * 00111 * \brief Represents a diagonal matrix with its storage 00112 * 00113 * \param _Scalar the type of coefficients 00114 * \param SizeAtCompileTime the dimension of the matrix, or Dynamic 00115 * \param MaxSizeAtCompileTime the dimension of the matrix, or Dynamic. This parameter is optional and defaults 00116 * to SizeAtCompileTime. Most of the time, you do not need to specify it. 00117 * 00118 * \sa class DiagonalWrapper 00119 */ 00120 00121 namespace internal { 00122 template<typename _Scalar, int SizeAtCompileTime, int MaxSizeAtCompileTime> 00123 struct traits<DiagonalMatrix<_Scalar,SizeAtCompileTime,MaxSizeAtCompileTime> > 00124 : traits<Matrix<_Scalar,SizeAtCompileTime,SizeAtCompileTime,0,MaxSizeAtCompileTime,MaxSizeAtCompileTime> > 00125 { 00126 typedef Matrix<_Scalar,SizeAtCompileTime,1,0,MaxSizeAtCompileTime,1> DiagonalVectorType; 00127 typedef Dense StorageKind; 00128 typedef DenseIndex Index; 00129 enum { 00130 Flags = LvalueBit 00131 }; 00132 }; 00133 } 00134 template<typename _Scalar, int SizeAtCompileTime, int MaxSizeAtCompileTime> 00135 class DiagonalMatrix 00136 : public DiagonalBase<DiagonalMatrix<_Scalar,SizeAtCompileTime,MaxSizeAtCompileTime> > 00137 { 00138 public: 00139 #ifndef EIGEN_PARSED_BY_DOXYGEN 00140 typedef typename internal::traits<DiagonalMatrix>::DiagonalVectorType DiagonalVectorType; 00141 typedef const DiagonalMatrix& Nested; 00142 typedef _Scalar Scalar; 00143 typedef typename internal::traits<DiagonalMatrix>::StorageKind StorageKind; 00144 typedef typename internal::traits<DiagonalMatrix>::Index Index; 00145 #endif 00146 00147 protected: 00148 00149 DiagonalVectorType m_diagonal; 00150 00151 public: 00152 00153 /** const version of diagonal(). */ 00154 inline const DiagonalVectorType& diagonal() const { return m_diagonal; } 00155 /** \returns a reference to the stored vector of diagonal coefficients. */ 00156 inline DiagonalVectorType& diagonal () { return m_diagonal; } 00157 00158 /** Default constructor without initialization */ 00159 inline DiagonalMatrix() {} 00160 00161 /** Constructs a diagonal matrix with given dimension */ 00162 inline DiagonalMatrix(Index dim) : m_diagonal(dim) {} 00163 00164 /** 2D constructor. */ 00165 inline DiagonalMatrix(const Scalar& x, const Scalar& y) : m_diagonal(x,y) {} 00166 00167 /** 3D constructor. */ 00168 inline DiagonalMatrix(const Scalar& x, const Scalar& y, const Scalar& z) : m_diagonal(x,y,z) {} 00169 00170 /** Copy constructor. */ 00171 template<typename OtherDerived> 00172 inline DiagonalMatrix(const DiagonalBase<OtherDerived>& other) : m_diagonal(other.diagonal()) {} 00173 00174 #ifndef EIGEN_PARSED_BY_DOXYGEN 00175 /** copy constructor. prevent a default copy constructor from hiding the other templated constructor */ 00176 inline DiagonalMatrix(const DiagonalMatrix& other) : m_diagonal(other.diagonal()) {} 00177 #endif 00178 00179 /** generic constructor from expression of the diagonal coefficients */ 00180 template<typename OtherDerived> 00181 explicit inline DiagonalMatrix(const MatrixBase<OtherDerived>& other) : m_diagonal(other) 00182 {} 00183 00184 /** Copy operator. */ 00185 template<typename OtherDerived> 00186 DiagonalMatrix& operator=(const DiagonalBase<OtherDerived>& other) 00187 { 00188 m_diagonal = other.diagonal(); 00189 return *this; 00190 } 00191 00192 #ifndef EIGEN_PARSED_BY_DOXYGEN 00193 /** This is a special case of the templated operator=. Its purpose is to 00194 * prevent a default operator= from hiding the templated operator=. 00195 */ 00196 DiagonalMatrix& operator=(const DiagonalMatrix& other) 00197 { 00198 m_diagonal = other.diagonal(); 00199 return *this; 00200 } 00201 #endif 00202 00203 /** Resizes to given size. */ 00204 inline void resize(Index size ) { m_diagonal.resize(size); } 00205 /** Sets all coefficients to zero. */ 00206 inline void setZero() { m_diagonal.setZero(); } 00207 /** Resizes and sets all coefficients to zero. */ 00208 inline void setZero(Index size ) { m_diagonal.setZero(size); } 00209 /** Sets this matrix to be the identity matrix of the current size. */ 00210 inline void setIdentity() { m_diagonal.setOnes(); } 00211 /** Sets this matrix to be the identity matrix of the given size. */ 00212 inline void setIdentity(Index size ) { m_diagonal.setOnes(size); } 00213 }; 00214 00215 /** \class DiagonalWrapper 00216 * \ingroup Core_Module 00217 * 00218 * \brief Expression of a diagonal matrix 00219 * 00220 * \param _DiagonalVectorType the type of the vector of diagonal coefficients 00221 * 00222 * This class is an expression of a diagonal matrix, but not storing its own vector of diagonal coefficients, 00223 * instead wrapping an existing vector expression. It is the return type of MatrixBase::asDiagonal() 00224 * and most of the time this is the only way that it is used. 00225 * 00226 * \sa class DiagonalMatrix, class DiagonalBase, MatrixBase::asDiagonal() 00227 */ 00228 00229 namespace internal { 00230 template<typename _DiagonalVectorType> 00231 struct traits<DiagonalWrapper<_DiagonalVectorType> > 00232 { 00233 typedef _DiagonalVectorType DiagonalVectorType; 00234 typedef typename DiagonalVectorType::Scalar Scalar; 00235 typedef typename DiagonalVectorType::Index Index; 00236 typedef typename DiagonalVectorType::StorageKind StorageKind; 00237 enum { 00238 RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, 00239 ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, 00240 MaxRowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, 00241 MaxColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, 00242 Flags = traits<DiagonalVectorType>::Flags & LvalueBit 00243 }; 00244 }; 00245 } 00246 00247 template<typename _DiagonalVectorType> 00248 class DiagonalWrapper 00249 : public DiagonalBase<DiagonalWrapper<_DiagonalVectorType> >, internal::no_assignment_operator 00250 { 00251 public: 00252 #ifndef EIGEN_PARSED_BY_DOXYGEN 00253 typedef _DiagonalVectorType DiagonalVectorType; 00254 typedef DiagonalWrapper Nested; 00255 #endif 00256 00257 /** Constructor from expression of diagonal coefficients to wrap. */ 00258 inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {} 00259 00260 /** \returns a const reference to the wrapped expression of diagonal coefficients. */ 00261 const DiagonalVectorType& diagonal () const { return m_diagonal; } 00262 00263 protected: 00264 typename DiagonalVectorType::Nested m_diagonal; 00265 }; 00266 00267 /** \returns a pseudo-expression of a diagonal matrix with *this as vector of diagonal coefficients 00268 * 00269 * \only_for_vectors 00270 * 00271 * Example: \include MatrixBase_asDiagonal.cpp 00272 * Output: \verbinclude MatrixBase_asDiagonal.out 00273 * 00274 * \sa class DiagonalWrapper, class DiagonalMatrix, diagonal(), isDiagonal() 00275 **/ 00276 template<typename Derived> 00277 inline const DiagonalWrapper<const Derived> 00278 MatrixBase<Derived>::asDiagonal () const 00279 { 00280 return derived(); 00281 } 00282 00283 /** \returns true if *this is approximately equal to a diagonal matrix, 00284 * within the precision given by \a prec. 00285 * 00286 * Example: \include MatrixBase_isDiagonal.cpp 00287 * Output: \verbinclude MatrixBase_isDiagonal.out 00288 * 00289 * \sa asDiagonal() 00290 */ 00291 template<typename Derived> 00292 bool MatrixBase<Derived>::isDiagonal (const RealScalar& prec) const 00293 { 00294 using std::abs; 00295 if(cols() != rows()) return false; 00296 RealScalar maxAbsOnDiagonal = static_cast<RealScalar>(-1); 00297 for(Index j = 0; j < cols(); ++j) 00298 { 00299 RealScalar absOnDiagonal = abs(coeff(j,j)); 00300 if(absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal; 00301 } 00302 for(Index j = 0; j < cols(); ++j) 00303 for(Index i = 0; i < j; ++i) 00304 { 00305 if(!internal::isMuchSmallerThan(coeff(i, j), maxAbsOnDiagonal, prec)) return false; 00306 if(!internal::isMuchSmallerThan(coeff(j, i), maxAbsOnDiagonal, prec)) return false; 00307 } 00308 return true; 00309 } 00310 00311 } // end namespace Eigen 00312 00313 #endif // EIGEN_DIAGONALMATRIX_H
Generated on Tue Jul 12 2022 17:46:52 by 1.7.2