matrix_decomp.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                           License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
00014 // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
00015 // Third party copyrights are property of their respective owners.
00016 //
00017 // Redistribution and use in source and binary forms, with or without modification,
00018 // are permitted provided that the following conditions are met:
00019 //
00020 //   * Redistribution's of source code must retain the above copyright notice,
00021 //     this list of conditions and the following disclaimer.
00022 //
00023 //   * Redistribution's in binary form must reproduce the above copyright notice,
00024 //     this list of conditions and the following disclaimer in the documentation
00025 //     and/or other materials provided with the distribution.
00026 //
00027 //   * The name of the copyright holders may not be used to endorse or promote products
00028 //     derived from this software without specific prior written permission.
00029 //
00030 // This software is provided by the copyright holders and contributors "as is" and
00031 // any express or implied warranties, including, but not limited to, the implied
00032 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00033 // In no event shall the Intel Corporation or contributors be liable for any direct,
00034 // indirect, incidental, special, exemplary, or consequential damages
00035 // (including, but not limited to, procurement of substitute goods or services;
00036 // loss of use, data, or profits; or business interruption) however caused
00037 // and on any theory of liability, whether in contract, strict liability,
00038 // or tort (including negligence or otherwise) arising in any way out of
00039 // the use of this software, even if advised of the possibility of such damage.
00040 //
00041 //M*/
00042 
00043 #include "precomp.hpp"
00044 
00045 namespace cv { namespace hal {
00046 
00047 /****************************************************************************************\
00048 *                     LU & Cholesky implementation for small matrices                    *
00049 \****************************************************************************************/
00050 
00051 template<typename _Tp> static inline int
00052 LUImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n, _Tp eps)
00053 {
00054     int i, j, k, p = 1;
00055     astep /= sizeof(A[0]);
00056     bstep /= sizeof(b[0]);
00057 
00058     for( i = 0; i < m; i++ )
00059     {
00060         k = i;
00061 
00062         for( j = i+1; j < m; j++ )
00063             if( std::abs(A[j*astep + i]) > std::abs(A[k*astep + i]) )
00064                 k = j;
00065 
00066         if( std::abs(A[k*astep + i]) < eps )
00067             return 0;
00068 
00069         if( k != i )
00070         {
00071             for( j = i; j < m; j++ )
00072                 std::swap(A[i*astep + j], A[k*astep + j]);
00073             if( b )
00074                 for( j = 0; j < n; j++ )
00075                     std::swap(b[i*bstep + j], b[k*bstep + j]);
00076             p = -p;
00077         }
00078 
00079         _Tp d = -1/A[i*astep + i];
00080 
00081         for( j = i+1; j < m; j++ )
00082         {
00083             _Tp alpha = A[j*astep + i]*d;
00084 
00085             for( k = i+1; k < m; k++ )
00086                 A[j*astep + k] += alpha*A[i*astep + k];
00087 
00088             if( b )
00089                 for( k = 0; k < n; k++ )
00090                     b[j*bstep + k] += alpha*b[i*bstep + k];
00091         }
00092 
00093         A[i*astep + i] = -d;
00094     }
00095 
00096     if( b )
00097     {
00098         for( i = m-1; i >= 0; i-- )
00099             for( j = 0; j < n; j++ )
00100             {
00101                 _Tp s = b[i*bstep + j];
00102                 for( k = i+1; k < m; k++ )
00103                     s -= A[i*astep + k]*b[k*bstep + j];
00104                 b[i*bstep + j] = s*A[i*astep + i];
00105             }
00106     }
00107 
00108     return p;
00109 }
00110 
00111 
00112 int LU32f(float* A, size_t astep, int m, float* b, size_t bstep, int n)
00113 {
00114     return LUImpl(A, astep, m, b, bstep, n, FLT_EPSILON*10);
00115 }
00116 
00117 
00118 int LU64f(double* A, size_t astep, int m, double* b, size_t bstep, int n)
00119 {
00120     return LUImpl(A, astep, m, b, bstep, n, DBL_EPSILON*100);
00121 }
00122 
00123 template<typename _Tp> static inline bool
00124 CholImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n)
00125 {
00126     _Tp* L = A;
00127     int i, j, k;
00128     double s;
00129     astep /= sizeof(A[0]);
00130     bstep /= sizeof(b[0]);
00131 
00132     for( i = 0; i < m; i++ )
00133     {
00134         for( j = 0; j < i; j++ )
00135         {
00136             s = A[i*astep + j];
00137             for( k = 0; k < j; k++ )
00138                 s -= L[i*astep + k]*L[j*astep + k];
00139             L[i*astep + j] = (_Tp)(s*L[j*astep + j]);
00140         }
00141         s = A[i*astep + i];
00142         for( k = 0; k < j; k++ )
00143         {
00144             double t = L[i*astep + k];
00145             s -= t*t;
00146         }
00147         if( s < std::numeric_limits<_Tp>::epsilon() )
00148             return false;
00149         L[i*astep + i] = (_Tp)(1./std::sqrt(s));
00150     }
00151 
00152     if( !b )
00153         return true;
00154 
00155     // LLt x = b
00156     // 1: L y = b
00157     // 2. Lt x = y
00158 
00159     /*
00160      [ L00             ]  y0   b0
00161      [ L10 L11         ]  y1 = b1
00162      [ L20 L21 L22     ]  y2   b2
00163      [ L30 L31 L32 L33 ]  y3   b3
00164 
00165      [ L00 L10 L20 L30 ]  x0   y0
00166      [     L11 L21 L31 ]  x1 = y1
00167      [         L22 L32 ]  x2   y2
00168      [             L33 ]  x3   y3
00169      */
00170 
00171     for( i = 0; i < m; i++ )
00172     {
00173         for( j = 0; j < n; j++ )
00174         {
00175             s = b[i*bstep + j];
00176             for( k = 0; k < i; k++ )
00177                 s -= L[i*astep + k]*b[k*bstep + j];
00178             b[i*bstep + j] = (_Tp)(s*L[i*astep + i]);
00179         }
00180     }
00181 
00182     for( i = m-1; i >= 0; i-- )
00183     {
00184         for( j = 0; j < n; j++ )
00185         {
00186             s = b[i*bstep + j];
00187             for( k = m-1; k > i; k-- )
00188                 s -= L[k*astep + i]*b[k*bstep + j];
00189             b[i*bstep + j] = (_Tp)(s*L[i*astep + i]);
00190         }
00191     }
00192 
00193     return true;
00194 }
00195 
00196 
00197 bool Cholesky32f(float* A, size_t astep, int m, float* b, size_t bstep, int n)
00198 {
00199     return CholImpl(A, astep, m, b, bstep, n);
00200 }
00201 
00202 bool Cholesky64f(double* A, size_t astep, int m, double* b, size_t bstep, int n)
00203 {
00204     return CholImpl(A, astep, m, b, bstep, n);
00205 }
00206 
00207 //=============================================================================
00208 // for compatibility with 3.0
00209 
00210 int LU(float* A, size_t astep, int m, float* b, size_t bstep, int n)
00211 {
00212     return LUImpl(A, astep, m, b, bstep, n, FLT_EPSILON*10);
00213 }
00214 
00215 int LU(double* A, size_t astep, int m, double* b, size_t bstep, int n)
00216 {
00217     return LUImpl(A, astep, m, b, bstep, n, DBL_EPSILON*100);
00218 }
00219 
00220 bool Cholesky(float* A, size_t astep, int m, float* b, size_t bstep, int n)
00221 {
00222     return CholImpl(A, astep, m, b, bstep, n);
00223 }
00224 
00225 bool Cholesky(double* A, size_t astep, int m, double* b, size_t bstep, int n)
00226 {
00227     return CholImpl(A, astep, m, b, bstep, n);
00228 }
00229 
00230 
00231 }}
00232