mirror of https://github.com/opencv/opencv.git
Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
503 lines
14 KiB
503 lines
14 KiB
#include "clapack.h" |
|
|
|
/* Table of constant values */ |
|
|
|
static integer c__1 = 1; |
|
|
|
/* Subroutine */ int slasd7_(integer *icompq, integer *nl, integer *nr, |
|
integer *sqre, integer *k, real *d__, real *z__, real *zw, real *vf, |
|
real *vfw, real *vl, real *vlw, real *alpha, real *beta, real *dsigma, |
|
integer *idx, integer *idxp, integer *idxq, integer *perm, integer * |
|
givptr, integer *givcol, integer *ldgcol, real *givnum, integer * |
|
ldgnum, real *c__, real *s, integer *info) |
|
{ |
|
/* System generated locals */ |
|
integer givcol_dim1, givcol_offset, givnum_dim1, givnum_offset, i__1; |
|
real r__1, r__2; |
|
|
|
/* Local variables */ |
|
integer i__, j, m, n, k2; |
|
real z1; |
|
integer jp; |
|
real eps, tau, tol; |
|
integer nlp1, nlp2, idxi, idxj; |
|
extern /* Subroutine */ int srot_(integer *, real *, integer *, real *, |
|
integer *, real *, real *); |
|
integer idxjp, jprev; |
|
extern /* Subroutine */ int scopy_(integer *, real *, integer *, real *, |
|
integer *); |
|
extern doublereal slapy2_(real *, real *), slamch_(char *); |
|
extern /* Subroutine */ int xerbla_(char *, integer *), slamrg_( |
|
integer *, integer *, real *, integer *, integer *, integer *); |
|
real hlftol; |
|
|
|
|
|
/* -- LAPACK auxiliary routine (version 3.1) -- */ |
|
/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */ |
|
/* November 2006 */ |
|
|
|
/* .. Scalar Arguments .. */ |
|
/* .. */ |
|
/* .. Array Arguments .. */ |
|
/* .. */ |
|
|
|
/* Purpose */ |
|
/* ======= */ |
|
|
|
/* SLASD7 merges the two sets of singular values together into a single */ |
|
/* sorted set. Then it tries to deflate the size of the problem. There */ |
|
/* are two ways in which deflation can occur: when two or more singular */ |
|
/* values are close together or if there is a tiny entry in the Z */ |
|
/* vector. For each such occurrence the order of the related */ |
|
/* secular equation problem is reduced by one. */ |
|
|
|
/* SLASD7 is called from SLASD6. */ |
|
|
|
/* Arguments */ |
|
/* ========= */ |
|
|
|
/* ICOMPQ (input) INTEGER */ |
|
/* Specifies whether singular vectors are to be computed */ |
|
/* in compact form, as follows: */ |
|
/* = 0: Compute singular values only. */ |
|
/* = 1: Compute singular vectors of upper */ |
|
/* bidiagonal matrix in compact form. */ |
|
|
|
/* NL (input) INTEGER */ |
|
/* The row dimension of the upper block. NL >= 1. */ |
|
|
|
/* NR (input) INTEGER */ |
|
/* The row dimension of the lower block. NR >= 1. */ |
|
|
|
/* SQRE (input) INTEGER */ |
|
/* = 0: the lower block is an NR-by-NR square matrix. */ |
|
/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */ |
|
|
|
/* The bidiagonal matrix has */ |
|
/* N = NL + NR + 1 rows and */ |
|
/* M = N + SQRE >= N columns. */ |
|
|
|
/* K (output) INTEGER */ |
|
/* Contains the dimension of the non-deflated matrix, this is */ |
|
/* the order of the related secular equation. 1 <= K <=N. */ |
|
|
|
/* D (input/output) REAL array, dimension ( N ) */ |
|
/* On entry D contains the singular values of the two submatrices */ |
|
/* to be combined. On exit D contains the trailing (N-K) updated */ |
|
/* singular values (those which were deflated) sorted into */ |
|
/* increasing order. */ |
|
|
|
/* Z (output) REAL array, dimension ( M ) */ |
|
/* On exit Z contains the updating row vector in the secular */ |
|
/* equation. */ |
|
|
|
/* ZW (workspace) REAL array, dimension ( M ) */ |
|
/* Workspace for Z. */ |
|
|
|
/* VF (input/output) REAL array, dimension ( M ) */ |
|
/* On entry, VF(1:NL+1) contains the first components of all */ |
|
/* right singular vectors of the upper block; and VF(NL+2:M) */ |
|
/* contains the first components of all right singular vectors */ |
|
/* of the lower block. On exit, VF contains the first components */ |
|
/* of all right singular vectors of the bidiagonal matrix. */ |
|
|
|
/* VFW (workspace) REAL array, dimension ( M ) */ |
|
/* Workspace for VF. */ |
|
|
|
/* VL (input/output) REAL array, dimension ( M ) */ |
|
/* On entry, VL(1:NL+1) contains the last components of all */ |
|
/* right singular vectors of the upper block; and VL(NL+2:M) */ |
|
/* contains the last components of all right singular vectors */ |
|
/* of the lower block. On exit, VL contains the last components */ |
|
/* of all right singular vectors of the bidiagonal matrix. */ |
|
|
|
/* VLW (workspace) REAL array, dimension ( M ) */ |
|
/* Workspace for VL. */ |
|
|
|
/* ALPHA (input) REAL */ |
|
/* Contains the diagonal element associated with the added row. */ |
|
|
|
/* BETA (input) REAL */ |
|
/* Contains the off-diagonal element associated with the added */ |
|
/* row. */ |
|
|
|
/* DSIGMA (output) REAL array, dimension ( N ) */ |
|
/* Contains a copy of the diagonal elements (K-1 singular values */ |
|
/* and one zero) in the secular equation. */ |
|
|
|
/* IDX (workspace) INTEGER array, dimension ( N ) */ |
|
/* This will contain the permutation used to sort the contents of */ |
|
/* D into ascending order. */ |
|
|
|
/* IDXP (workspace) INTEGER array, dimension ( N ) */ |
|
/* This will contain the permutation used to place deflated */ |
|
/* values of D at the end of the array. On output IDXP(2:K) */ |
|
/* points to the nondeflated D-values and IDXP(K+1:N) */ |
|
/* points to the deflated singular values. */ |
|
|
|
/* IDXQ (input) INTEGER array, dimension ( N ) */ |
|
/* This contains the permutation which separately sorts the two */ |
|
/* sub-problems in D into ascending order. Note that entries in */ |
|
/* the first half of this permutation must first be moved one */ |
|
/* position backward; and entries in the second half */ |
|
/* must first have NL+1 added to their values. */ |
|
|
|
/* PERM (output) INTEGER array, dimension ( N ) */ |
|
/* The permutations (from deflation and sorting) to be applied */ |
|
/* to each singular block. Not referenced if ICOMPQ = 0. */ |
|
|
|
/* GIVPTR (output) INTEGER */ |
|
/* The number of Givens rotations which took place in this */ |
|
/* subproblem. Not referenced if ICOMPQ = 0. */ |
|
|
|
/* GIVCOL (output) INTEGER array, dimension ( LDGCOL, 2 ) */ |
|
/* Each pair of numbers indicates a pair of columns to take place */ |
|
/* in a Givens rotation. Not referenced if ICOMPQ = 0. */ |
|
|
|
/* LDGCOL (input) INTEGER */ |
|
/* The leading dimension of GIVCOL, must be at least N. */ |
|
|
|
/* GIVNUM (output) REAL array, dimension ( LDGNUM, 2 ) */ |
|
/* Each number indicates the C or S value to be used in the */ |
|
/* corresponding Givens rotation. Not referenced if ICOMPQ = 0. */ |
|
|
|
/* LDGNUM (input) INTEGER */ |
|
/* The leading dimension of GIVNUM, must be at least N. */ |
|
|
|
/* C (output) REAL */ |
|
/* C contains garbage if SQRE =0 and the C-value of a Givens */ |
|
/* rotation related to the right null space if SQRE = 1. */ |
|
|
|
/* S (output) REAL */ |
|
/* S contains garbage if SQRE =0 and the S-value of a Givens */ |
|
/* rotation related to the right null space if SQRE = 1. */ |
|
|
|
/* INFO (output) INTEGER */ |
|
/* = 0: successful exit. */ |
|
/* < 0: if INFO = -i, the i-th argument had an illegal value. */ |
|
|
|
/* Further Details */ |
|
/* =============== */ |
|
|
|
/* Based on contributions by */ |
|
/* Ming Gu and Huan Ren, Computer Science Division, University of */ |
|
/* California at Berkeley, USA */ |
|
|
|
/* ===================================================================== */ |
|
|
|
/* .. Parameters .. */ |
|
/* .. */ |
|
/* .. Local Scalars .. */ |
|
|
|
/* .. */ |
|
/* .. External Subroutines .. */ |
|
/* .. */ |
|
/* .. External Functions .. */ |
|
/* .. */ |
|
/* .. Intrinsic Functions .. */ |
|
/* .. */ |
|
/* .. Executable Statements .. */ |
|
|
|
/* Test the input parameters. */ |
|
|
|
/* Parameter adjustments */ |
|
--d__; |
|
--z__; |
|
--zw; |
|
--vf; |
|
--vfw; |
|
--vl; |
|
--vlw; |
|
--dsigma; |
|
--idx; |
|
--idxp; |
|
--idxq; |
|
--perm; |
|
givcol_dim1 = *ldgcol; |
|
givcol_offset = 1 + givcol_dim1; |
|
givcol -= givcol_offset; |
|
givnum_dim1 = *ldgnum; |
|
givnum_offset = 1 + givnum_dim1; |
|
givnum -= givnum_offset; |
|
|
|
/* Function Body */ |
|
*info = 0; |
|
n = *nl + *nr + 1; |
|
m = n + *sqre; |
|
|
|
if (*icompq < 0 || *icompq > 1) { |
|
*info = -1; |
|
} else if (*nl < 1) { |
|
*info = -2; |
|
} else if (*nr < 1) { |
|
*info = -3; |
|
} else if (*sqre < 0 || *sqre > 1) { |
|
*info = -4; |
|
} else if (*ldgcol < n) { |
|
*info = -22; |
|
} else if (*ldgnum < n) { |
|
*info = -24; |
|
} |
|
if (*info != 0) { |
|
i__1 = -(*info); |
|
xerbla_("SLASD7", &i__1); |
|
return 0; |
|
} |
|
|
|
nlp1 = *nl + 1; |
|
nlp2 = *nl + 2; |
|
if (*icompq == 1) { |
|
*givptr = 0; |
|
} |
|
|
|
/* Generate the first part of the vector Z and move the singular */ |
|
/* values in the first part of D one position backward. */ |
|
|
|
z1 = *alpha * vl[nlp1]; |
|
vl[nlp1] = 0.f; |
|
tau = vf[nlp1]; |
|
for (i__ = *nl; i__ >= 1; --i__) { |
|
z__[i__ + 1] = *alpha * vl[i__]; |
|
vl[i__] = 0.f; |
|
vf[i__ + 1] = vf[i__]; |
|
d__[i__ + 1] = d__[i__]; |
|
idxq[i__ + 1] = idxq[i__] + 1; |
|
/* L10: */ |
|
} |
|
vf[1] = tau; |
|
|
|
/* Generate the second part of the vector Z. */ |
|
|
|
i__1 = m; |
|
for (i__ = nlp2; i__ <= i__1; ++i__) { |
|
z__[i__] = *beta * vf[i__]; |
|
vf[i__] = 0.f; |
|
/* L20: */ |
|
} |
|
|
|
/* Sort the singular values into increasing order */ |
|
|
|
i__1 = n; |
|
for (i__ = nlp2; i__ <= i__1; ++i__) { |
|
idxq[i__] += nlp1; |
|
/* L30: */ |
|
} |
|
|
|
/* DSIGMA, IDXC, IDXC, and ZW are used as storage space. */ |
|
|
|
i__1 = n; |
|
for (i__ = 2; i__ <= i__1; ++i__) { |
|
dsigma[i__] = d__[idxq[i__]]; |
|
zw[i__] = z__[idxq[i__]]; |
|
vfw[i__] = vf[idxq[i__]]; |
|
vlw[i__] = vl[idxq[i__]]; |
|
/* L40: */ |
|
} |
|
|
|
slamrg_(nl, nr, &dsigma[2], &c__1, &c__1, &idx[2]); |
|
|
|
i__1 = n; |
|
for (i__ = 2; i__ <= i__1; ++i__) { |
|
idxi = idx[i__] + 1; |
|
d__[i__] = dsigma[idxi]; |
|
z__[i__] = zw[idxi]; |
|
vf[i__] = vfw[idxi]; |
|
vl[i__] = vlw[idxi]; |
|
/* L50: */ |
|
} |
|
|
|
/* Calculate the allowable deflation tolerence */ |
|
|
|
eps = slamch_("Epsilon"); |
|
/* Computing MAX */ |
|
r__1 = dabs(*alpha), r__2 = dabs(*beta); |
|
tol = dmax(r__1,r__2); |
|
/* Computing MAX */ |
|
r__2 = (r__1 = d__[n], dabs(r__1)); |
|
tol = eps * 64.f * dmax(r__2,tol); |
|
|
|
/* There are 2 kinds of deflation -- first a value in the z-vector */ |
|
/* is small, second two (or more) singular values are very close */ |
|
/* together (their difference is small). */ |
|
|
|
/* If the value in the z-vector is small, we simply permute the */ |
|
/* array so that the corresponding singular value is moved to the */ |
|
/* end. */ |
|
|
|
/* If two values in the D-vector are close, we perform a two-sided */ |
|
/* rotation designed to make one of the corresponding z-vector */ |
|
/* entries zero, and then permute the array so that the deflated */ |
|
/* singular value is moved to the end. */ |
|
|
|
/* If there are multiple singular values then the problem deflates. */ |
|
/* Here the number of equal singular values are found. As each equal */ |
|
/* singular value is found, an elementary reflector is computed to */ |
|
/* rotate the corresponding singular subspace so that the */ |
|
/* corresponding components of Z are zero in this new basis. */ |
|
|
|
*k = 1; |
|
k2 = n + 1; |
|
i__1 = n; |
|
for (j = 2; j <= i__1; ++j) { |
|
if ((r__1 = z__[j], dabs(r__1)) <= tol) { |
|
|
|
/* Deflate due to small z component. */ |
|
|
|
--k2; |
|
idxp[k2] = j; |
|
if (j == n) { |
|
goto L100; |
|
} |
|
} else { |
|
jprev = j; |
|
goto L70; |
|
} |
|
/* L60: */ |
|
} |
|
L70: |
|
j = jprev; |
|
L80: |
|
++j; |
|
if (j > n) { |
|
goto L90; |
|
} |
|
if ((r__1 = z__[j], dabs(r__1)) <= tol) { |
|
|
|
/* Deflate due to small z component. */ |
|
|
|
--k2; |
|
idxp[k2] = j; |
|
} else { |
|
|
|
/* Check if singular values are close enough to allow deflation. */ |
|
|
|
if ((r__1 = d__[j] - d__[jprev], dabs(r__1)) <= tol) { |
|
|
|
/* Deflation is possible. */ |
|
|
|
*s = z__[jprev]; |
|
*c__ = z__[j]; |
|
|
|
/* Find sqrt(a**2+b**2) without overflow or */ |
|
/* destructive underflow. */ |
|
|
|
tau = slapy2_(c__, s); |
|
z__[j] = tau; |
|
z__[jprev] = 0.f; |
|
*c__ /= tau; |
|
*s = -(*s) / tau; |
|
|
|
/* Record the appropriate Givens rotation */ |
|
|
|
if (*icompq == 1) { |
|
++(*givptr); |
|
idxjp = idxq[idx[jprev] + 1]; |
|
idxj = idxq[idx[j] + 1]; |
|
if (idxjp <= nlp1) { |
|
--idxjp; |
|
} |
|
if (idxj <= nlp1) { |
|
--idxj; |
|
} |
|
givcol[*givptr + (givcol_dim1 << 1)] = idxjp; |
|
givcol[*givptr + givcol_dim1] = idxj; |
|
givnum[*givptr + (givnum_dim1 << 1)] = *c__; |
|
givnum[*givptr + givnum_dim1] = *s; |
|
} |
|
srot_(&c__1, &vf[jprev], &c__1, &vf[j], &c__1, c__, s); |
|
srot_(&c__1, &vl[jprev], &c__1, &vl[j], &c__1, c__, s); |
|
--k2; |
|
idxp[k2] = jprev; |
|
jprev = j; |
|
} else { |
|
++(*k); |
|
zw[*k] = z__[jprev]; |
|
dsigma[*k] = d__[jprev]; |
|
idxp[*k] = jprev; |
|
jprev = j; |
|
} |
|
} |
|
goto L80; |
|
L90: |
|
|
|
/* Record the last singular value. */ |
|
|
|
++(*k); |
|
zw[*k] = z__[jprev]; |
|
dsigma[*k] = d__[jprev]; |
|
idxp[*k] = jprev; |
|
|
|
L100: |
|
|
|
/* Sort the singular values into DSIGMA. The singular values which */ |
|
/* were not deflated go into the first K slots of DSIGMA, except */ |
|
/* that DSIGMA(1) is treated separately. */ |
|
|
|
i__1 = n; |
|
for (j = 2; j <= i__1; ++j) { |
|
jp = idxp[j]; |
|
dsigma[j] = d__[jp]; |
|
vfw[j] = vf[jp]; |
|
vlw[j] = vl[jp]; |
|
/* L110: */ |
|
} |
|
if (*icompq == 1) { |
|
i__1 = n; |
|
for (j = 2; j <= i__1; ++j) { |
|
jp = idxp[j]; |
|
perm[j] = idxq[idx[jp] + 1]; |
|
if (perm[j] <= nlp1) { |
|
--perm[j]; |
|
} |
|
/* L120: */ |
|
} |
|
} |
|
|
|
/* The deflated singular values go back into the last N - K slots of */ |
|
/* D. */ |
|
|
|
i__1 = n - *k; |
|
scopy_(&i__1, &dsigma[*k + 1], &c__1, &d__[*k + 1], &c__1); |
|
|
|
/* Determine DSIGMA(1), DSIGMA(2), Z(1), VF(1), VL(1), VF(M), and */ |
|
/* VL(M). */ |
|
|
|
dsigma[1] = 0.f; |
|
hlftol = tol / 2.f; |
|
if (dabs(dsigma[2]) <= hlftol) { |
|
dsigma[2] = hlftol; |
|
} |
|
if (m > n) { |
|
z__[1] = slapy2_(&z1, &z__[m]); |
|
if (z__[1] <= tol) { |
|
*c__ = 1.f; |
|
*s = 0.f; |
|
z__[1] = tol; |
|
} else { |
|
*c__ = z1 / z__[1]; |
|
*s = -z__[m] / z__[1]; |
|
} |
|
srot_(&c__1, &vf[m], &c__1, &vf[1], &c__1, c__, s); |
|
srot_(&c__1, &vl[m], &c__1, &vl[1], &c__1, c__, s); |
|
} else { |
|
if (dabs(z1) <= tol) { |
|
z__[1] = tol; |
|
} else { |
|
z__[1] = z1; |
|
} |
|
} |
|
|
|
/* Restore Z, VF, and VL. */ |
|
|
|
i__1 = *k - 1; |
|
scopy_(&i__1, &zw[2], &c__1, &z__[2], &c__1); |
|
i__1 = n - 1; |
|
scopy_(&i__1, &vfw[2], &c__1, &vf[2], &c__1); |
|
i__1 = n - 1; |
|
scopy_(&i__1, &vlw[2], &c__1, &vl[2], &c__1); |
|
|
|
return 0; |
|
|
|
/* End of SLASD7 */ |
|
|
|
} /* slasd7_ */
|
|
|