94 #define icvGivens_64f( n, x, y, c, s ) \ 100 for( _i = 0; _i < n; _i++ ) \ 102 double t0 = _x[_i]; \ 103 double t1 = _y[_i]; \ 104 _x[_i] = t0*c + t1*s; \ 105 _y[_i] = -t0*s + t1*c; \ 111 #define CV_CHECK_NANS( arr ) 112 #define CV_SWAP(a,b,t) ((t) = (a), (a) = (b), (b) = (t)) 115 #define CV_SVD_MODIFY_A (1 << 0) 116 #define CV_SVD_U_T (1 << 1) 117 #define CV_SVD_V_T (1 << 2) 120 #define cvStackAlloc malloc 127 icvMatrAXPY_64f(
int m,
int n,
const double*
x,
int dx,
128 const double*
a,
double*
y,
int dy )
132 for( i = 0; i < m; i++, x += dx, y += dy )
136 for( j = 0; j <= n - 4; j += 4 )
138 double t0 = y[j] + s*x[j];
139 double t1 = y[j+1] + s*x[j+1];
142 t0 = y[j+2] + s*x[j+2];
143 t1 = y[j+3] + s*x[j+3];
148 for( ; j <
n; j++ ) y[j] += s*x[j];
156 icvMatrAXPY3_64f(
int m,
int n,
const double* x,
int l,
double* y,
double h )
160 for( i = 1; i < m; i++ )
166 for( j = 0; j <= n - 4; j += 4 )
167 s += x[j]*y[j] + x[j+1]*y[j+1] + x[j+2]*y[j+2] + x[j+3]*y[j+3];
169 for( ; j <
n; j++ ) s += x[j]*y[j];
174 for( j = 0; j <= n - 4; j += 4 )
176 double t0 = y[j] + s*x[j];
177 double t1 = y[j+1] + s*x[j+1];
180 t0 = y[j+2] + s*x[j+2];
181 t1 = y[j+3] + s*x[j+3];
186 for( ; j <
n; j++ ) y[j] += s*x[j];
191 #define icvGivens_32f( n, x, y, c, s ) \ 197 for( _i = 0; _i < n; _i++ ) \ 199 double t0 = _x[_i]; \ 200 double t1 = _y[_i]; \ 201 _x[_i] = (float)(t0*c + t1*s); \ 202 _y[_i] = (float)(-t0*s + t1*c);\ 207 icvMatrAXPY_32f(
int m,
int n,
const float* x,
int dx,
208 const float* a,
float* y,
int dy )
212 for( i = 0; i < m; i++, x += dx, y += dy )
216 for( j = 0; j <= n - 4; j += 4 )
218 double t0 = y[j] + s*x[j];
219 double t1 = y[j+1] + s*x[j+1];
222 t0 = y[j+2] + s*x[j+2];
223 t1 = y[j+3] + s*x[j+3];
229 y[j] = (
float)(y[j] + s*x[j]);
235 icvMatrAXPY3_32f(
int m,
int n,
const float* x,
int l,
float* y,
double h )
239 for( i = 1; i < m; i++ )
244 for( j = 0; j <= n - 4; j += 4 )
245 s += x[j]*y[j] + x[j+1]*y[j+1] + x[j+2]*y[j+2] + x[j+3]*y[j+3];
247 for( ; j <
n; j++ ) s += x[j]*y[j];
250 y[-1] = (float)(s*x[-1]);
252 for( j = 0; j <= n - 4; j += 4 )
254 double t0 = y[j] + s*x[j];
255 double t1 = y[j+1] + s*x[j+1];
258 t0 = y[j+2] + s*x[j+2];
259 t1 = y[j+3] + s*x[j+3];
264 for( ; j <
n; j++ ) y[j] = (
float)(y[j] + s*x[j]);
270 pythag(
double a,
double b )
277 a *= sqrt( 1. + b * b );
282 a = b * sqrt( 1. + a * a );
294 icvSVD_64f(
double* a,
int lda,
int m,
int n,
296 double* uT,
int lduT,
int nu,
297 double* vT,
int ldvT,
304 double ku0 = 0, kv0 = 0;
306 double *a1, *u0 = uT, *
v0 = vT;
312 double* hv0 = (
double*)
cvStackAlloc( (m+2)*
sizeof(hv0[0])) + 1;
321 memset( w, 0, nm *
sizeof( w[0] ));
322 memset( e, 0, nm *
sizeof( e[0] ));
337 update_u = uT && m1 > m - nu;
338 hv = update_u ? uT : hv0;
340 for( j = 0, a1 = a; j < m1; j++, a1 += lda )
343 scale += fabs( hv[j] = t );
350 for( j = 0; j < m1; j++ )
352 double t = (hv[j] *= f);
361 h = 1. / (f * g -
s);
363 memset( temp, 0, n1 *
sizeof( temp[0] ));
366 icvMatrAXPY_64f( m1, n1 - 1, a + 1, lda, hv, temp + 1, 0 );
367 for( k = 1; k < n1; k++ ) temp[k] *= h;
370 icvMatrAXPY_64f( m1, n1 - 1, temp + 1, 0, hv, a + 1, lda );
393 update_v = vT && n1 > n - nv;
395 hv = update_v ? vT : hv0;
397 for( j = 0; j < n1; j++ )
400 scale += fabs( hv[j] = t );
407 for( j = 0; j < n1; j++ )
409 double t = (hv[j] *= f);
418 h = 1. / (f * g -
s);
422 icvMatrAXPY3_64f( m1, n1, hv, lda, a, h );
451 for( i = m1; i < nu; i++, uT += lduT )
453 memset( uT + m1, 0, (m - m1) *
sizeof( uT[0] ));
457 for( i = m1 - 1; i >= 0; i-- )
464 hv = u0 + (lduT + 1) * i;
465 h = i == 0 ? ku0 : hv[-1];
468 if (h > 0) printf(
"assert: h <= 0 not satisfied, h = %.2f\n", h);
473 icvMatrAXPY3_64f( lh, l-1, hv+1, lduT, uT+1, h );
476 for( k = 0; k < l; k++ ) hv[k] *= s;
481 for( j = 1; j < l; j++ )
483 for( j = 1; j < lh; j++ )
496 for( i = n1; i < nv; i++, vT += ldvT )
498 memset( vT + n1, 0, (n - n1) *
sizeof( vT[0] ));
502 for( i = n1 - 1; i >= 0; i-- )
508 hv = v0 + (ldvT + 1) * i;
509 h = i == 0 ? kv0 : hv[-1];
512 if (h > 0) printf(
"assert: h <= 0 not satisfied, h = %.2f (2)\n", h);
517 icvMatrAXPY3_64f( lh, l-1, hv+1, ldvT, vT+1, h );
520 for( k = 0; k < l; k++ ) hv[k] *= s;
525 for( j = 1; j < l; j++ )
527 for( j = 1; j < lh; j++ )
535 for( i = 0; i < nm; i++ )
537 double tnorm = fabs( w[i] );
538 tnorm += fabs( e[i] );
544 anorm *= DBL_EPSILON;
547 for( k = nm - 1; k >= 0; k-- )
554 double c,
s, f,
g,
x,
y;
558 for( l = k; l >= 0; l-- )
560 if( fabs(e[l]) <= anorm )
566 if (l <= 0) printf(
"assert: l > 0 not satisfied, l = %i\n", l);
567 if( fabs(w[l - 1]) <= anorm )
576 for( i = l; i <= k; i++ )
582 if( anorm + fabs( f ) == anorm )
592 icvGivens_64f( m, uT + lduT * (l - 1), uT + lduT * i, c, s );
605 f = 0.5 * (((g +
z) / h) * ((g -
z) / y) + y / h - h /
y);
609 f = x - (z /
x) * z + (h / x) * (y / (f +
g) - h);
613 for( i = l + 1; i <= k; i++ )
629 icvGivens_64f( n, vT + ldvT * (i - 1), vT + ldvT * i, c, s );
644 icvGivens_64f( m, uT + lduT * (i - 1), uT + lduT * i, c, s );
660 for( j = 0; j <
n; j++ )
661 vT[j + k * ldvT] = -vT[j + k * ldvT];
667 for( i = 0; i < nm; i++ )
670 for( j = i + 1; j < nm; j++ )
680 for( j = 0; j <
n; j++ )
681 CV_SWAP( vT[j + ldvT*k], vT[j + ldvT*i], t );
684 for( j = 0; j < m; j++ )
685 CV_SWAP( uT[j + lduT*k], uT[j + lduT*i], t );
694 icvSVD_32f(
float* a,
int lda,
int m,
int n,
696 float* uT,
int lduT,
int nu,
704 double ku0 = 0, kv0 = 0;
706 float *a1, *u0 = uT, *
v0 = vT;
712 float* hv0 = (
float*)
cvStackAlloc( (m+2)*
sizeof(hv0[0])) + 1;
722 memset( w, 0, nm *
sizeof( w[0] ));
723 memset( e, 0, nm *
sizeof( e[0] ));
739 update_u = uT && m1 > m - nu;
740 hv = update_u ? uT : hv0;
742 for( j = 0, a1 = a; j < m1; j++, a1 += lda )
745 scale += fabs( hv[j] = (
float)t );
752 for( j = 0; j < m1; j++ )
754 double t = (hv[j] = (float)(hv[j]*f));
762 hv[0] = (float)(f - g);
763 h = 1. / (f * g -
s);
765 memset( temp, 0, n1 *
sizeof( temp[0] ));
768 icvMatrAXPY_32f( m1, n1 - 1, a + 1, lda, hv, temp + 1, 0 );
770 for( k = 1; k < n1; k++ ) temp[k] = (
float)(temp[k]*h);
773 icvMatrAXPY_32f( m1, n1 - 1, temp + 1, 0, hv, a + 1, lda );
774 *w1 = (float)(g*scale);
796 update_v = vT && n1 > n - nv;
797 hv = update_v ? vT : hv0;
799 for( j = 0; j < n1; j++ )
802 scale += fabs( hv[j] = (
float)t );
809 for( j = 0; j < n1; j++ )
811 double t = (hv[j] = (float)(hv[j]*f));
819 hv[0] = (float)(f - g);
820 h = 1. / (f * g -
s);
824 icvMatrAXPY3_32f( m1, n1, hv, lda, a, h );
826 *e1 = (float)(g*scale);
853 for( i = m1; i < nu; i++, uT += lduT )
855 memset( uT + m1, 0, (m - m1) *
sizeof( uT[0] ));
859 for( i = m1 - 1; i >= 0; i-- )
866 hv = u0 + (lduT + 1) * i;
867 h = i == 0 ? ku0 : hv[-1];
870 if (h > 0) printf(
"assert: h <= 0 not satisfied, h = %.2f (3)\n", h);
875 icvMatrAXPY3_32f( lh, l-1, hv+1, lduT, uT+1, h );
878 for( k = 0; k < l; k++ ) hv[k] = (
float)(hv[k]*
s);
883 for( j = 1; j < l; j++ )
885 for( j = 1; j < lh; j++ )
898 for( i = n1; i < nv; i++, vT += ldvT )
900 memset( vT + n1, 0, (n - n1) *
sizeof( vT[0] ));
904 for( i = n1 - 1; i >= 0; i-- )
910 hv = v0 + (ldvT + 1) * i;
911 h = i == 0 ? kv0 : hv[-1];
914 if (h > 0) printf(
"assert: h <= 0 not satisfied, h = %.2f (4)\n", h);
919 icvMatrAXPY3_32f( lh, l-1, hv+1, ldvT, vT+1, h );
922 for( k = 0; k < l; k++ ) hv[k] = (
float)(hv[k]*
s);
927 for( j = 1; j < l; j++ )
929 for( j = 1; j < lh; j++ )
937 for( i = 0; i < nm; i++ )
939 double tnorm = fabs( w[i] );
940 tnorm += fabs( e[i] );
946 anorm *= FLT_EPSILON;
949 for( k = nm - 1; k >= 0; k-- )
956 double c,
s, f,
g,
x,
y;
960 for( l = k; l >= 0; l-- )
962 if( fabs( e[l] ) <= anorm )
968 if (l <= 0) printf(
"assert: l > 0 not satisfied, l = %i\n", l);
970 if( fabs( w[l - 1] ) <= anorm )
979 for( i = l; i <= k; i++ )
982 e[i] = (float)(e[i]*c);
984 if( anorm + fabs( f ) == anorm )
994 icvGivens_32f( m, uT + lduT * (l - 1), uT + lduT * i, c, s );
1007 f = 0.5 * (((g +
z) / h) * ((g -
z) / y) + y / h - h /
y);
1011 f = x - (z /
x) * z + (h / x) * (y / (f +
g) - h);
1015 for( i = l + 1; i <= k; i++ )
1022 e[i - 1] = (float)z;
1031 icvGivens_32f( n, vT + ldvT * (i - 1), vT + ldvT * i, c, s );
1034 w[i - 1] = (float)z;
1046 icvGivens_32f( m, uT + lduT * (i - 1), uT + lduT * i, c, s );
1062 for( j = 0; j <
n; j++ )
1063 vT[j + k * ldvT] = -vT[j + k * ldvT];
1069 for( i = 0; i < nm; i++ )
1072 for( j = i + 1; j < nm; j++ )
1082 for( j = 0; j <
n; j++ )
1083 CV_SWAP( vT[j + ldvT*k], vT[j + ldvT*i], t );
1086 for( j = 0; j < m; j++ )
1087 CV_SWAP( uT[j + lduT*k], uT[j + lduT*i], t );
1096 icvSVBkSb_64f(
int m,
int n,
const double* w,
1097 const double* uT,
int lduT,
1098 const double* vT,
int ldvT,
1099 const double* b,
int ldb,
int nb,
1100 double* x,
int ldx,
double* buffer )
1102 double threshold = 0;
1103 int i, j, nm =
MY_MIN( m, n );
1108 for( i = 0; i <
n; i++ )
1109 memset( x + i*ldx, 0, nb*
sizeof(x[0]));
1111 for( i = 0; i < nm; i++ )
1113 threshold *= 2*DBL_EPSILON;
1116 for( i = 0; i < nm; i++, uT += lduT, vT += ldvT )
1120 if( wi > threshold )
1131 for( j = 0; j <= m - 4; j += 4 )
1132 s += uT[j]*b[j] + uT[j+1]*b[j+1] + uT[j+2]*b[j+2] + uT[j+3]*b[j+3];
1138 for( j = 0; j < m; j++ )
1139 s += uT[j]*b[j*ldb];
1147 for( j = 0; j <= n - 4; j += 4 )
1149 double t0 = x[j] + s*vT[j];
1150 double t1 = x[j+1] + s*vT[j+1];
1153 t0 = x[j+2] + s*vT[j+2];
1154 t1 = x[j+3] + s*vT[j+3];
1164 for( j = 0; j <
n; j++ )
1165 x[j*ldx] += s*vT[j];
1172 memset( buffer, 0, nb*
sizeof(buffer[0]));
1173 icvMatrAXPY_64f( m, nb, b, ldb, uT, buffer, 0 );
1174 for( j = 0; j < nb; j++ )
1179 for( j = 0; j < nb; j++ )
1180 buffer[j] = uT[j]*wi;
1182 icvMatrAXPY_64f( n, nb, buffer, 0, vT, x, ldx );
1190 icvSVBkSb_32f(
int m,
int n,
const float* w,
1191 const float* uT,
int lduT,
1192 const float* vT,
int ldvT,
1193 const float* b,
int ldb,
int nb,
1194 float* x,
int ldx,
float* buffer )
1196 float threshold = 0.f;
1197 int i, j, nm =
MY_MIN( m, n );
1202 for( i = 0; i <
n; i++ )
1203 memset( x + i*ldx, 0, nb*
sizeof(x[0]));
1205 for( i = 0; i < nm; i++ )
1207 threshold *= 2*FLT_EPSILON;
1210 for( i = 0; i < nm; i++, uT += lduT, vT += ldvT )
1214 if( wi > threshold )
1225 for( j = 0; j <= m - 4; j += 4 )
1226 s += uT[j]*b[j] + uT[j+1]*b[j+1] + uT[j+2]*b[j+2] + uT[j+3]*b[j+3];
1232 for( j = 0; j < m; j++ )
1233 s += uT[j]*b[j*ldb];
1242 for( j = 0; j <= n - 4; j += 4 )
1244 double t0 = x[j] + s*vT[j];
1245 double t1 = x[j+1] + s*vT[j+1];
1248 t0 = x[j+2] + s*vT[j+2];
1249 t1 = x[j+3] + s*vT[j+3];
1255 x[j] = (
float)(x[j] + s*vT[j]);
1259 for( j = 0; j <
n; j++ )
1260 x[j*ldx] = (
float)(x[j*ldx] + s*vT[j]);
1267 memset( buffer, 0, nb*
sizeof(buffer[0]));
1268 icvMatrAXPY_32f( m, nb, b, ldb, uT, buffer, 0 );
1269 for( j = 0; j < nb; j++ )
1270 buffer[j] = (
float)(buffer[j]*wi);
1274 for( j = 0; j < nb; j++ )
1275 buffer[j] = (
float)(uT[j]*wi);
1277 icvMatrAXPY_32f( n, nb, buffer, 0, vT, x, ldx );
1287 typedef unsigned char uchar;
1299 int a_buf_offset = 0, u_buf_offset = 0, buf_size, pix_size;
1304 int u_rows = 0, u_cols = 0;
1332 w_is_mat = w_cols > 1 && w_rows > 1;
1333 if (!w_is_mat && w_cols + w_rows - 1 == n)
1334 tw = (uchar*) w->
data;
1349 if( u_rows != m || (u_cols != m && u_cols != n))
1350 !t_svd ? printf(
"U matrix has unappropriate size" ) : printf(
"V matrix has unappropriate size" );
1354 if( w_is_mat && u_cols != w_rows )
1355 !t_svd ? printf(
"U and W have incompatible sizes" ) : printf(
"V and W have incompatible sizes" );
1380 if( v_rows != n || v_cols != n )
1381 t_svd ? printf(
"U matrix has unappropriate size") : printf(
"V matrix has unappropriate size" );
1383 if( w_is_mat && w_cols != v_cols )
1384 t_svd ? printf(
"U and W have incompatible sizes") : printf(
"V and W have incompatible sizes" );
1394 pix_size =
sizeof(float);
1399 a_buf_offset = buf_size;
1405 u_buf_offset = buf_size;
1409 buf_size *= pix_size;
1411 buffer = (uchar*)malloc( buf_size );
1413 if( !(flags & CV_SVD_MODIFY_A) )
1417 tmat.
data = (
float *)(buffer + a_buf_offset * pix_size);
1428 ustub.
rows = u_cols;
1430 ustub.
data = (
float *)(buffer + u_buf_offset * pix_size);
1435 tw = buffer + (n + m)*pix_size;
1444 if( (
void *) tw != (
void *) w->
data )
1449 for(
int i = 0; i <
n; i++ )
1450 ((
float*)(((
unsigned char *) w->
data) + i*(w->
columns*
sizeof(
float))))[i*shift] = ((
float*)tw)[i];
1474 typedef unsigned char uchar;
1486 int a_buf_offset = 0, u_buf_offset = 0, buf_size, pix_size;
1491 int u_rows = 0, u_cols = 0;
1519 w_is_mat = w_cols > 1 && w_rows > 1;
1520 if (!w_is_mat && w_cols + w_rows - 1 == n)
1521 tw = (uchar*) w->
data;
1536 if( u_rows != m || (u_cols != m && u_cols != n))
1537 !t_svd ? printf(
"U matrix has unappropriate size" ) : printf(
"V matrix has unappropriate size" );
1541 if( w_is_mat && u_cols != w_rows )
1542 !t_svd ? printf(
"U and W have incompatible sizes" ) : printf(
"V and W have incompatible sizes" );
1567 if( v_rows != n || v_cols != n )
1568 t_svd ? printf(
"U matrix has unappropriate size" ) : printf(
"V matrix has unappropriate size" );
1570 if( w_is_mat && w_cols != v_cols )
1571 t_svd ? printf(
"U and W have incompatible sizes" ) : printf(
"V and W have incompatible sizes" );
1581 pix_size =
sizeof(double);
1586 a_buf_offset = buf_size;
1592 u_buf_offset = buf_size;
1596 buf_size *= pix_size;
1598 buffer = (uchar*)malloc( buf_size );
1600 if( !(flags & CV_SVD_MODIFY_A) )
1604 tmat.
data = (
double *)(buffer + a_buf_offset * pix_size);
1615 ustub.
rows = u_cols;
1617 ustub.
data = (
double *)(buffer + u_buf_offset * pix_size);
1622 tw = buffer + (n + m)*pix_size;
1631 if( (
void *) tw != (
void *) w->
data )
1636 for(
int i = 0; i <
n; i++ )
1637 ((
double*)(((
unsigned char *) w->
data) + i*(w->
columns*
sizeof(
double))))[i*shift] = ((
double*)tw)[i];
1662 const int columns = A->
columns;
1663 const int rows = A->
rows;
1667 printf(
"error: W should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, rows);
1673 printf(
"error: U should have %i columns and %i rows for LinearAlgebra::SVD\n", rows, rows);
1677 if (V && (V->
columns != columns || V->
rows != columns))
1679 printf(
"error: V should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, columns);
1688 if (bReturnUTransposed)
1691 if (bReturnVTransposed)
1694 cvSVD(A, W, U, V, flags);
1699 const int columns = A->
columns;
1700 const int rows = A->
rows;
1704 printf(
"error: W should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, rows);
1710 printf(
"error: U should have %i columns and %i rows for LinearAlgebra::SVD\n", rows, rows);
1714 if (V && (V->
columns != columns || V->
rows != columns))
1716 printf(
"error: V should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, columns);
1725 if (bReturnUTransposed)
1728 if (bReturnVTransposed)
1731 cvSVD(A, W, U, V, flags);
GLenum GLenum GLenum GLenum GLenum scale
void SVD(const CFloatMatrix *A, CFloatMatrix *W, CFloatMatrix *U=0, CFloatMatrix *V=0, bool bAllowModifyA=false, bool bReturnUTransposed=false, bool bReturnVTransposed=false)
#define icvGivens_32f(n, x, y, c, s)
bool CopyMatrix(const CFloatMatrix *pInputImage, CFloatMatrix *pOutputImage)
Copies one CFloatMatrix to another.
Data structure for the representation of a matrix of values of the data type float.
GLubyte GLubyte GLubyte a
Data structure for the representation of a matrix of values of the data type double.
void Transpose(const CFloatMatrix *pMatrix, CFloatMatrix *pResultMatrix)
#define icvGivens_64f(n, x, y, c, s)
void Zero(CByteImage *pImage, const MyRegion *pROI=0)
Sets all values in a CByteImage to zero.
GLubyte GLubyte GLubyte GLubyte w
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1