Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
SelfadjointMatrixVector.h
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2008-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_SELFADJOINT_MATRIX_VECTOR_H 00011 #define EIGEN_SELFADJOINT_MATRIX_VECTOR_H 00012 00013 namespace Eigen { 00014 00015 namespace internal { 00016 00017 /* Optimized selfadjoint matrix * vector product: 00018 * This algorithm processes 2 columns at onces that allows to both reduce 00019 * the number of load/stores of the result by a factor 2 and to reduce 00020 * the instruction dependency. 00021 */ 00022 00023 template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version=Specialized> 00024 struct selfadjoint_matrix_vector_product; 00025 00026 template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version> 00027 struct selfadjoint_matrix_vector_product 00028 00029 { 00030 static EIGEN_DONT_INLINE void run( 00031 Index size, 00032 const Scalar* lhs, Index lhsStride, 00033 const Scalar* _rhs, Index rhsIncr, 00034 Scalar* res, 00035 Scalar alpha); 00036 }; 00037 00038 template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version> 00039 EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrder,UpLo,ConjugateLhs,ConjugateRhs,Version>::run( 00040 Index size, 00041 const Scalar* lhs, Index lhsStride, 00042 const Scalar* _rhs, Index rhsIncr, 00043 Scalar* res, 00044 Scalar alpha) 00045 { 00046 typedef typename packet_traits<Scalar>::type Packet; 00047 const Index PacketSize = sizeof(Packet)/sizeof(Scalar); 00048 00049 enum { 00050 IsRowMajor = StorageOrder==RowMajor ? 1 : 0, 00051 IsLower = UpLo == Lower ? 1 : 0, 00052 FirstTriangular = IsRowMajor == IsLower 00053 }; 00054 00055 conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, IsRowMajor), ConjugateRhs> cj0; 00056 conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> cj1; 00057 conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex, ConjugateRhs> cjd; 00058 00059 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, IsRowMajor), ConjugateRhs> pcj0; 00060 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> pcj1; 00061 00062 Scalar cjAlpha = ConjugateRhs ? numext::conj(alpha) : alpha; 00063 00064 // FIXME this copy is now handled outside product_selfadjoint_vector, so it could probably be removed. 00065 // if the rhs is not sequentially stored in memory we copy it to a temporary buffer, 00066 // this is because we need to extract packets 00067 ei_declare_aligned_stack_constructed_variable(Scalar,rhs,size,rhsIncr==1 ? const_cast<Scalar*>(_rhs) : 0); 00068 if (rhsIncr!=1) 00069 { 00070 const Scalar* it = _rhs; 00071 for (Index i=0; i<size; ++i, it+=rhsIncr) 00072 rhs[i] = *it; 00073 } 00074 00075 Index bound = (std::max)(Index(0),size-8) & 0xfffffffe; 00076 if (FirstTriangular) 00077 bound = size - bound; 00078 00079 for (Index j=FirstTriangular ? bound : 0; 00080 j<(FirstTriangular ? size : bound);j+=2) 00081 { 00082 const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride; 00083 const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride; 00084 00085 Scalar t0 = cjAlpha * rhs[j]; 00086 Packet ptmp0 = pset1<Packet>(t0); 00087 Scalar t1 = cjAlpha * rhs[j+1]; 00088 Packet ptmp1 = pset1<Packet>(t1); 00089 00090 Scalar t2(0); 00091 Packet ptmp2 = pset1<Packet>(t2); 00092 Scalar t3(0); 00093 Packet ptmp3 = pset1<Packet>(t3); 00094 00095 size_t starti = FirstTriangular ? 0 : j+2; 00096 size_t endi = FirstTriangular ? j : size; 00097 size_t alignedStart = (starti) + internal::first_aligned(&res[starti], endi-starti); 00098 size_t alignedEnd = alignedStart + ((endi-alignedStart)/(PacketSize))*(PacketSize); 00099 00100 // TODO make sure this product is a real * complex and that the rhs is properly conjugated if needed 00101 res[j] += cjd.pmul(numext::real(A0[j]), t0); 00102 res[j+1] += cjd.pmul(numext::real(A1[j+1]), t1); 00103 if(FirstTriangular) 00104 { 00105 res[j] += cj0.pmul(A1[j], t1); 00106 t3 += cj1.pmul(A1[j], rhs[j]); 00107 } 00108 else 00109 { 00110 res[j+1] += cj0.pmul(A0[j+1],t0); 00111 t2 += cj1.pmul(A0[j+1], rhs[j+1]); 00112 } 00113 00114 for (size_t i=starti; i<alignedStart; ++i) 00115 { 00116 res[i] += t0 * A0[i] + t1 * A1[i]; 00117 t2 += numext::conj(A0[i]) * rhs[i]; 00118 t3 += numext::conj(A1[i]) * rhs[i]; 00119 } 00120 // Yes this an optimization for gcc 4.3 and 4.4 (=> huge speed up) 00121 // gcc 4.2 does this optimization automatically. 00122 const Scalar* EIGEN_RESTRICT a0It = A0 + alignedStart; 00123 const Scalar* EIGEN_RESTRICT a1It = A1 + alignedStart; 00124 const Scalar* EIGEN_RESTRICT rhsIt = rhs + alignedStart; 00125 Scalar* EIGEN_RESTRICT resIt = res + alignedStart; 00126 for (size_t i=alignedStart; i<alignedEnd; i+=PacketSize) 00127 { 00128 Packet A0i = ploadu<Packet>(a0It); a0It += PacketSize; 00129 Packet A1i = ploadu<Packet>(a1It); a1It += PacketSize; 00130 Packet Bi = ploadu<Packet>(rhsIt); rhsIt += PacketSize; // FIXME should be aligned in most cases 00131 Packet Xi = pload <Packet>(resIt); 00132 00133 Xi = pcj0.pmadd(A0i,ptmp0, pcj0.pmadd(A1i,ptmp1,Xi)); 00134 ptmp2 = pcj1.pmadd(A0i, Bi, ptmp2); 00135 ptmp3 = pcj1.pmadd(A1i, Bi, ptmp3); 00136 pstore(resIt,Xi); resIt += PacketSize; 00137 } 00138 for (size_t i=alignedEnd; i<endi; i++) 00139 { 00140 res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i],t1); 00141 t2 += cj1.pmul(A0[i], rhs[i]); 00142 t3 += cj1.pmul(A1[i], rhs[i]); 00143 } 00144 00145 res[j] += alpha * (t2 + predux(ptmp2)); 00146 res[j+1] += alpha * (t3 + predux(ptmp3)); 00147 } 00148 for (Index j=FirstTriangular ? 0 : bound;j<(FirstTriangular ? bound : size);j++) 00149 { 00150 const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride; 00151 00152 Scalar t1 = cjAlpha * rhs[j]; 00153 Scalar t2(0); 00154 // TODO make sure this product is a real * complex and that the rhs is properly conjugated if needed 00155 res[j] += cjd.pmul(numext::real(A0[j]), t1); 00156 for (Index i=FirstTriangular ? 0 : j+1; i<(FirstTriangular ? j : size); i++) 00157 { 00158 res[i] += cj0.pmul(A0[i], t1); 00159 t2 += cj1.pmul(A0[i], rhs[i]); 00160 } 00161 res[j] += alpha * t2; 00162 } 00163 } 00164 00165 } // end namespace internal 00166 00167 /*************************************************************************** 00168 * Wrapper to product_selfadjoint_vector 00169 ***************************************************************************/ 00170 00171 namespace internal { 00172 template<typename Lhs, int LhsMode, typename Rhs> 00173 struct traits<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true> > 00174 : traits<ProductBase<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>, Lhs, Rhs> > 00175 {}; 00176 } 00177 00178 template<typename Lhs, int LhsMode, typename Rhs> 00179 struct SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true> 00180 : public ProductBase<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>, Lhs, Rhs > 00181 { 00182 EIGEN_PRODUCT_PUBLIC_INTERFACE(SelfadjointProductMatrix) 00183 00184 enum { 00185 LhsUpLo = LhsMode&(Upper|Lower) 00186 }; 00187 00188 SelfadjointProductMatrix(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {} 00189 00190 template<typename Dest> void scaleAndAddTo(Dest& dest, const Scalar& alpha) const 00191 { 00192 typedef typename Dest::Scalar ResScalar; 00193 typedef typename Base::RhsScalar RhsScalar; 00194 typedef Map<Matrix<ResScalar,Dynamic,1>, Aligned> MappedDest; 00195 00196 eigen_assert(dest.rows()==m_lhs.rows() && dest.cols()==m_rhs.cols()); 00197 00198 typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(m_lhs); 00199 typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(m_rhs); 00200 00201 Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(m_lhs) 00202 * RhsBlasTraits::extractScalarFactor(m_rhs); 00203 00204 enum { 00205 EvalToDest = (Dest::InnerStrideAtCompileTime==1), 00206 UseRhs = (_ActualRhsType::InnerStrideAtCompileTime==1) 00207 }; 00208 00209 internal::gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,!EvalToDest> static_dest; 00210 internal::gemv_static_vector_if<RhsScalar,_ActualRhsType::SizeAtCompileTime,_ActualRhsType::MaxSizeAtCompileTime,!UseRhs> static_rhs; 00211 00212 ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(), 00213 EvalToDest ? dest.data() : static_dest.data()); 00214 00215 ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,rhs.size(), 00216 UseRhs ? const_cast<RhsScalar*>(rhs.data()) : static_rhs.data()); 00217 00218 if(!EvalToDest) 00219 { 00220 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00221 int size = dest.size(); 00222 EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00223 #endif 00224 MappedDest(actualDestPtr, dest.size()) = dest; 00225 } 00226 00227 if(!UseRhs) 00228 { 00229 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00230 int size = rhs.size(); 00231 EIGEN_DENSE_STORAGE_CTOR_PLUGIN 00232 #endif 00233 Map<typename _ActualRhsType::PlainObject>(actualRhsPtr, rhs.size()) = rhs; 00234 } 00235 00236 00237 internal::selfadjoint_matrix_vector_product<Scalar, Index, (internal::traits<_ActualLhsType>::Flags&RowMajorBit) ? RowMajor : ColMajor, int(LhsUpLo), bool(LhsBlasTraits::NeedToConjugate), bool(RhsBlasTraits::NeedToConjugate)>::run 00238 ( 00239 lhs.rows(), // size 00240 &lhs.coeffRef(0,0), lhs.outerStride(), // lhs info 00241 actualRhsPtr, 1, // rhs info 00242 actualDestPtr, // result info 00243 actualAlpha // scale factor 00244 ); 00245 00246 if(!EvalToDest) 00247 dest = MappedDest(actualDestPtr, dest.size()); 00248 } 00249 }; 00250 00251 namespace internal { 00252 template<typename Lhs, typename Rhs, int RhsMode> 00253 struct traits<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false> > 00254 : traits<ProductBase<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>, Lhs, Rhs> > 00255 {}; 00256 } 00257 00258 template<typename Lhs, typename Rhs, int RhsMode> 00259 struct SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false> 00260 : public ProductBase<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>, Lhs, Rhs > 00261 { 00262 EIGEN_PRODUCT_PUBLIC_INTERFACE(SelfadjointProductMatrix) 00263 00264 enum { 00265 RhsUpLo = RhsMode&(Upper|Lower) 00266 }; 00267 00268 SelfadjointProductMatrix(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {} 00269 00270 template<typename Dest> void scaleAndAddTo(Dest& dest, const Scalar& alpha) const 00271 { 00272 // let's simply transpose the product 00273 Transpose<Dest> destT(dest); 00274 SelfadjointProductMatrix<Transpose<const Rhs>, int(RhsUpLo)==Upper ? Lower : Upper, false, 00275 Transpose<const Lhs>, 0, true>(m_rhs.transpose(), m_lhs.transpose()).scaleAndAddTo(destT, alpha); 00276 } 00277 }; 00278 00279 } // end namespace Eigen 00280 00281 #endif // EIGEN_SELFADJOINT_MATRIX_VECTOR_H
Generated on Tue Jul 12 2022 17:47:00 by 1.7.2