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
LUImpl(_Tp * A,size_t astep,int m,_Tp * b,size_t bstep,int n,_Tp eps)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
LU(float * A,size_t astep,int m,float * b,size_t bstep,int n)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
LU(double * A,size_t astep,int m,double * b,size_t bstep,int n)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
CholImpl(_Tp * A,size_t astep,int m,_Tp * b,size_t bstep,int n)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
Cholesky(float * A,size_t astep,int m,float * b,size_t bstep,int n)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
Cholesky(double * A,size_t astep,int m,double * b,size_t bstep,int n)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