/*
Returns the Nesterov-Todd scaling W at points s and z, and stores the
scaled variable in lmbda.
W * z = W^{-T} * s = lmbda.
W is a MatrixSet with entries:
- W['dnl']: positive vector
- W['dnli']: componentwise inverse of W['dnl']
- W['d']: positive vector
- W['di']: componentwise inverse of W['d']
- W['v']: lists of 2nd order cone vectors with unit hyperbolic norms
- W['beta']: list of positive numbers
- W['r']: list of square matrices
- W['rti']: list of square matrices. rti[k] is the inverse transpose
of r[k].
*/
func computeScaling(s, z, lmbda *matrix.FloatMatrix, dims *sets.DimensionSet, mnl int) (W *sets.FloatMatrixSet, err error) {
/*DEBUGGED*/
err = nil
W = sets.NewFloatSet("dnl", "dnli", "d", "di", "v", "beta", "r", "rti")
// For the nonlinear block:
//
// W['dnl'] = sqrt( s[:mnl] ./ z[:mnl] )
// W['dnli'] = sqrt( z[:mnl] ./ s[:mnl] )
// lambda[:mnl] = sqrt( s[:mnl] .* z[:mnl] )
var stmp, ztmp, lmd *matrix.FloatMatrix
if mnl > 0 {
stmp = matrix.FloatVector(s.FloatArray()[:mnl])
ztmp = matrix.FloatVector(z.FloatArray()[:mnl])
//dnl := stmp.Div(ztmp)
//dnl.Apply(dnl, math.Sqrt)
dnl := matrix.Sqrt(matrix.Div(stmp, ztmp))
//dnli := dnl.Copy()
//dnli.Apply(dnli, func(a float64)float64 { return 1.0/a })
dnli := matrix.Inv(dnl)
W.Set("dnl", dnl)
W.Set("dnli", dnli)
//lmd = stmp.Mul(ztmp)
//lmd.Apply(lmd, math.Sqrt)
lmd = matrix.Sqrt(matrix.Mul(stmp, ztmp))
lmbda.SetIndexesFromArray(lmd.FloatArray(), matrix.MakeIndexSet(0, mnl, 1)...)
} else {
// set for empty matrices
//W.Set("dnl", matrix.FloatZeros(0, 1))
//W.Set("dnli", matrix.FloatZeros(0, 1))
mnl = 0
}
// For the 'l' block:
//
// W['d'] = sqrt( sk ./ zk )
// W['di'] = sqrt( zk ./ sk )
// lambdak = sqrt( sk .* zk )
//
// where sk and zk are the first dims['l'] entries of s and z.
// lambda_k is stored in the first dims['l'] positions of lmbda.
m := dims.At("l")[0]
//td := s.FloatArray()
stmp = matrix.FloatVector(s.FloatArray()[mnl : mnl+m])
//zd := z.FloatArray()
ztmp = matrix.FloatVector(z.FloatArray()[mnl : mnl+m])
//fmt.Printf(".Sqrt()=\n%v\n", matrix.Div(stmp, ztmp).Sqrt().ToString("%.17f"))
//d := stmp.Div(ztmp)
//d.Apply(d, math.Sqrt)
d := matrix.Div(stmp, ztmp).Sqrt()
//di := d.Copy()
//di.Apply(di, func(a float64)float64 { return 1.0/a })
di := matrix.Inv(d)
//fmt.Printf("d:\n%v\n", d)
//fmt.Printf("di:\n%v\n", di)
W.Set("d", d)
W.Set("di", di)
//lmd = stmp.Mul(ztmp)
//lmd.Apply(lmd, math.Sqrt)
lmd = matrix.Mul(stmp, ztmp).Sqrt()
// lmd has indexes mnl:mnl+m and length of m
lmbda.SetIndexesFromArray(lmd.FloatArray(), matrix.MakeIndexSet(mnl, mnl+m, 1)...)
//fmt.Printf("after l:\n%v\n", lmbda)
/*
For the 'q' blocks, compute lists 'v', 'beta'.
The vector v[k] has unit hyperbolic norm:
(sqrt( v[k]' * J * v[k] ) = 1 with J = [1, 0; 0, -I]).
beta[k] is a positive scalar.
The hyperbolic Householder matrix H = 2*v[k]*v[k]' - J
defined by v[k] satisfies
(beta[k] * H) * zk = (beta[k] * H) \ sk = lambda_k
where sk = s[indq[k]:indq[k+1]], zk = z[indq[k]:indq[k+1]].
lambda_k is stored in lmbda[indq[k]:indq[k+1]].
*/
ind := mnl + dims.At("l")[0]
var beta *matrix.FloatMatrix
for _, k := range dims.At("q") {
W.Append("v", matrix.FloatZeros(k, 1))
}
beta = matrix.FloatZeros(len(dims.At("q")), 1)
W.Set("beta", beta)
vset := W.At("v")
for k, m := range dims.At("q") {
v := vset[k]
// a = sqrt( sk' * J * sk ) where J = [1, 0; 0, -I]
aa := jnrm2(s, m, ind)
// b = sqrt( zk' * J * zk )
bb := jnrm2(z, m, ind)
// beta[k] = ( a / b )**1/2
beta.SetIndex(k, math.Sqrt(aa/bb))
// c = sqrt( (sk/a)' * (zk/b) + 1 ) / sqrt(2)
c0 := blas.DotFloat(s, z, &la_.IOpt{"n", m},
&la_.IOpt{"offsetx", ind}, &la_.IOpt{"offsety", ind})
cc := math.Sqrt((c0/aa/bb + 1.0) / 2.0)
// vk = 1/(2*c) * ( (sk/a) + J * (zk/b) )
blas.CopyFloat(z, v, &la_.IOpt{"offsetx", ind}, &la_.IOpt{"n", m})
blas.ScalFloat(v, -1.0/bb)
v.SetIndex(0, -1.0*v.GetIndex(0))
blas.AxpyFloat(s, v, 1.0/aa, &la_.IOpt{"offsetx", ind}, &la_.IOpt{"n", m})
blas.ScalFloat(v, 1.0/2.0/cc)
// v[k] = 1/sqrt(2*(vk0 + 1)) * ( vk + e ), e = [1; 0]
v.SetIndex(0, v.GetIndex(0)+1.0)
blas.ScalFloat(v, (1.0 / math.Sqrt(2.0*v.GetIndex(0))))
/*
To get the scaled variable lambda_k
d = sk0/a + zk0/b + 2*c
lambda_k = [ c;
(c + zk0/b)/d * sk1/a + (c + sk0/a)/d * zk1/b ]
lambda_k *= sqrt(a * b)
*/
lmbda.SetIndex(ind, cc)
dd := 2*cc + s.GetIndex(ind)/aa + z.GetIndex(ind)/bb
blas.CopyFloat(s, lmbda, &la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", ind + 1},
&la_.IOpt{"n", m - 1})
zz := (cc + z.GetIndex(ind)/bb) / dd / aa
ss := (cc + s.GetIndex(ind)/aa) / dd / bb
blas.ScalFloat(lmbda, zz, &la_.IOpt{"offset", ind + 1}, &la_.IOpt{"n", m - 1})
blas.AxpyFloat(z, lmbda, ss, &la_.IOpt{"offsetx", ind + 1},
&la_.IOpt{"offsety", ind + 1}, &la_.IOpt{"n", m - 1})
blas.ScalFloat(lmbda, math.Sqrt(aa*bb), &la_.IOpt{"offset", ind}, &la_.IOpt{"n", m})
ind += m
//fmt.Printf("after q[%d]:\n%v\n", k, lmbda)
}
/*
For the 's' blocks: compute two lists 'r' and 'rti'.
r[k]' * sk^{-1} * r[k] = diag(lambda_k)^{-1}
r[k]' * zk * r[k] = diag(lambda_k)
where sk and zk are the entries inds[k] : inds[k+1] of
s and z, reshaped into symmetric matrices.
rti[k] is the inverse of r[k]', so
rti[k]' * sk * rti[k] = diag(lambda_k)^{-1}
rti[k]' * zk^{-1} * rti[k] = diag(lambda_k).
The vectors lambda_k are stored in
lmbda[ dims['l'] + sum(dims['q']) : -1 ]
*/
for _, k := range dims.At("s") {
W.Append("r", matrix.FloatZeros(k, k))
W.Append("rti", matrix.FloatZeros(k, k))
}
maxs := maxdim(dims.At("s"))
work := matrix.FloatZeros(maxs*maxs, 1)
Ls := matrix.FloatZeros(maxs*maxs, 1)
Lz := matrix.FloatZeros(maxs*maxs, 1)
ind2 := ind
for k, m := range dims.At("s") {
r := W.At("r")[k]
rti := W.At("rti")[k]
// Factor sk = Ls*Ls'; store Ls in ds[inds[k]:inds[k+1]].
blas.CopyFloat(s, Ls, &la_.IOpt{"offsetx", ind2}, &la_.IOpt{"n", m * m})
lapack.PotrfFloat(Ls, &la_.IOpt{"n", m}, &la_.IOpt{"lda", m})
// Factor zs[k] = Lz*Lz'; store Lz in dz[inds[k]:inds[k+1]].
blas.CopyFloat(z, Lz, &la_.IOpt{"offsetx", ind2}, &la_.IOpt{"n", m * m})
lapack.PotrfFloat(Lz, &la_.IOpt{"n", m}, &la_.IOpt{"lda", m})
// SVD Lz'*Ls = U*diag(lambda_k)*V'. Keep U in work.
for i := 0; i < m; i++ {
blas.ScalFloat(Ls, 0.0, &la_.IOpt{"offset", i * m}, &la_.IOpt{"n", i})
}
blas.CopyFloat(Ls, work, &la_.IOpt{"n", m * m})
blas.TrmmFloat(Lz, work, 1.0, la_.OptTransA, &la_.IOpt{"lda", m}, &la_.IOpt{"ldb", m},
&la_.IOpt{"n", m}, &la_.IOpt{"m", m})
lapack.GesvdFloat(work, lmbda, nil, nil,
la_.OptJobuO, &la_.IOpt{"lda", m}, &la_.IOpt{"offsetS", ind},
&la_.IOpt{"n", m}, &la_.IOpt{"m", m})
// r = Lz^{-T} * U
blas.CopyFloat(work, r, &la_.IOpt{"n", m * m})
blas.TrsmFloat(Lz, r, 1.0, la_.OptTransA,
&la_.IOpt{"lda", m}, &la_.IOpt{"n", m}, &la_.IOpt{"m", m})
// rti = Lz * U
blas.CopyFloat(work, rti, &la_.IOpt{"n", m * m})
blas.TrmmFloat(Lz, rti, 1.0,
&la_.IOpt{"lda", m}, &la_.IOpt{"n", m}, &la_.IOpt{"m", m})
// r := r * diag(sqrt(lambda_k))
// rti := rti * diag(1 ./ sqrt(lambda_k))
for i := 0; i < m; i++ {
a := math.Sqrt(lmbda.GetIndex(ind + i))
blas.ScalFloat(r, a, &la_.IOpt{"offset", m * i}, &la_.IOpt{"n", m})
blas.ScalFloat(rti, 1.0/a, &la_.IOpt{"offset", m * i}, &la_.IOpt{"n", m})
}
ind += m
ind2 += m * m
}
return
}
func trsmSolve(t *testing.T, A *matrix.FloatMatrix, flags Flags, rand bool, nrhs, nb int) bool {
var B0 *matrix.FloatMatrix
side := linalg.OptLeft
trans := linalg.OptNoTrans
N := A.Cols()
S := 0
E := A.Rows()
_ = S
_ = E
if flags&RIGHT != 0 {
if rand {
B0 = matrix.FloatNormal(nrhs, A.Rows())
} else {
B0 = matrix.FloatWithValue(nrhs, A.Rows(), 2.0)
}
side = linalg.OptRight
E = B0.Rows()
} else {
if rand {
B0 = matrix.FloatNormal(A.Rows(), nrhs)
} else {
B0 = matrix.FloatWithValue(A.Rows(), nrhs, 2.0)
}
E = B0.Cols()
}
B1 := B0.Copy()
diag := linalg.OptNonUnit
if flags&UNIT != 0 {
diag = linalg.OptUnit
}
uplo := linalg.OptUpper
if flags&LOWER != 0 {
uplo = linalg.OptLower
}
if flags&TRANSA != 0 {
trans = linalg.OptTransA
}
blas.TrsmFloat(A, B0, 1.0, uplo, diag, side, trans)
Ar := A.FloatArray()
Br := B1.FloatArray()
if nb == 0 || nb == N {
DSolveUnblk(Br, Ar, 1.0, flags, B1.LeadingIndex(), A.LeadingIndex(), N, S, E)
} else {
DSolveBlk(Br, Ar, 1.0, flags, B1.LeadingIndex(), A.LeadingIndex(), N, S, E, nb)
}
result := B1.AllClose(B0)
t.Logf("B1 == B0: %v\n", result)
if !result {
if nrhs < 10 {
t.Logf("blas: B0\n%v\n", B0)
t.Logf("B1:\n%v\n", B1)
} else {
b0 := B0.FloatArray()
b1 := B1.FloatArray()
for k := 0; k < len(b0); k++ {
if !isClose(b0[k], b1[k]) {
t.Logf("first divergences at %d ... col %d, row %d\n", k, k/B0.Rows(), k%B0.Rows())
break
}
}
}
}
return result
}
func kktChol2(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {
if len(dims.At("q")) > 0 || len(dims.At("s")) > 0 {
return nil, errors.New("'chol2' solver only for problems with no second-order or " +
"semidefinite cone constraints")
}
p, n := A.Size()
ml := dims.At("l")[0]
F := &chol2Data{firstcall: true, singular: false, A: A, G: G, dims: dims}
factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
var err error = nil
minor := 0
if !checkpnt.MinorEmpty() {
minor = checkpnt.MinorTop()
}
if F.firstcall {
F.Gs = matrix.FloatZeros(F.G.Size())
if mnl > 0 {
F.Dfs = matrix.FloatZeros(Df.Size())
}
F.S = matrix.FloatZeros(n, n)
F.K = matrix.FloatZeros(p, p)
checkpnt.AddMatrixVar("Gs", F.Gs)
checkpnt.AddMatrixVar("Dfs", F.Dfs)
checkpnt.AddMatrixVar("S", F.S)
checkpnt.AddMatrixVar("K", F.K)
}
if mnl > 0 {
dnli := matrix.FloatZeros(mnl, mnl)
dnli.SetIndexesFromArray(W.At("dnli")[0].FloatArray(), matrix.DiagonalIndexes(dnli)...)
blas.GemmFloat(dnli, Df, F.Dfs, 1.0, 0.0)
}
checkpnt.Check("02factor_chol2", minor)
di := matrix.FloatZeros(ml, ml)
di.SetIndexesFromArray(W.At("di")[0].FloatArray(), matrix.DiagonalIndexes(di)...)
err = blas.GemmFloat(di, G, F.Gs, 1.0, 0.0)
checkpnt.Check("06factor_chol2", minor)
if F.firstcall {
blas.SyrkFloat(F.Gs, F.S, 1.0, 0.0, la.OptTrans)
if mnl > 0 {
blas.SyrkFloat(F.Dfs, F.S, 1.0, 1.0, la.OptTrans)
}
if H != nil {
F.S.Plus(H)
}
checkpnt.Check("10factor_chol2", minor)
err = lapack.Potrf(F.S)
if err != nil {
err = nil // reset error
F.singular = true
// original code recreates F.S as dense if it is sparse and
// A is dense, we don't do it as currently no sparse matrices
//F.S = matrix.FloatZeros(n, n)
//checkpnt.AddMatrixVar("S", F.S)
blas.SyrkFloat(F.Gs, F.S, 1.0, 0.0, la.OptTrans)
if mnl > 0 {
blas.SyrkFloat(F.Dfs, F.S, 1.0, 1.0, la.OptTrans)
}
checkpnt.Check("14factor_chol2", minor)
blas.SyrkFloat(F.A, F.S, 1.0, 1.0, la.OptTrans)
if H != nil {
F.S.Plus(H)
}
lapack.Potrf(F.S)
}
F.firstcall = false
checkpnt.Check("20factor_chol2", minor)
} else {
blas.SyrkFloat(F.Gs, F.S, 1.0, 0.0, la.OptTrans)
if mnl > 0 {
blas.SyrkFloat(F.Dfs, F.S, 1.0, 1.0, la.OptTrans)
}
if H != nil {
F.S.Plus(H)
}
checkpnt.Check("40factor_chol2", minor)
if F.singular {
blas.SyrkFloat(F.A, F.S, 1.0, 1.0, la.OptTrans)
}
lapack.Potrf(F.S)
checkpnt.Check("50factor_chol2", minor)
}
// Asct := L^{-1}*A'. Factor K = Asct'*Asct.
Asct := F.A.Transpose()
blas.TrsmFloat(F.S, Asct, 1.0)
blas.SyrkFloat(Asct, F.K, 1.0, 0.0, la.OptTrans)
lapack.Potrf(F.K)
checkpnt.Check("90factor_chol2", minor)
solve := func(x, y, z *matrix.FloatMatrix) (err error) {
// Solve
//
// [ H A' GG'*W^{-1} ] [ ux ] [ bx ]
// [ A 0 0 ] * [ uy ] = [ by ]
// [ W^{-T}*GG 0 -I ] [ W*uz ] [ W^{-T}*bz ]
//
// and return ux, uy, W*uz.
//
// If not F['singular']:
//
// K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz ) - by
// S*ux = bx + GG'*W^{-1}*W^{-T}*bz - A'*uy
// W*uz = W^{-T} * ( GG*ux - bz ).
//
// If F['singular']:
//
// K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz + A'*by )
// - by
// S*ux = bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y.
// W*uz = W^{-T} * ( GG*ux - bz ).
minor := 0
if !checkpnt.MinorEmpty() {
minor = checkpnt.MinorTop()
}
// z := W^{-1} * z = W^{-1} * bz
scale(z, W, true, true)
checkpnt.Check("10solve_chol2", minor)
// If not F['singular']:
// x := L^{-1} * P * (x + GGs'*z)
// = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz)
//
// If F['singular']:
// x := L^{-1} * P * (x + GGs'*z + A'*y))
// = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz + A'*y)
if mnl > 0 {
blas.GemvFloat(F.Dfs, z, x, 1.0, 1.0, la.OptTrans)
}
blas.GemvFloat(F.Gs, z, x, 1.0, 1.0, la.OptTrans, &la.IOpt{"offsetx", mnl})
//checkpnt.Check("20solve_chol2", minor)
if F.singular {
blas.GemvFloat(F.A, y, x, 1.0, 1.0, la.OptTrans)
}
checkpnt.Check("30solve_chol2", minor)
blas.TrsvFloat(F.S, x)
//checkpnt.Check("50solve_chol2", minor)
// y := K^{-1} * (Asc*x - y)
// = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz) - by)
// (if not F['singular'])
// = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz +
// A'*by) - by)
// (if F['singular']).
blas.GemvFloat(Asct, x, y, 1.0, -1.0, la.OptTrans)
//checkpnt.Check("55solve_chol2", minor)
lapack.Potrs(F.K, y)
//checkpnt.Check("60solve_chol2", minor)
// x := P' * L^{-T} * (x - Asc'*y)
// = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz - A'*y)
// (if not F['singular'])
// = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y)
// (if F['singular'])
blas.GemvFloat(Asct, y, x, -1.0, 1.0)
blas.TrsvFloat(F.S, x, la.OptTrans)
checkpnt.Check("70solve_chol2", minor)
// W*z := GGs*x - z = W^{-T} * (GG*x - bz)
if mnl > 0 {
blas.GemvFloat(F.Dfs, x, z, 1.0, -1.0)
}
blas.GemvFloat(F.Gs, x, z, 1.0, -1.0, &la.IOpt{"offsety", mnl})
checkpnt.Check("90solve_chol2", minor)
return nil
}
return solve, err
}
return factor, nil
}