Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
StableNorm.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_STABLENORM_H 00011 #define EIGEN_STABLENORM_H 00012 00013 namespace Eigen { 00014 00015 namespace internal { 00016 00017 template<typename ExpressionType, typename Scalar> 00018 inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale) 00019 { 00020 using std::max; 00021 Scalar maxCoeff = bl.cwiseAbs().maxCoeff(); 00022 00023 if (maxCoeff>scale) 00024 { 00025 ssq = ssq * numext::abs2(scale/maxCoeff); 00026 Scalar tmp = Scalar(1)/maxCoeff; 00027 if(tmp > NumTraits<Scalar>::highest()) 00028 { 00029 invScale = NumTraits<Scalar>::highest(); 00030 scale = Scalar(1)/invScale; 00031 } 00032 else 00033 { 00034 scale = maxCoeff; 00035 invScale = tmp; 00036 } 00037 } 00038 00039 // TODO if the maxCoeff is much much smaller than the current scale, 00040 // then we can neglect this sub vector 00041 if(scale>Scalar(0)) // if scale==0, then bl is 0 00042 ssq += (bl*invScale).squaredNorm(); 00043 } 00044 00045 template<typename Derived> 00046 inline typename NumTraits<typename traits<Derived>::Scalar>::Real 00047 blueNorm_impl(const EigenBase<Derived>& _vec) 00048 { 00049 typedef typename Derived::RealScalar RealScalar; 00050 typedef typename Derived::Index Index; 00051 using std::pow; 00052 using std::min; 00053 using std::max; 00054 using std::sqrt; 00055 using std::abs; 00056 const Derived& vec(_vec.derived()); 00057 static bool initialized = false; 00058 static RealScalar b1, b2, s1m, s2m, overfl, rbig, relerr; 00059 if(!initialized) 00060 { 00061 int ibeta, it, iemin, iemax, iexp; 00062 RealScalar eps; 00063 // This program calculates the machine-dependent constants 00064 // bl, b2, slm, s2m, relerr overfl 00065 // from the "basic" machine-dependent numbers 00066 // nbig, ibeta, it, iemin, iemax, rbig. 00067 // The following define the basic machine-dependent constants. 00068 // For portability, the PORT subprograms "ilmaeh" and "rlmach" 00069 // are used. For any specific computer, each of the assignment 00070 // statements can be replaced 00071 ibeta = std::numeric_limits<RealScalar>::radix; // base for floating-point numbers 00072 it = std::numeric_limits<RealScalar>::digits; // number of base-beta digits in mantissa 00073 iemin = std::numeric_limits<RealScalar>::min_exponent; // minimum exponent 00074 iemax = std::numeric_limits<RealScalar>::max_exponent; // maximum exponent 00075 rbig = (std::numeric_limits<RealScalar>::max)(); // largest floating-point number 00076 00077 iexp = -((1-iemin)/2); 00078 b1 = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp))); // lower boundary of midrange 00079 iexp = (iemax + 1 - it)/2; 00080 b2 = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp))); // upper boundary of midrange 00081 00082 iexp = (2-iemin)/2; 00083 s1m = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp))); // scaling factor for lower range 00084 iexp = - ((iemax+it)/2); 00085 s2m = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp))); // scaling factor for upper range 00086 00087 overfl = rbig*s2m; // overflow boundary for abig 00088 eps = RealScalar(pow(double(ibeta), 1-it)); 00089 relerr = sqrt(eps); // tolerance for neglecting asml 00090 initialized = true; 00091 } 00092 Index n = vec.size(); 00093 RealScalar ab2 = b2 / RealScalar(n); 00094 RealScalar asml = RealScalar(0); 00095 RealScalar amed = RealScalar(0); 00096 RealScalar abig = RealScalar(0); 00097 for(typename Derived::InnerIterator it(vec, 0); it; ++it) 00098 { 00099 RealScalar ax = abs(it.value()); 00100 if(ax > ab2) abig += numext::abs2(ax*s2m); 00101 else if(ax < b1) asml += numext::abs2(ax*s1m); 00102 else amed += numext::abs2(ax); 00103 } 00104 if(abig > RealScalar(0)) 00105 { 00106 abig = sqrt(abig); 00107 if(abig > overfl) 00108 { 00109 return rbig; 00110 } 00111 if(amed > RealScalar(0)) 00112 { 00113 abig = abig/s2m; 00114 amed = sqrt(amed); 00115 } 00116 else 00117 return abig/s2m; 00118 } 00119 else if(asml > RealScalar(0)) 00120 { 00121 if (amed > RealScalar(0)) 00122 { 00123 abig = sqrt(amed); 00124 amed = sqrt(asml) / s1m; 00125 } 00126 else 00127 return sqrt(asml)/s1m; 00128 } 00129 else 00130 return sqrt(amed); 00131 asml = (min)(abig, amed); 00132 abig = (max)(abig, amed); 00133 if(asml <= abig*relerr) 00134 return abig; 00135 else 00136 return abig * sqrt(RealScalar(1) + numext::abs2(asml/abig)); 00137 } 00138 00139 } // end namespace internal 00140 00141 /** \returns the \em l2 norm of \c *this avoiding underflow and overflow. 00142 * This version use a blockwise two passes algorithm: 00143 * 1 - find the absolute largest coefficient \c s 00144 * 2 - compute \f$ s \Vert \frac{*this}{s} \Vert \f$ in a standard way 00145 * 00146 * For architecture/scalar types supporting vectorization, this version 00147 * is faster than blueNorm(). Otherwise the blueNorm() is much faster. 00148 * 00149 * \sa norm(), blueNorm(), hypotNorm() 00150 */ 00151 template<typename Derived> 00152 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real 00153 MatrixBase<Derived>::stableNorm () const 00154 { 00155 using std::min; 00156 using std::sqrt; 00157 const Index blockSize = 4096; 00158 RealScalar scale(0); 00159 RealScalar invScale(1); 00160 RealScalar ssq(0); // sum of square 00161 enum { 00162 Alignment = (int(Flags)&DirectAccessBit) || (int(Flags)&AlignedBit) ? 1 : 0 00163 }; 00164 Index n = size(); 00165 Index bi = internal::first_aligned(derived()); 00166 if (bi>0) 00167 internal::stable_norm_kernel(this->head(bi), ssq, scale, invScale); 00168 for (; bi<n; bi+=blockSize) 00169 internal::stable_norm_kernel(this->segment(bi,(min)(blockSize, n - bi)).template forceAlignedAccessIf<Alignment>(), ssq, scale, invScale); 00170 return scale * sqrt(ssq); 00171 } 00172 00173 /** \returns the \em l2 norm of \c *this using the Blue's algorithm. 00174 * A Portable Fortran Program to Find the Euclidean Norm of a Vector, 00175 * ACM TOMS, Vol 4, Issue 1, 1978. 00176 * 00177 * For architecture/scalar types without vectorization, this version 00178 * is much faster than stableNorm(). Otherwise the stableNorm() is faster. 00179 * 00180 * \sa norm(), stableNorm(), hypotNorm() 00181 */ 00182 template<typename Derived> 00183 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real 00184 MatrixBase<Derived>::blueNorm () const 00185 { 00186 return internal::blueNorm_impl(*this); 00187 } 00188 00189 /** \returns the \em l2 norm of \c *this avoiding undeflow and overflow. 00190 * This version use a concatenation of hypot() calls, and it is very slow. 00191 * 00192 * \sa norm(), stableNorm() 00193 */ 00194 template<typename Derived> 00195 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real 00196 MatrixBase<Derived>::hypotNorm () const 00197 { 00198 return this->cwiseAbs().redux(internal::scalar_hypot_op<RealScalar>()); 00199 } 00200 00201 } // end namespace Eigen 00202 00203 #endif // EIGEN_STABLENORM_H
Generated on Tue Jul 12 2022 17:47:00 by 1.7.2