• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_STABLENORM_H
11 #define EIGEN_STABLENORM_H
12 
13 namespace Eigen {
14 
15 namespace internal {
16 
17 template<typename ExpressionType, typename Scalar>
stable_norm_kernel(const ExpressionType & bl,Scalar & ssq,Scalar & scale,Scalar & invScale)18 inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale)
19 {
20   Scalar maxCoeff = bl.cwiseAbs().maxCoeff();
21 
22   if(maxCoeff>scale)
23   {
24     ssq = ssq * numext::abs2(scale/maxCoeff);
25     Scalar tmp = Scalar(1)/maxCoeff;
26     if(tmp > NumTraits<Scalar>::highest())
27     {
28       invScale = NumTraits<Scalar>::highest();
29       scale = Scalar(1)/invScale;
30     }
31     else if(maxCoeff>NumTraits<Scalar>::highest()) // we got a INF
32     {
33       invScale = Scalar(1);
34       scale = maxCoeff;
35     }
36     else
37     {
38       scale = maxCoeff;
39       invScale = tmp;
40     }
41   }
42   else if(maxCoeff!=maxCoeff) // we got a NaN
43   {
44     scale = maxCoeff;
45   }
46 
47   // TODO if the maxCoeff is much much smaller than the current scale,
48   // then we can neglect this sub vector
49   if(scale>Scalar(0)) // if scale==0, then bl is 0
50     ssq += (bl*invScale).squaredNorm();
51 }
52 
53 template<typename VectorType, typename RealScalar>
stable_norm_impl_inner_step(const VectorType & vec,RealScalar & ssq,RealScalar & scale,RealScalar & invScale)54 void stable_norm_impl_inner_step(const VectorType &vec, RealScalar& ssq, RealScalar& scale, RealScalar& invScale)
55 {
56   typedef typename VectorType::Scalar Scalar;
57   const Index blockSize = 4096;
58 
59   typedef typename internal::nested_eval<VectorType,2>::type VectorTypeCopy;
60   typedef typename internal::remove_all<VectorTypeCopy>::type VectorTypeCopyClean;
61   const VectorTypeCopy copy(vec);
62 
63   enum {
64     CanAlign = (   (int(VectorTypeCopyClean::Flags)&DirectAccessBit)
65                 || (int(internal::evaluator<VectorTypeCopyClean>::Alignment)>0) // FIXME Alignment)>0 might not be enough
66                ) && (blockSize*sizeof(Scalar)*2<EIGEN_STACK_ALLOCATION_LIMIT)
67                  && (EIGEN_MAX_STATIC_ALIGN_BYTES>0) // if we cannot allocate on the stack, then let's not bother about this optimization
68   };
69   typedef typename internal::conditional<CanAlign, Ref<const Matrix<Scalar,Dynamic,1,0,blockSize,1>, internal::evaluator<VectorTypeCopyClean>::Alignment>,
70                                                    typename VectorTypeCopyClean::ConstSegmentReturnType>::type SegmentWrapper;
71   Index n = vec.size();
72 
73   Index bi = internal::first_default_aligned(copy);
74   if (bi>0)
75     internal::stable_norm_kernel(copy.head(bi), ssq, scale, invScale);
76   for (; bi<n; bi+=blockSize)
77     internal::stable_norm_kernel(SegmentWrapper(copy.segment(bi,numext::mini(blockSize, n - bi))), ssq, scale, invScale);
78 }
79 
80 template<typename VectorType>
81 typename VectorType::RealScalar
82 stable_norm_impl(const VectorType &vec, typename enable_if<VectorType::IsVectorAtCompileTime>::type* = 0 )
83 {
84   using std::sqrt;
85   using std::abs;
86 
87   Index n = vec.size();
88 
89   if(n==1)
90     return abs(vec.coeff(0));
91 
92   typedef typename VectorType::RealScalar RealScalar;
93   RealScalar scale(0);
94   RealScalar invScale(1);
95   RealScalar ssq(0); // sum of squares
96 
97   stable_norm_impl_inner_step(vec, ssq, scale, invScale);
98 
99   return scale * sqrt(ssq);
100 }
101 
102 template<typename MatrixType>
103 typename MatrixType::RealScalar
104 stable_norm_impl(const MatrixType &mat, typename enable_if<!MatrixType::IsVectorAtCompileTime>::type* = 0 )
105 {
106   using std::sqrt;
107 
108   typedef typename MatrixType::RealScalar RealScalar;
109   RealScalar scale(0);
110   RealScalar invScale(1);
111   RealScalar ssq(0); // sum of squares
112 
113   for(Index j=0; j<mat.outerSize(); ++j)
114     stable_norm_impl_inner_step(mat.innerVector(j), ssq, scale, invScale);
115   return scale * sqrt(ssq);
116 }
117 
118 template<typename Derived>
119 inline typename NumTraits<typename traits<Derived>::Scalar>::Real
blueNorm_impl(const EigenBase<Derived> & _vec)120 blueNorm_impl(const EigenBase<Derived>& _vec)
121 {
122   typedef typename Derived::RealScalar RealScalar;
123   using std::pow;
124   using std::sqrt;
125   using std::abs;
126 
127   // This program calculates the machine-dependent constants
128   // bl, b2, slm, s2m, relerr overfl
129   // from the "basic" machine-dependent numbers
130   // nbig, ibeta, it, iemin, iemax, rbig.
131   // The following define the basic machine-dependent constants.
132   // For portability, the PORT subprograms "ilmaeh" and "rlmach"
133   // are used. For any specific computer, each of the assignment
134   // statements can be replaced
135   static const int ibeta = std::numeric_limits<RealScalar>::radix;  // base for floating-point numbers
136   static const int it    = NumTraits<RealScalar>::digits();  // number of base-beta digits in mantissa
137   static const int iemin = NumTraits<RealScalar>::min_exponent();  // minimum exponent
138   static const int iemax = NumTraits<RealScalar>::max_exponent();  // maximum exponent
139   static const RealScalar rbig   = NumTraits<RealScalar>::highest();  // largest floating-point number
140   static const RealScalar b1     = RealScalar(pow(RealScalar(ibeta),RealScalar(-((1-iemin)/2))));  // lower boundary of midrange
141   static const RealScalar b2     = RealScalar(pow(RealScalar(ibeta),RealScalar((iemax + 1 - it)/2)));  // upper boundary of midrange
142   static const RealScalar s1m    = RealScalar(pow(RealScalar(ibeta),RealScalar((2-iemin)/2)));  // scaling factor for lower range
143   static const RealScalar s2m    = RealScalar(pow(RealScalar(ibeta),RealScalar(- ((iemax+it)/2))));  // scaling factor for upper range
144   static const RealScalar eps    = RealScalar(pow(double(ibeta), 1-it));
145   static const RealScalar relerr = sqrt(eps);  // tolerance for neglecting asml
146 
147   const Derived& vec(_vec.derived());
148   Index n = vec.size();
149   RealScalar ab2 = b2 / RealScalar(n);
150   RealScalar asml = RealScalar(0);
151   RealScalar amed = RealScalar(0);
152   RealScalar abig = RealScalar(0);
153 
154   for(Index j=0; j<vec.outerSize(); ++j)
155   {
156     for(typename Derived::InnerIterator iter(vec, j); iter; ++iter)
157     {
158       RealScalar ax = abs(iter.value());
159       if(ax > ab2)     abig += numext::abs2(ax*s2m);
160       else if(ax < b1) asml += numext::abs2(ax*s1m);
161       else             amed += numext::abs2(ax);
162     }
163   }
164   if(amed!=amed)
165     return amed;  // we got a NaN
166   if(abig > RealScalar(0))
167   {
168     abig = sqrt(abig);
169     if(abig > rbig) // overflow, or *this contains INF values
170       return abig;  // return INF
171     if(amed > RealScalar(0))
172     {
173       abig = abig/s2m;
174       amed = sqrt(amed);
175     }
176     else
177       return abig/s2m;
178   }
179   else if(asml > RealScalar(0))
180   {
181     if (amed > RealScalar(0))
182     {
183       abig = sqrt(amed);
184       amed = sqrt(asml) / s1m;
185     }
186     else
187       return sqrt(asml)/s1m;
188   }
189   else
190     return sqrt(amed);
191   asml = numext::mini(abig, amed);
192   abig = numext::maxi(abig, amed);
193   if(asml <= abig*relerr)
194     return abig;
195   else
196     return abig * sqrt(RealScalar(1) + numext::abs2(asml/abig));
197 }
198 
199 } // end namespace internal
200 
201 /** \returns the \em l2 norm of \c *this avoiding underflow and overflow.
202   * This version use a blockwise two passes algorithm:
203   *  1 - find the absolute largest coefficient \c s
204   *  2 - compute \f$ s \Vert \frac{*this}{s} \Vert \f$ in a standard way
205   *
206   * For architecture/scalar types supporting vectorization, this version
207   * is faster than blueNorm(). Otherwise the blueNorm() is much faster.
208   *
209   * \sa norm(), blueNorm(), hypotNorm()
210   */
211 template<typename Derived>
212 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
stableNorm()213 MatrixBase<Derived>::stableNorm() const
214 {
215   return internal::stable_norm_impl(derived());
216 }
217 
218 /** \returns the \em l2 norm of \c *this using the Blue's algorithm.
219   * A Portable Fortran Program to Find the Euclidean Norm of a Vector,
220   * ACM TOMS, Vol 4, Issue 1, 1978.
221   *
222   * For architecture/scalar types without vectorization, this version
223   * is much faster than stableNorm(). Otherwise the stableNorm() is faster.
224   *
225   * \sa norm(), stableNorm(), hypotNorm()
226   */
227 template<typename Derived>
228 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
blueNorm()229 MatrixBase<Derived>::blueNorm() const
230 {
231   return internal::blueNorm_impl(*this);
232 }
233 
234 /** \returns the \em l2 norm of \c *this avoiding undeflow and overflow.
235   * This version use a concatenation of hypot() calls, and it is very slow.
236   *
237   * \sa norm(), stableNorm()
238   */
239 template<typename Derived>
240 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
hypotNorm()241 MatrixBase<Derived>::hypotNorm() const
242 {
243   if(size()==1)
244     return numext::abs(coeff(0,0));
245   else
246     return this->cwiseAbs().redux(internal::scalar_hypot_op<RealScalar>());
247 }
248 
249 } // end namespace Eigen
250 
251 #endif // EIGEN_STABLENORM_H
252