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 LU(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 LU(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 124 template<typename _Tp> static inline bool 125 CholImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n) 126 { 127 _Tp* L = A; 128 int i, j, k; 129 double s; 130 astep /= sizeof(A[0]); 131 bstep /= sizeof(b[0]); 132 133 for( i = 0; i < m; i++ ) 134 { 135 for( j = 0; j < i; j++ ) 136 { 137 s = A[i*astep + j]; 138 for( k = 0; k < j; k++ ) 139 s -= L[i*astep + k]*L[j*astep + k]; 140 L[i*astep + j] = (_Tp)(s*L[j*astep + j]); 141 } 142 s = A[i*astep + i]; 143 for( k = 0; k < j; k++ ) 144 { 145 double t = L[i*astep + k]; 146 s -= t*t; 147 } 148 if( s < std::numeric_limits<_Tp>::epsilon() ) 149 return false; 150 L[i*astep + i] = (_Tp)(1./std::sqrt(s)); 151 } 152 153 if( !b ) 154 return true; 155 156 // LLt x = b 157 // 1: L y = b 158 // 2. Lt x = y 159 160 /* 161 [ L00 ] y0 b0 162 [ L10 L11 ] y1 = b1 163 [ L20 L21 L22 ] y2 b2 164 [ L30 L31 L32 L33 ] y3 b3 165 166 [ L00 L10 L20 L30 ] x0 y0 167 [ L11 L21 L31 ] x1 = y1 168 [ L22 L32 ] x2 y2 169 [ L33 ] x3 y3 170 */ 171 172 for( i = 0; i < m; i++ ) 173 { 174 for( j = 0; j < n; j++ ) 175 { 176 s = b[i*bstep + j]; 177 for( k = 0; k < i; k++ ) 178 s -= L[i*astep + k]*b[k*bstep + j]; 179 b[i*bstep + j] = (_Tp)(s*L[i*astep + i]); 180 } 181 } 182 183 for( i = m-1; i >= 0; i-- ) 184 { 185 for( j = 0; j < n; j++ ) 186 { 187 s = b[i*bstep + j]; 188 for( k = m-1; k > i; k-- ) 189 s -= L[k*astep + i]*b[k*bstep + j]; 190 b[i*bstep + j] = (_Tp)(s*L[i*astep + i]); 191 } 192 } 193 194 return true; 195 } 196 197 198 bool Cholesky(float* A, size_t astep, int m, float* b, size_t bstep, int n) 199 { 200 return CholImpl(A, astep, m, b, bstep, n); 201 } 202 203 bool Cholesky(double* A, size_t astep, int m, double* b, size_t bstep, int n) 204 { 205 return CholImpl(A, astep, m, b, bstep, n); 206 } 207 208 }} 209
