Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
TriangularMatrixVector.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 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef EIGEN_TRIANGULARMATRIXVECTOR_H 00011 #define EIGEN_TRIANGULARMATRIXVECTOR_H 00012 00013 namespace Eigen { 00014 00015 namespace internal { 00016 00017 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int StorageOrder, int Version=Specialized> 00018 struct triangular_matrix_vector_product; 00019 00020 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version> 00021 struct triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,ColMajor,Version> 00022 { 00023 typedef typename scalar_product_traits<LhsScalar, RhsScalar>::ReturnType ResScalar; 00024 enum { 00025 IsLower = ((Mode&Lower)==Lower), 00026 HasUnitDiag = (Mode & UnitDiag)==UnitDiag, 00027 HasZeroDiag = (Mode & ZeroDiag)==ZeroDiag 00028 }; 00029 static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride, 00030 const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha); 00031 }; 00032 00033 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version> 00034 EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,ColMajor,Version> 00035 ::run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride, 00036 const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha) 00037 { 00038 static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH; 00039 Index size = (std::min)(_rows,_cols); 00040 Index rows = IsLower ? _rows : (std::min)(_rows,_cols); 00041 Index cols = IsLower ? (std::min)(_rows,_cols) : _cols; 00042 00043 typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > LhsMap; 00044 const LhsMap lhs(_lhs,rows,cols,OuterStride<>(lhsStride)); 00045 typename conj_expr_if<ConjLhs,LhsMap>::type cjLhs(lhs); 00046 00047 typedef Map<const Matrix<RhsScalar,Dynamic,1>, 0, InnerStride<> > RhsMap; 00048 const RhsMap rhs(_rhs,cols,InnerStride<>(rhsIncr)); 00049 typename conj_expr_if<ConjRhs,RhsMap>::type cjRhs(rhs); 00050 00051 typedef Map<Matrix<ResScalar,Dynamic,1> > ResMap; 00052 ResMap res(_res,rows); 00053 00054 for (Index pi=0; pi<size; pi+=PanelWidth) 00055 { 00056 Index actualPanelWidth = (std::min)(PanelWidth, size-pi); 00057 for (Index k=0; k<actualPanelWidth; ++k) 00058 { 00059 Index i = pi + k; 00060 Index s = IsLower ? ((HasUnitDiag||HasZeroDiag) ? i+1 : i ) : pi; 00061 Index r = IsLower ? actualPanelWidth-k : k+1; 00062 if ((!(HasUnitDiag||HasZeroDiag)) || (--r)>0) 00063 res.segment(s,r) += (alpha * cjRhs.coeff(i)) * cjLhs.col(i).segment(s,r); 00064 if (HasUnitDiag) 00065 res.coeffRef(i) += alpha * cjRhs.coeff(i); 00066 } 00067 Index r = IsLower ? rows - pi - actualPanelWidth : pi; 00068 if (r>0) 00069 { 00070 Index s = IsLower ? pi+actualPanelWidth : 0; 00071 general_matrix_vector_product<Index,LhsScalar,ColMajor,ConjLhs,RhsScalar,ConjRhs,BuiltIn>::run( 00072 r, actualPanelWidth, 00073 &lhs.coeffRef(s,pi), lhsStride, 00074 &rhs.coeffRef(pi), rhsIncr, 00075 &res.coeffRef(s), resIncr, alpha); 00076 } 00077 } 00078 if((!IsLower) && cols>size) 00079 { 00080 general_matrix_vector_product<Index,LhsScalar,ColMajor,ConjLhs,RhsScalar,ConjRhs>::run( 00081 rows, cols-size, 00082 &lhs.coeffRef(0,size), lhsStride, 00083 &rhs.coeffRef(size), rhsIncr, 00084 _res, resIncr, alpha); 00085 } 00086 } 00087 00088 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,int Version> 00089 struct triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,RowMajor,Version> 00090 { 00091 typedef typename scalar_product_traits<LhsScalar, RhsScalar>::ReturnType ResScalar; 00092 enum { 00093 IsLower = ((Mode&Lower)==Lower), 00094 HasUnitDiag = (Mode & UnitDiag)==UnitDiag, 00095 HasZeroDiag = (Mode & ZeroDiag)==ZeroDiag 00096 }; 00097 static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride, 00098 const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha); 00099 }; 00100 00101 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,int Version> 00102 EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,RowMajor,Version> 00103 ::run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride, 00104 const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha) 00105 { 00106 static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH; 00107 Index diagSize = (std::min)(_rows,_cols); 00108 Index rows = IsLower ? _rows : diagSize; 00109 Index cols = IsLower ? diagSize : _cols; 00110 00111 typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,RowMajor>, 0, OuterStride<> > LhsMap; 00112 const LhsMap lhs(_lhs,rows,cols,OuterStride<>(lhsStride)); 00113 typename conj_expr_if<ConjLhs,LhsMap>::type cjLhs(lhs); 00114 00115 typedef Map<const Matrix<RhsScalar,Dynamic,1> > RhsMap; 00116 const RhsMap rhs(_rhs,cols); 00117 typename conj_expr_if<ConjRhs,RhsMap>::type cjRhs(rhs); 00118 00119 typedef Map<Matrix<ResScalar,Dynamic,1>, 0, InnerStride<> > ResMap; 00120 ResMap res(_res,rows,InnerStride<>(resIncr)); 00121 00122 for (Index pi=0; pi<diagSize; pi+=PanelWidth) 00123 { 00124 Index actualPanelWidth = (std::min)(PanelWidth, diagSize-pi); 00125 for (Index k=0; k<actualPanelWidth; ++k) 00126 { 00127 Index i = pi + k; 00128 Index s = IsLower ? pi : ((HasUnitDiag||HasZeroDiag) ? i+1 : i); 00129 Index r = IsLower ? k+1 : actualPanelWidth-k; 00130 if ((!(HasUnitDiag||HasZeroDiag)) || (--r)>0) 00131 res.coeffRef(i) += alpha * (cjLhs.row(i).segment(s,r).cwiseProduct(cjRhs.segment(s,r).transpose())).sum(); 00132 if (HasUnitDiag) 00133 res.coeffRef(i) += alpha * cjRhs.coeff(i); 00134 } 00135 Index r = IsLower ? pi : cols - pi - actualPanelWidth; 00136 if (r>0) 00137 { 00138 Index s = IsLower ? 0 : pi + actualPanelWidth; 00139 general_matrix_vector_product<Index,LhsScalar,RowMajor,ConjLhs,RhsScalar,ConjRhs,BuiltIn>::run( 00140 actualPanelWidth, r, 00141 &lhs.coeffRef(pi,s), lhsStride, 00142 &rhs.coeffRef(s), rhsIncr, 00143 &res.coeffRef(pi), resIncr, alpha); 00144 } 00145 } 00146 if(IsLower && rows>diagSize) 00147 { 00148 general_matrix_vector_product<Index,LhsScalar,RowMajor,ConjLhs,RhsScalar,ConjRhs>::run( 00149 rows-diagSize, cols, 00150 &lhs.coeffRef(diagSize,0), lhsStride, 00151 &rhs.coeffRef(0), rhsIncr, 00152 &res.coeffRef(diagSize), resIncr, alpha); 00153 } 00154 } 00155 00156 /*************************************************************************** 00157 * Wrapper to product_triangular_vector 00158 ***************************************************************************/ 00159 00160 template<int Mode, bool LhsIsTriangular, typename Lhs, typename Rhs> 00161 struct traits<TriangularProduct<Mode,LhsIsTriangular,Lhs,false,Rhs,true> > 00162 : traits<ProductBase<TriangularProduct<Mode,LhsIsTriangular,Lhs,false,Rhs,true>, Lhs, Rhs> > 00163 {}; 00164 00165 template<int Mode, bool LhsIsTriangular, typename Lhs, typename Rhs> 00166 struct traits<TriangularProduct<Mode,LhsIsTriangular,Lhs,true,Rhs,false> > 00167 : traits<ProductBase<TriangularProduct<Mode,LhsIsTriangular,Lhs,true,Rhs,false>, Lhs, Rhs> > 00168 {}; 00169 00170 00171 template<int StorageOrder> 00172 struct trmv_selector; 00173 00174 } // end namespace internal 00175 00176 template<int Mode, typename Lhs, typename Rhs> 00177 struct TriangularProduct<Mode,true,Lhs,false,Rhs,true> 00178 : public ProductBase<TriangularProduct<Mode,true,Lhs,false,Rhs,true>, Lhs, Rhs > 00179 { 00180 EIGEN_PRODUCT_PUBLIC_INTERFACE(TriangularProduct) 00181 00182 TriangularProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {} 00183 00184 template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const 00185 { 00186 eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols()); 00187 00188 internal::trmv_selector<(int(internal::traits<Lhs>::Flags)&RowMajorBit) ? RowMajor : ColMajor>::run(*this, dst, alpha); 00189 } 00190 }; 00191 00192 template<int Mode, typename Lhs, typename Rhs> 00193 struct TriangularProduct<Mode,false,Lhs,true,Rhs,false> 00194 : public ProductBase<TriangularProduct<Mode,false,Lhs,true,Rhs,false>, Lhs, Rhs > 00195 { 00196 EIGEN_PRODUCT_PUBLIC_INTERFACE(TriangularProduct) 00197 00198 TriangularProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {} 00199 00200 template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const 00201 { 00202 eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols()); 00203 00204 typedef TriangularProduct<(Mode & (UnitDiag|ZeroDiag)) | ((Mode & Lower) ? Upper : Lower),true,Transpose<const Rhs>,false,Transpose<const Lhs>,true> TriangularProductTranspose; 00205 Transpose<Dest> dstT(dst); 00206 internal::trmv_selector<(int(internal::traits<Rhs>::Flags)&RowMajorBit) ? ColMajor : RowMajor>::run( 00207 TriangularProductTranspose(m_rhs.transpose(),m_lhs.transpose()), dstT, alpha); 00208 } 00209 }; 00210 00211 namespace internal { 00212 00213 // TODO: find a way to factorize this piece of code with gemv_selector since the logic is exactly the same. 00214 00215 template<> struct trmv_selector<ColMajor> 00216 { 00217 template<int Mode, typename Lhs, typename Rhs, typename Dest> 00218 static void run(const TriangularProduct<Mode,true,Lhs,false,Rhs,true>& prod, Dest& dest, const typename TriangularProduct<Mode,true,Lhs,false,Rhs,true>::Scalar& alpha) 00219 { 00220 typedef TriangularProduct<Mode,true,Lhs,false,Rhs,true> ProductType; 00221 typedef typename ProductType::Index Index; 00222 typedef typename ProductType::LhsScalar LhsScalar; 00223 typedef typename ProductType::RhsScalar RhsScalar; 00224 typedef typename ProductType::Scalar ResScalar; 00225 typedef typename ProductType::RealScalar RealScalar; 00226 typedef typename ProductType::ActualLhsType ActualLhsType; 00227 typedef typename ProductType::ActualRhsType ActualRhsType; 00228 typedef typename ProductType::LhsBlasTraits LhsBlasTraits; 00229 typedef typename ProductType::RhsBlasTraits RhsBlasTraits; 00230 typedef Map<Matrix<ResScalar,Dynamic,1>, Aligned> MappedDest; 00231 00232 typename internal::add_const_on_value_type<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(prod.lhs()); 00233 typename internal::add_const_on_value_type<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(prod.rhs()); 00234 00235 ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs()) 00236 * RhsBlasTraits::extractScalarFactor(prod.rhs()); 00237 00238 enum { 00239 // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1 00240 // on, the other hand it is good for the cache to pack the vector anyways... 00241 EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1, 00242 ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex), 00243 MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal 00244 }; 00245 00246 gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest; 00247 00248 bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0)); 00249 bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible; 00250 00251 RhsScalar compatibleAlpha = get_factor<ResScalar,RhsScalar>::run(actualAlpha); 00252 00253 ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(), 00254 evalToDest ? dest.data() : static_dest.data()); 00255 00256 if(!evalToDest) 00257 { 00258 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00259 Index size = dest.size(); 00260 EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00261 #endif 00262 if(!alphaIsCompatible) 00263 { 00264 MappedDest(actualDestPtr, dest.size()).setZero(); 00265 compatibleAlpha = RhsScalar(1); 00266 } 00267 else 00268 MappedDest(actualDestPtr, dest.size()) = dest; 00269 } 00270 00271 internal::triangular_matrix_vector_product 00272 <Index,Mode, 00273 LhsScalar, LhsBlasTraits::NeedToConjugate, 00274 RhsScalar, RhsBlasTraits::NeedToConjugate, 00275 ColMajor> 00276 ::run(actualLhs.rows(),actualLhs.cols(), 00277 actualLhs.data(),actualLhs.outerStride(), 00278 actualRhs.data(),actualRhs.innerStride(), 00279 actualDestPtr,1,compatibleAlpha); 00280 00281 if (!evalToDest) 00282 { 00283 if(!alphaIsCompatible) 00284 dest += actualAlpha * MappedDest(actualDestPtr, dest.size()); 00285 else 00286 dest = MappedDest(actualDestPtr, dest.size()); 00287 } 00288 } 00289 }; 00290 00291 template<> struct trmv_selector<RowMajor> 00292 { 00293 template<int Mode, typename Lhs, typename Rhs, typename Dest> 00294 static void run(const TriangularProduct<Mode,true,Lhs,false,Rhs,true>& prod, Dest& dest, const typename TriangularProduct<Mode,true,Lhs,false,Rhs,true>::Scalar& alpha) 00295 { 00296 typedef TriangularProduct<Mode,true,Lhs,false,Rhs,true> ProductType; 00297 typedef typename ProductType::LhsScalar LhsScalar; 00298 typedef typename ProductType::RhsScalar RhsScalar; 00299 typedef typename ProductType::Scalar ResScalar; 00300 typedef typename ProductType::Index Index; 00301 typedef typename ProductType::ActualLhsType ActualLhsType; 00302 typedef typename ProductType::ActualRhsType ActualRhsType; 00303 typedef typename ProductType::_ActualRhsType _ActualRhsType; 00304 typedef typename ProductType::LhsBlasTraits LhsBlasTraits; 00305 typedef typename ProductType::RhsBlasTraits RhsBlasTraits; 00306 00307 typename add_const<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(prod.lhs()); 00308 typename add_const<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(prod.rhs()); 00309 00310 ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs()) 00311 * RhsBlasTraits::extractScalarFactor(prod.rhs()); 00312 00313 enum { 00314 DirectlyUseRhs = _ActualRhsType::InnerStrideAtCompileTime==1 00315 }; 00316 00317 gemv_static_vector_if<RhsScalar,_ActualRhsType::SizeAtCompileTime,_ActualRhsType::MaxSizeAtCompileTime,!DirectlyUseRhs> static_rhs; 00318 00319 ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,actualRhs.size(), 00320 DirectlyUseRhs ? const_cast<RhsScalar*>(actualRhs.data()) : static_rhs.data()); 00321 00322 if(!DirectlyUseRhs) 00323 { 00324 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00325 int size = actualRhs.size(); 00326 EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00327 #endif 00328 Map<typename _ActualRhsType::PlainObject>(actualRhsPtr, actualRhs.size()) = actualRhs; 00329 } 00330 00331 internal::triangular_matrix_vector_product 00332 <Index,Mode, 00333 LhsScalar, LhsBlasTraits::NeedToConjugate, 00334 RhsScalar, RhsBlasTraits::NeedToConjugate, 00335 RowMajor> 00336 ::run(actualLhs.rows(),actualLhs.cols(), 00337 actualLhs.data(),actualLhs.outerStride(), 00338 actualRhsPtr,1, 00339 dest.data(),dest.innerStride(), 00340 actualAlpha); 00341 } 00342 }; 00343 00344 } // end namespace internal 00345 00346 } // end namespace Eigen 00347 00348 #endif // EIGEN_TRIANGULARMATRIXVECTOR_H
Generated on Tue Jul 12 2022 17:47:01 by 1.7.2