modules/core/src/matrix_decomp.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44
  45 namespace cv { namespace hal {
  46
  47 /****************************************************************************************\
  48 *                     LU & Cholesky implementation for small matrices                    *
  49 \****************************************************************************************/
  50
  51 template<typename _Tp> static inline int
  52 LUImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n, _Tp eps)
  53 {
  54     int i, j, k, p = 1;
  55     astep /= sizeof(A[0]);
  56     bstep /= sizeof(b[0]);
  57
  58     for( i = 0; i < m; i++ )
  59     {
  60         k = i;
  61
  62         for( j = i+1; j < m; j++ )
  63             if( std::abs(A[j*astep + i]) > std::abs(A[k*astep + i]) )
  64                 k = j;
  65
  66         if( std::abs(A[k*astep + i]) < eps )
  67             return 0;
  68
  69         if( k != i )
  70         {
  71             for( j = i; j < m; j++ )
  72                 std::swap(A[i*astep + j], A[k*astep + j]);
  73             if( b )
  74                 for( j = 0; j < n; j++ )
  75                     std::swap(b[i*bstep + j], b[k*bstep + j]);
  76             p = -p;
  77         }
  78
  79         _Tp d = -1/A[i*astep + i];
  80
  81         for( j = i+1; j < m; j++ )
  82         {
  83             _Tp alpha = A[j*astep + i]*d;
  84
  85             for( k = i+1; k < m; k++ )
  86                 A[j*astep + k] += alpha*A[i*astep + k];
  87
  88             if( b )
  89                 for( k = 0; k < n; k++ )
  90                     b[j*bstep + k] += alpha*b[i*bstep + k];
  91         }
  92
  93         A[i*astep + i] = -d;
  94     }
  95
  96     if( b )
  97     {
  98         for( i = m-1; i >= 0; i-- )
  99             for( j = 0; j < n; j++ )
 100             {
 101                 _Tp s = b[i*bstep + j];
 102                 for( k = i+1; k < m; k++ )
 103                     s -= A[i*astep + k]*b[k*bstep + j];
 104                 b[i*bstep + j] = s*A[i*astep + i];
 105             }
 106     }
 107
 108     return p;
 109 }
 110
 111
 112 int LU32f(float* A, size_t astep, int m, float* b, size_t bstep, int n)
 113 {
 114     return LUImpl(A, astep, m, b, bstep, n, FLT_EPSILON*10);
 115 }
 116
 117
 118 int LU64f(double* A, size_t astep, int m, double* b, size_t bstep, int n)
 119 {
 120     return LUImpl(A, astep, m, b, bstep, n, DBL_EPSILON*100);
 121 }
 122
 123 template<typename _Tp> static inline bool
 124 CholImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n)
 125 {
 126     _Tp* L = A;
 127     int i, j, k;
 128     double s;
 129     astep /= sizeof(A[0]);
 130     bstep /= sizeof(b[0]);
 131
 132     for( i = 0; i < m; i++ )
 133     {
 134         for( j = 0; j < i; j++ )
 135         {
 136             s = A[i*astep + j];
 137             for( k = 0; k < j; k++ )
 138                 s -= L[i*astep + k]*L[j*astep + k];
 139             L[i*astep + j] = (_Tp)(s*L[j*astep + j]);
 140         }
 141         s = A[i*astep + i];
 142         for( k = 0; k < j; k++ )
 143         {
 144             double t = L[i*astep + k];
 145             s -= t*t;
 146         }
 147         if( s < std::numeric_limits<_Tp>::epsilon() )
 148             return false;
 149         L[i*astep + i] = (_Tp)(1./std::sqrt(s));
 150     }
 151
 152     if( !b )
 153         return true;
 154
 155     // LLt x = b
 156     // 1: L y = b
 157     // 2. Lt x = y
 158
 159     /*
 160      [ L00             ]  y0   b0
 161      [ L10 L11         ]  y1 = b1
 162      [ L20 L21 L22     ]  y2   b2
 163      [ L30 L31 L32 L33 ]  y3   b3
 164
 165      [ L00 L10 L20 L30 ]  x0   y0
 166      [     L11 L21 L31 ]  x1 = y1
 167      [         L22 L32 ]  x2   y2
 168      [             L33 ]  x3   y3
 169      */
 170
 171     for( i = 0; i < m; i++ )
 172     {
 173         for( j = 0; j < n; j++ )
 174         {
 175             s = b[i*bstep + j];
 176             for( k = 0; k < i; k++ )
 177                 s -= L[i*astep + k]*b[k*bstep + j];
 178             b[i*bstep + j] = (_Tp)(s*L[i*astep + i]);
 179         }
 180     }
 181
 182     for( i = m-1; i >= 0; i-- )
 183     {
 184         for( j = 0; j < n; j++ )
 185         {
 186             s = b[i*bstep + j];
 187             for( k = m-1; k > i; k-- )
 188                 s -= L[k*astep + i]*b[k*bstep + j];
 189             b[i*bstep + j] = (_Tp)(s*L[i*astep + i]);
 190         }
 191     }
 192
 193     return true;
 194 }
 195
 196
 197 bool Cholesky32f(float* A, size_t astep, int m, float* b, size_t bstep, int n)
 198 {
 199     return CholImpl(A, astep, m, b, bstep, n);
 200 }
 201
 202 bool Cholesky64f(double* A, size_t astep, int m, double* b, size_t bstep, int n)
 203 {
 204     return CholImpl(A, astep, m, b, bstep, n);
 205 }
 206
 207 //=============================================================================
 208 // for compatibility with 3.0
 209
 210 int LU(float* A, size_t astep, int m, float* b, size_t bstep, int n)
 211 {
 212     return LUImpl(A, astep, m, b, bstep, n, FLT_EPSILON*10);
 213 }
 214
 215 int LU(double* A, size_t astep, int m, double* b, size_t bstep, int n)
 216 {
 217     return LUImpl(A, astep, m, b, bstep, n, DBL_EPSILON*100);
 218 }
 219
 220 bool Cholesky(float* A, size_t astep, int m, float* b, size_t bstep, int n)
 221 {
 222     return CholImpl(A, astep, m, b, bstep, n);
 223 }
 224
 225 bool Cholesky(double* A, size_t astep, int m, double* b, size_t bstep, int n)
 226 {
 227     return CholImpl(A, astep, m, b, bstep, n);
 228 }
 229
 230
 231 }}