// Solve multiple right sides. If flags&UNIT then A diagonal is assumed to
// to unit and is not referenced. (blas.TRSM)
// alpha*B = A.-1*B if flags&LEFT
// alpha*B = A.-T*B if flags&(LEFT|TRANS)
// alpha*B = B*A.-1 if flags&RIGHT
// alpha*B = B*A.-T if flags&(RIGHT|TRANS)
//
// Matrix A is N*N triangular matrix defined with flags bits as follow
// LOWER non-unit lower triangular
// LOWER|UNIT unit lower triangular
// UPPER non-unit upper triangular
// UPPER|UNIT unit upper triangular
//
// Matrix B is N*P if flags&LEFT or P*N if flags&RIGHT.
//
func SolveTrm(B, A *matrix.FloatMatrix, alpha float64, flags Flags) error {
ok := true
empty := false
br, bc := B.Size()
ar, ac := A.Size()
switch flags & (LEFT | RIGHT) {
case LEFT:
empty = br == 0
ok = br == ac && ac == ar
case RIGHT:
empty = bc == 0
ok = bc == ar && ac == ar
}
if empty {
return nil
}
if !ok {
return onError("A, B size mismatch")
}
Ar := A.FloatArray()
ldA := A.LeadingIndex()
Br := B.FloatArray()
ldB := B.LeadingIndex()
E := bc
if flags&RIGHT != 0 {
E = br
}
// if more workers available can divide to tasks by B columns if flags&LEFT or by
// B rows if flags&RIGHT.
calgo.DSolveBlk(Br, Ar, alpha, calgo.Flags(flags), ldB, ldA, ac, 0, E, nB)
return nil
}
// Generic matrix-matrix multpily. (blas.GEMM). Calculates
// C = beta*C + alpha*A*B (default)
// C = beta*C + alpha*A.T*B flags&TRANSA
// C = beta*C + alpha*A*B.T flags&TRANSB
// C = beta*C + alpha*A.T*B.T flags&(TRANSA|TRANSB)
//
// C is M*N, A is M*P or P*M if flags&TRANSA. B is P*N or N*P if flags&TRANSB.
//
func Mult(C, A, B *matrix.FloatMatrix, alpha, beta float64, flags Flags) error {
var ok, empty bool
// error checking must take in account flag values!
ar, ac := A.Size()
br, bc := B.Size()
cr, cc := C.Size()
switch flags & (TRANSA | TRANSB) {
case TRANSA | TRANSB:
empty = ac == 0 || br == 0
ok = cr == ac && cc == br && ar == bc
case TRANSA:
empty = ac == 0 || bc == 0
ok = cr == ac && cc == bc && ar == br
case TRANSB:
empty = ar == 0 || br == 0
ok = cr == ar && cc == br && ac == bc
default:
empty = ar == 0 || bc == 0
ok = cr == ar && cc == bc && ac == br
}
if empty {
return nil
}
if !ok {
return errors.New("Mult: size mismatch")
}
psize := int64(C.NumElements()) * int64(A.Cols())
Ar := A.FloatArray()
ldA := A.LeadingIndex()
Br := B.FloatArray()
ldB := B.LeadingIndex()
Cr := C.FloatArray()
ldC := C.LeadingIndex()
// matrix A, B common dimension
P := A.Cols()
if flags&TRANSA != 0 {
P = A.Rows()
}
if nWorker <= 1 || psize <= limitOne {
calgo.DMult(Cr, Ar, Br, alpha, beta, calgo.Flags(flags), ldC, ldA, ldB, P,
0, C.Cols(), 0, C.Rows(),
vpLen, nB, mB)
return nil
}
// here we have more than one worker available
worker := func(cstart, cend, rstart, rend int, ready chan int) {
calgo.DMult(Cr, Ar, Br, alpha, beta, calgo.Flags(flags), ldC, ldA, ldB, P,
cstart, cend, rstart, rend, vpLen, nB, mB)
ready <- 1
}
colworks, rowworks := divideWork(C.Rows(), C.Cols(), nWorker)
scheduleWork(colworks, rowworks, C.Cols(), C.Rows(), worker)
return nil
}
// Symmetric matrix multiply. (blas.SYMM)
// C = beta*C + alpha*A*B (default)
// C = beta*C + alpha*A.T*B flags&TRANSA
// C = beta*C + alpha*A*B.T flags&TRANSB
// C = beta*C + alpha*A.T*B.T flags&(TRANSA|TRANSB)
//
// C is N*P, A is N*N symmetric matrix. B is N*P or P*N if flags&TRANSB.
//
func MultSym(C, A, B *matrix.FloatMatrix, alpha, beta float64, flags Flags) error {
var ok, empty bool
ar, ac := A.Size()
br, bc := B.Size()
cr, cc := C.Size()
switch flags & (TRANSA | TRANSB) {
case TRANSA | TRANSB:
empty = ac == 0 || br == 0
ok = ar == ac && cr == ac && cc == br && ar == bc
case TRANSA:
empty = ac == 0 || bc == 0
ok = ar == ac && cr == ac && cc == bc && ar == br
case TRANSB:
empty = ar == 0 || br == 0
ok = ar == ac && cr == ar && cc == br && ac == bc
default:
empty = ar == 0 || bc == 0
ok = ar == ac && cr == ar && cc == bc && ac == br
}
if empty {
return nil
}
if !ok {
return errors.New("MultSym: size mismatch")
}
/*
if A.Rows() != A.Cols() {
return errors.New("A matrix not square matrix.");
}
if A.Cols() != B.Rows() {
return errors.New("A.cols != B.rows: size mismatch")
}
*/
psize := int64(C.NumElements()) * int64(A.Cols())
Ar := A.FloatArray()
ldA := A.LeadingIndex()
Br := B.FloatArray()
ldB := B.LeadingIndex()
Cr := C.FloatArray()
ldC := C.LeadingIndex()
if nWorker <= 1 || psize <= limitOne {
calgo.DMultSymm(Cr, Ar, Br, alpha, beta, calgo.Flags(flags), ldC, ldA, ldB,
A.Cols(), 0, C.Cols(), 0, C.Rows(), vpLen, nB, mB)
return nil
}
// here we have more than one worker available
worker := func(cstart, cend, rstart, rend int, ready chan int) {
calgo.DMultSymm(Cr, Ar, Br, alpha, beta, calgo.Flags(flags), ldC, ldA, ldB,
A.Cols(), cstart, cend, rstart, rend, vpLen, nB, mB)
ready <- 1
}
colworks, rowworks := divideWork(C.Rows(), C.Cols(), nWorker)
scheduleWork(colworks, rowworks, C.Cols(), C.Rows(), worker)
return nil
}
// Solves a convex optimization problem with a linear objective
//
// minimize c'*x
// subject to f(x) <= 0
// G*x <= h
// A*x = b.
//
// f is vector valued, convex and twice differentiable. The linear
// inequalities are with respect to a cone C defined as the Cartesian
// product of N + M + 1 cones:
//
// C = C_0 x C_1 x .... x C_N x C_{N+1} x ... x C_{N+M}.
//
// The first cone C_0 is the nonnegative orthant of dimension ml. The
// next N cones are second order cones of dimension r[0], ..., r[N-1].
// The second order cone of dimension m is defined as
//
// { (u0, u1) in R x R^{m-1} | u0 >= ||u1||_2 }.
//
// The next M cones are positive semidefinite cones of order t[0], ..., t[M-1] >= 0.
//
// The structure of C is specified by DimensionSet dims which holds following sets
//
// dims.At("l") l, the dimension of the nonnegative orthant (array of length 1)
// dims.At("q") r[0], ... r[N-1], list with the dimesions of the second-order cones
// dims.At("s") t[0], ... t[M-1], array with the dimensions of the positive
// semidefinite cones
//
// The default value for dims is l: []int{h.Rows()}, q: []int{}, s: []int{}.
//
// On exit Solution contains the result and information about the accurancy of the
// solution. if SolutionStatus is Optimal then Solution.Result contains solutions
// for the problems.
//
// Result.At("x")[0] primal solution
// Result.At("snl")[0] non-linear constraint slacks
// Result.At("sl")[0] linear constraint slacks
// Result.At("y")[0] values for linear equality constraints y
// Result.At("znl")[0] values of dual variables for nonlinear inequalities
// Result.At("zl")[0] values of dual variables for linear inequalities
//
// If err is non-nil then sol is nil and err contains information about the argument or
// computation error.
//
func Cpl(F ConvexProg, c, G, h, A, b *matrix.FloatMatrix, dims *sets.DimensionSet, solopts *SolverOptions) (sol *Solution, err error) {
var mnl int
var x0 *matrix.FloatMatrix
mnl, x0, err = F.F0()
if err != nil {
return
}
if x0.Cols() != 1 {
err = errors.New("'x0' must be matrix with one column")
return
}
if c == nil {
err = errors.New("'c' must be non nil matrix")
return
}
if !c.SizeMatch(x0.Size()) {
err = errors.New(fmt.Sprintf("'c' must be matrix of size (%d,1)", x0.Rows()))
return
}
if h == nil {
h = matrix.FloatZeros(0, 1)
}
if h.Cols() > 1 {
err = errors.New("'h' must be matrix with 1 column")
return
}
if dims == nil {
dims = sets.NewDimensionSet("l", "q", "s")
dims.Set("l", []int{h.Rows()})
}
cdim := dims.Sum("l", "q") + dims.SumSquared("s")
//cdim_pckd := dims.Sum("l", "q") + dims.SumPacked("s")
//cdim_diag := dims.Sum("l", "q", "s")
if h.Rows() != cdim {
err = errors.New(fmt.Sprintf("'h' must be float matrix of size (%d,1)", cdim))
return
}
if G == nil {
G = matrix.FloatZeros(0, c.Rows())
}
if !G.SizeMatch(cdim, c.Rows()) {
estr := fmt.Sprintf("'G' must be of size (%d,%d)", cdim, c.Rows())
err = errors.New(estr)
return
}
// Check A and set defaults if it is nil
if A == nil {
// zeros rows reduces Gemv to vector products
A = matrix.FloatZeros(0, c.Rows())
}
if A.Cols() != c.Rows() {
estr := fmt.Sprintf("'A' must have %d columns", c.Rows())
err = errors.New(estr)
return
}
// Check b and set defaults if it is nil
if b == nil {
b = matrix.FloatZeros(0, 1)
}
if b.Cols() != 1 {
estr := fmt.Sprintf("'b' must be a matrix with 1 column")
err = errors.New(estr)
return
}
if b.Rows() != A.Rows() {
estr := fmt.Sprintf("'b' must have length %d", A.Rows())
err = errors.New(estr)
return
}
var mc = matrixVar{c}
var mb = matrixVar{b}
var mA = matrixVarA{A}
var mG = matrixVarG{G, dims}
solvername := solopts.KKTSolverName
if len(solvername) == 0 {
if len(dims.At("q")) > 0 || len(dims.At("s")) > 0 {
solvername = "chol"
} else {
solvername = "chol2"
}
}
var factor kktFactor
var kktsolver KKTCpSolver = nil
if kktfunc, ok := solvers[solvername]; ok {
// kkt function returns us problem spesific factor function.
factor, err = kktfunc(G, dims, A, mnl)
// solver is
kktsolver = func(W *sets.FloatMatrixSet, x, z *matrix.FloatMatrix) (KKTFunc, error) {
_, Df, H, err := F.F2(x, z)
if err != nil {
return nil, err
}
return factor(W, H, Df)
}
} else {
err = errors.New(fmt.Sprintf("solver '%s' not known", solvername))
return
}
//return CplCustom(F, c, &mG, h, &mA, b, dims, kktsolver, solopts)
return cpl_problem(F, &mc, &mG, h, &mA, &mb, dims, kktsolver, solopts, x0, mnl)
}
// Solves a convex optimization problem with a linear objective
//
// minimize c'*x
// subject to f(x) <= 0
// G*x <= h
// A*x = b.
//
// using custom KTT equation solver and custom constraints G and A.
//
func CplCustomMatrix(F ConvexProg, c *matrix.FloatMatrix, G MatrixG, h *matrix.FloatMatrix,
A MatrixA, b *matrix.FloatMatrix, dims *sets.DimensionSet, kktsolver KKTCpSolver,
solopts *SolverOptions) (sol *Solution, err error) {
var mnl int
var x0 *matrix.FloatMatrix
mnl, x0, err = F.F0()
if err != nil {
return
}
if x0.Cols() != 1 {
err = errors.New("'x0' must be matrix with one column")
return
}
if c == nil {
err = errors.New("'c' must be non nil matrix")
return
}
if !c.SizeMatch(x0.Size()) {
err = errors.New(fmt.Sprintf("'c' must be matrix of size (%d,1)", x0.Rows()))
return
}
if h == nil {
h = matrix.FloatZeros(0, 1)
}
if h.Cols() > 1 {
err = errors.New("'h' must be matrix with 1 column")
return
}
if dims == nil {
dims = sets.NewDimensionSet("l", "q", "s")
dims.Set("l", []int{h.Rows()})
}
cdim := dims.Sum("l", "q") + dims.SumSquared("s")
if h.Rows() != cdim {
err = errors.New(fmt.Sprintf("'h' must be float matrix of size (%d,1)", cdim))
return
}
// Check b and set defaults if it is nil
if b == nil {
b = matrix.FloatZeros(0, 1)
}
if b.Cols() != 1 {
estr := fmt.Sprintf("'b' must be a matrix with 1 column")
err = errors.New(estr)
return
}
mc := matrixVar{c}
mb := matrixVar{b}
var mG MatrixVarG
var mA MatrixVarA
if G == nil {
mG = &matrixVarG{matrix.FloatZeros(0, c.Rows()), dims}
} else {
mG = &matrixIfG{G}
}
if A == nil {
mA = &matrixVarA{matrix.FloatZeros(0, c.Rows())}
} else {
mA = &matrixIfA{A}
}
return cpl_problem(F, &mc, mG, h, mA, &mb, dims, kktsolver, solopts, x0, mnl)
}
// Solves a convex optimization problem with a linear objective
//
// minimize c'*x
// subject to f(x) <= 0
// G*x <= h
// A*x = b.
//
// using custom KTT equation solver.
//
func CplCustomKKT(F ConvexProg, c *matrix.FloatMatrix, G, h, A, b *matrix.FloatMatrix,
dims *sets.DimensionSet, kktsolver KKTCpSolver,
solopts *SolverOptions) (sol *Solution, err error) {
var mnl int
var x0 *matrix.FloatMatrix
mnl, x0, err = F.F0()
if err != nil {
return
}
if x0.Cols() != 1 {
err = errors.New("'x0' must be matrix with one column")
return
}
if c == nil {
err = errors.New("'c' must be non nil matrix")
return
}
if !c.SizeMatch(x0.Size()) {
err = errors.New(fmt.Sprintf("'c' must be matrix of size (%d,1)", x0.Rows()))
return
}
if h == nil {
h = matrix.FloatZeros(0, 1)
}
if h.Cols() > 1 {
err = errors.New("'h' must be matrix with 1 column")
return
}
if dims == nil {
dims = sets.NewDimensionSet("l", "q", "s")
dims.Set("l", []int{h.Rows()})
}
cdim := dims.Sum("l", "q") + dims.SumSquared("s")
if h.Rows() != cdim {
err = errors.New(fmt.Sprintf("'h' must be float matrix of size (%d,1)", cdim))
return
}
if G == nil {
G = matrix.FloatZeros(0, c.Rows())
}
if !G.SizeMatch(cdim, c.Rows()) {
estr := fmt.Sprintf("'G' must be of size (%d,%d)", cdim, c.Rows())
err = errors.New(estr)
return
}
// Check A and set defaults if it is nil
if A == nil {
// zeros rows reduces Gemv to vector products
A = matrix.FloatZeros(0, c.Rows())
}
if A.Cols() != c.Rows() {
estr := fmt.Sprintf("'A' must have %d columns", c.Rows())
err = errors.New(estr)
return
}
// Check b and set defaults if it is nil
if b == nil {
b = matrix.FloatZeros(0, 1)
}
if b.Cols() != 1 {
estr := fmt.Sprintf("'b' must be a matrix with 1 column")
err = errors.New(estr)
return
}
if b.Rows() != A.Rows() {
estr := fmt.Sprintf("'b' must have length %d", A.Rows())
err = errors.New(estr)
return
}
var mc = matrixVar{c}
var mb = matrixVar{b}
var mA = matrixVarA{A}
var mG = matrixVarG{G, dims}
return cpl_problem(F, &mc, &mG, h, &mA, &mb, dims, kktsolver, solopts, x0, mnl)
}
func blockedQRT(A, T, W *matrix.FloatMatrix, nb int) {
var ATL, ATR, ABL, ABR, AL, AR matrix.FloatMatrix
var A00, A01, A02, A10, A11, A12, A20, A21, A22 matrix.FloatMatrix
var WT, WB, W0, W1, W2 matrix.FloatMatrix
var TTL, TTR, TBL, TBR matrix.FloatMatrix
var T00, T01, T02, T11, T12, T22 matrix.FloatMatrix
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pTOPLEFT)
partition2x2(
&TTL, &TTR,
&TBL, &TBR, T, 0, 0, pTOPLEFT)
partition2x1(
&WT,
&WB, W, 0, pTOP)
for ABR.Rows() > 0 && ABR.Cols() > 0 {
repartition2x2to3x3(&ATL,
&A00, &A01, &A02,
&A10, &A11, &A12,
&A20, &A21, &A22, A, nb, pBOTTOMRIGHT)
repartition2x2to3x3(&TTL,
&T00, &T01, &T02,
nil, &T11, &T12,
nil, nil, &T22, T, nb, pBOTTOMRIGHT)
repartition2x1to3x1(&WT,
&W0,
&W1,
&W2, W, nb, pBOTTOM)
partition1x2(
&AL, &AR, &ABR, nb, pLEFT)
// current block size
cb, rb := A11.Size()
if rb < cb {
cb = rb
}
// --------------------------------------------------------
// decompose left side AL == /A11\
// \A21/
unblockedQRT(&AL, &T11)
// update A'tail i.e. A12 and A22 with (I - Y*T*Y.T).T * A'tail
// compute: Q*T.C == C - Y*(C.T*Y*T).T
updateWithQT(&A12, &A22, &A11, &A21, &T11, &W2, cb, true)
// update T01: T01 = -T00*Y1.T*Y2*T11
// Y1 = /A10\ Y2 = /A11\
// \A20/ \A21/
//
updateQRTReflector(&T01, &A10, &A20, &A11, &A21, &T00, &T11)
// --------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &A11, &A22, A, pBOTTOMRIGHT)
continue3x3to2x2(
&TTL, &TTR,
&TBL, &TBR, &T00, &T11, &T22, T, pBOTTOMRIGHT)
continue3x1to2x1(
&WT,
&WB, &W0, &W1, W, pBOTTOM)
}
}
/*
* Blocked QR decomposition with compact WY transform. As implemented
* in lapack.xGEQRF subroutine.
*/
func blockedQR(A, Tvec, Twork, W *matrix.FloatMatrix, nb int) {
var ATL, ATR, ABL, ABR, AL, AR matrix.FloatMatrix
var A00, A01, A02, A10, A11, A12, A20, A21, A22 matrix.FloatMatrix
var TT, TB matrix.FloatMatrix
var t0, tau, t2, Tdiag, WT, WB, W0, W1, W2 matrix.FloatMatrix
//var Twork, W *matrix.FloatMatrix
Tdiag.DiagOf(Twork)
partition2x2(
&ATL, &ATR,
&ABL, &ABR, A, 0, 0, pTOPLEFT)
partition2x1(
&TT,
&TB, Tvec, 0, pTOP)
partition2x1(
&WT,
&WB, W, 0, pTOP)
for ABR.Rows() > 0 && ABR.Cols() > 0 {
repartition2x2to3x3(&ATL,
&A00, &A01, &A02,
&A10, &A11, &A12,
&A20, &A21, &A22, A, nb, pBOTTOMRIGHT)
repartition2x1to3x1(&TT,
&t0,
&tau,
&t2, Tvec, nb, pBOTTOM)
repartition2x1to3x1(&WT,
&W0,
&W1,
&W2, W, nb, pBOTTOM)
partition1x2(
&AL, &AR, &ABR, nb, pLEFT)
// current block size
cb, rb := A11.Size()
if rb < cb {
cb = rb
}
// --------------------------------------------------------
// decompose left side AL == /A11\
// \A21/
unblockedQRT(&AL, Twork)
// copy scaling from T diagonal to tau-vector
Tdiag.CopyTo(&tau)
// update A'tail i.e. A12 and A22 with (I - Y*T*Y.T).T * A'tail
// compute: C - Y*(C.T*Y*T).T
updateWithQT(&A12, &A22, &A11, &A21, Twork, &W2, cb, true)
// --------------------------------------------------------
continue3x3to2x2(
&ATL, &ATR,
&ABL, &ABR, &A00, &A11, &A22, A, pBOTTOMRIGHT)
continue3x1to2x1(
&TT,
&TB, &t0, &tau, Tvec, pBOTTOM)
continue3x1to2x1(
&WT,
&WB, &W0, &W1, W, pBOTTOM)
}
}
// Computes analytic center of A*x <= b with A m by n of rank n.
// We assume that b > 0 and the feasible set is bounded.
func acent(A, b *matrix.FloatMatrix, niters int) (x *matrix.FloatMatrix, ntdecrs []float64, err error) {
err = nil
if niters <= 0 {
niters = MAXITERS
}
ntdecrs = make([]float64, 0, niters)
if A.Rows() != b.Rows() {
return nil, nil, errors.New("A.Rows() != b.Rows()")
}
m, n := A.Size()
x = matrix.FloatZeros(n, 1)
H := matrix.FloatZeros(n, n)
// Helper m*n matrix
Dmn := matrix.FloatZeros(m, n)
for i := 0; i < niters; i++ {
// Gradient is g = A^T * (1.0/(b - A*x)). d = 1.0/(b - A*x)
// d is m*1 matrix, g is n*1 matrix
d := matrix.Minus(b, matrix.Times(A, x)).Inv()
g := matrix.Times(A.Transpose(), d)
// Hessian is H = A^T * diag(1./(b-A*x))^2 * A.
// in the original python code expression d[:,n*[0]] creates
// a m*n matrix where each column is copy of column 0.
// We do it here manually.
for i := 0; i < n; i++ {
Dmn.SetColumn(i, d)
}
// Function mul creates element wise product of matrices.
Asc := matrix.Mul(Dmn, A)
blas.SyrkFloat(Asc, H, 1.0, 0.0, linalg.OptTrans)
// Newton step is v = H^-1 * g.
v := matrix.Scale(g, -1.0)
PosvFloat(H, v)
// Directional derivative and Newton decrement.
lam := blas.DotFloat(g, v)
ntdecrs = append(ntdecrs, math.Sqrt(-lam))
if ntdecrs[len(ntdecrs)-1] < TOL {
return x, ntdecrs, err
}
// Backtracking line search.
// y = d .* A*v
y := matrix.Mul(d, matrix.Times(A, v))
step := 1.0
for 1-step*y.Max() < 0 {
step *= BETA
}
search:
for {
// t = -step*y + 1 [e.g. t = 1 - step*y]
t := matrix.Scale(y, -step).Add(1.0)
// ts = sum(log(1-step*y))
ts := t.Log().Sum()
if -ts < ALPHA*step*lam {
break search
}
step *= BETA
}
v.Scale(step)
x.Plus(v)
}
// no solution !!
err = errors.New(fmt.Sprintf("Iteration %d exhausted\n", niters))
return x, ntdecrs, err
}
// Solution of KKT equations by a dense LDL factorization of the
// 3 x 3 system.
//
// Returns a function that (1) computes the LDL factorization of
//
// [ H A' GG'*W^{-1} ]
// [ A 0 0 ],
// [ W^{-T}*GG 0 -I ]
//
// given H, Df, W, where GG = [Df; G], and (2) returns a function for
// solving
//
// [ H A' GG' ] [ ux ] [ bx ]
// [ A 0 0 ] * [ uy ] = [ by ].
// [ GG 0 -W'*W ] [ uz ] [ bz ]
//
// H is n x n, A is p x n, Df is mnl x n, G is N x n where
// N = dims['l'] + sum(dims['q']) + sum( k**2 for k in dims['s'] ).
//
func kktLdl(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {
p, n := A.Size()
ldK := n + p + mnl + dims.At("l")[0] + dims.Sum("q") + dims.SumPacked("s")
K := matrix.FloatZeros(ldK, ldK)
ipiv := make([]int32, ldK)
u := matrix.FloatZeros(ldK, 1)
g := matrix.FloatZeros(mnl+G.Rows(), 1)
//checkpnt.AddMatrixVar("u", u)
//checkpnt.AddMatrixVar("K", K)
factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
var err error = nil
// Zero K for each call.
blas.ScalFloat(K, 0.0)
if H != nil {
K.SetSubMatrix(0, 0, H)
}
K.SetSubMatrix(n, 0, A)
for k := 0; k < n; k++ {
// g is (mnl + G.Rows(), 1) matrix, Df is (mnl, n), G is (N, n)
if mnl > 0 {
// set values g[0:mnl] = Df[,k]
g.SetIndexesFromArray(Df.GetColumnArray(k, nil), matrix.MakeIndexSet(0, mnl, 1)...)
}
// set values g[mnl:] = G[,k]
g.SetIndexesFromArray(G.GetColumnArray(k, nil), matrix.MakeIndexSet(mnl, mnl+g.Rows(), 1)...)
scale(g, W, true, true)
if err != nil {
//fmt.Printf("scale error: %s\n", err)
}
pack(g, K, dims, &la.IOpt{"mnl", mnl}, &la.IOpt{"offsety", k*ldK + n + p})
}
setDiagonal(K, n+p, n+n, ldK, ldK, -1.0)
err = lapack.Sytrf(K, ipiv)
if err != nil {
return nil, err
}
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.
//
// On entry, x, y, z contain bx, by, bz. On exit, they contain
// the solution ux, uy, W*uz.
err = nil
blas.Copy(x, u)
blas.Copy(y, u, &la.IOpt{"offsety", n})
err = scale(z, W, true, true)
if err != nil {
return
}
err = pack(z, u, dims, &la.IOpt{"mnl", mnl}, &la.IOpt{"offsety", n + p})
if err != nil {
return
}
err = lapack.Sytrs(K, u, ipiv)
if err != nil {
return
}
blas.Copy(u, x, &la.IOpt{"n", n})
blas.Copy(u, y, &la.IOpt{"n", p}, &la.IOpt{"offsetx", n})
err = unpack(u, z, dims, &la.IOpt{"mnl", mnl}, &la.IOpt{"offsetx", n + p})
return
}
return solve, err
}
return factor, nil
}
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
}
// Solution of KKT equations by reduction to a 2 x 2 system, a QR
// factorization to eliminate the equality constraints, and a dense
// Cholesky factorization of order n-p.
//
// Computes the QR factorization
//
// A' = [Q1, Q2] * [R; 0]
//
// and returns a function that (1) computes the Cholesky factorization
//
// Q_2^T * (H + GG^T * W^{-1} * W^{-T} * GG) * Q2 = L * L^T,
//
// given H, Df, W, where GG = [Df; G], and (2) returns a function for
// solving
//
// [ H A' GG' ] [ ux ] [ bx ]
// [ A 0 0 ] * [ uy ] = [ by ].
// [ GG 0 -W'*W ] [ uz ] [ bz ]
//
// H is n x n, A is p x n, Df is mnl x n, G is N x n where
// N = dims['l'] + sum(dims['q']) + sum( k**2 for k in dims['s'] ).
//
func kktChol(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {
p, n := A.Size()
cdim := mnl + dims.Sum("l", "q") + dims.SumSquared("s")
cdim_pckd := mnl + dims.Sum("l", "q") + dims.SumPacked("s")
QA := A.Transpose()
tauA := matrix.FloatZeros(p, 1)
lapack.Geqrf(QA, tauA)
Gs := matrix.FloatZeros(cdim, n)
K := matrix.FloatZeros(n, n)
bzp := matrix.FloatZeros(cdim_pckd, 1)
yy := matrix.FloatZeros(p, 1)
checkpnt.AddMatrixVar("tauA", tauA)
checkpnt.AddMatrixVar("Gs", Gs)
checkpnt.AddMatrixVar("K", K)
factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
// Compute
//
// K = [Q1, Q2]' * (H + GG' * W^{-1} * W^{-T} * GG) * [Q1, Q2]
//
// and take the Cholesky factorization of the 2,2 block
//
// Q_2' * (H + GG^T * W^{-1} * W^{-T} * GG) * Q2.
var err error = nil
minor := 0
if !checkpnt.MinorEmpty() {
minor = checkpnt.MinorTop()
}
// Gs = W^{-T} * GG in packed storage.
if mnl > 0 {
Gs.SetSubMatrix(0, 0, Df)
}
Gs.SetSubMatrix(mnl, 0, G)
checkpnt.Check("00factor_chol", minor)
scale(Gs, W, true, true)
pack2(Gs, dims, mnl)
//checkpnt.Check("10factor_chol", minor)
// K = [Q1, Q2]' * (H + Gs' * Gs) * [Q1, Q2].
blas.SyrkFloat(Gs, K, 1.0, 0.0, la.OptTrans, &la.IOpt{"k", cdim_pckd})
if H != nil {
K.SetSubMatrix(0, 0, matrix.Plus(H, K.GetSubMatrix(0, 0, H.Rows(), H.Cols())))
}
//checkpnt.Check("20factor_chol", minor)
symm(K, n, 0)
lapack.Ormqr(QA, tauA, K, la.OptLeft, la.OptTrans)
lapack.Ormqr(QA, tauA, K, la.OptRight)
//checkpnt.Check("30factor_chol", minor)
// Cholesky factorization of 2,2 block of K.
lapack.Potrf(K, &la.IOpt{"n", n - p}, &la.IOpt{"offseta", p * (n + 1)})
checkpnt.Check("40factor_chol", minor)
solve := func(x, y, z *matrix.FloatMatrix) (err error) {
// Solve
//
// [ 0 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.
//
// On entry, x, y, z contain bx, by, bz. On exit, they contain
// the solution ux, uy, W*uz.
//
// If we change variables ux = Q1*v + Q2*w, the system becomes
//
// [ K11 K12 R ] [ v ] [Q1'*(bx+GG'*W^{-1}*W^{-T}*bz)]
// [ K21 K22 0 ] * [ w ] = [Q2'*(bx+GG'*W^{-1}*W^{-T}*bz)]
// [ R^T 0 0 ] [ uy ] [by ]
//
// W*uz = W^{-T} * ( GG*ux - bz ).
minor := 0
if !checkpnt.MinorEmpty() {
minor = checkpnt.MinorTop()
}
// bzp := W^{-T} * bz in packed storage
scale(z, W, true, true)
pack(z, bzp, dims, &la.IOpt{"mnl", mnl})
// x := [Q1, Q2]' * (x + Gs' * bzp)
// = [Q1, Q2]' * (bx + Gs' * W^{-T} * bz)
blas.GemvFloat(Gs, bzp, x, 1.0, 1.0, la.OptTrans, &la.IOpt{"m", cdim_pckd})
lapack.Ormqr(QA, tauA, x, la.OptLeft, la.OptTrans)
// y := x[:p]
// = Q1' * (bx + Gs' * W^{-T} * bz)
blas.Copy(y, yy)
blas.Copy(x, y, &la.IOpt{"n", p})
// x[:p] := v = R^{-T} * by
blas.Copy(yy, x)
lapack.Trtrs(QA, x, la.OptUpper, la.OptTrans, &la.IOpt{"n", p})
// x[p:] := K22^{-1} * (x[p:] - K21*x[:p])
// = K22^{-1} * (Q2' * (bx + Gs' * W^{-T} * bz) - K21*v)
blas.GemvFloat(K, x, x, -1.0, 1.0, &la.IOpt{"m", n - p}, &la.IOpt{"n", p},
&la.IOpt{"offseta", p}, &la.IOpt{"offsety", p})
lapack.Potrs(K, x, &la.IOpt{"n", n - p}, &la.IOpt{"offseta", p * (n + 1)},
&la.IOpt{"offsetb", p})
// y := y - [K11, K12] * x
// = Q1' * (bx + Gs' * W^{-T} * bz) - K11*v - K12*w
blas.GemvFloat(K, x, y, -1.0, 1.0, &la.IOpt{"m", p}, &la.IOpt{"n", n})
// y := R^{-1}*y
// = R^{-1} * (Q1' * (bx + Gs' * W^{-T} * bz) - K11*v
// - K12*w)
lapack.Trtrs(QA, y, la.OptUpper, &la.IOpt{"n", p})
// x := [Q1, Q2] * x
lapack.Ormqr(QA, tauA, x, la.OptLeft)
// bzp := Gs * x - bzp.
// = W^{-T} * ( GG*ux - bz ) in packed storage.
// Unpack and copy to z.
blas.GemvFloat(Gs, x, bzp, 1.0, -1.0, &la.IOpt{"m", cdim_pckd})
unpack(bzp, z, dims, &la.IOpt{"mnl", mnl})
checkpnt.Check("90solve_chol", minor)
return nil
}
return solve, err
}
return factor, nil
}
// Solution of KKT equations with zero 1,1 block, by eliminating the
// equality constraints via a QR factorization, and solving the
// reduced KKT system by another QR factorization.
//
// Computes the QR factorization
//
// A' = [Q1, Q2] * [R1; 0]
//
// and returns a function that (1) computes the QR factorization
//
// W^{-T} * G * Q2 = Q3 * R3
//
// (with columns of W^{-T}*G in packed storage), and (2) returns a function for solving
//
// [ 0 A' G' ] [ ux ] [ bx ]
// [ A 0 0 ] * [ uy ] = [ by ].
// [ G 0 -W'*W ] [ uz ] [ bz ]
//
// A is p x n and G is N x n where N = dims['l'] + sum(dims['q']) +
// sum( k**2 for k in dims['s'] ).
//
func kktQr(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {
p, n := A.Size()
cdim := dims.Sum("l", "q") + dims.SumSquared("s")
cdim_pckd := dims.Sum("l", "q") + dims.SumPacked("s")
QA := A.Transpose()
tauA := matrix.FloatZeros(p, 1)
lapack.Geqrf(QA, tauA)
Gs := matrix.FloatZeros(cdim, n)
tauG := matrix.FloatZeros(n-p, 1)
u := matrix.FloatZeros(cdim_pckd, 1)
vv := matrix.FloatZeros(n, 1)
w := matrix.FloatZeros(cdim_pckd, 1)
checkpnt.AddMatrixVar("tauA", tauA)
checkpnt.AddMatrixVar("tauG", tauG)
checkpnt.AddMatrixVar("Gs", Gs)
checkpnt.AddMatrixVar("qr_u", u)
checkpnt.AddMatrixVar("qr_vv", vv)
factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
var err error = nil
minor := 0
if !checkpnt.MinorEmpty() {
minor = checkpnt.MinorTop()
}
// Gs = W^{-T}*G, in packed storage.
blas.Copy(G, Gs)
//checkpnt.Check("00factor_qr", minor)
scale(Gs, W, true, true)
//checkpnt.Check("01factor_qr", minor)
pack2(Gs, dims, 0)
//checkpnt.Check("02factor_qr", minor)
// Gs := [ Gs1, Gs2 ]
// = Gs * [ Q1, Q2 ]
lapack.Ormqr(QA, tauA, Gs, la.OptRight, &la.IOpt{"m", cdim_pckd})
//checkpnt.Check("03factor_qr", minor)
// QR factorization Gs2 := [ Q3, Q4 ] * [ R3; 0 ]
lapack.Geqrf(Gs, tauG, &la.IOpt{"n", n - p}, &la.IOpt{"m", cdim_pckd},
&la.IOpt{"offseta", Gs.Rows() * p})
checkpnt.Check("10factor_qr", minor)
solve := func(x, y, z *matrix.FloatMatrix) (err error) {
// On entry, x, y, z contain bx, by, bz. On exit, they
// contain the solution x, y, W*z of
//
// [ 0 A' G'*W^{-1} ] [ x ] [bx ]
// [ A 0 0 ] * [ y ] = [by ].
// [ W^{-T}*G 0 -I ] [ W*z ] [W^{-T}*bz]
//
// The system is solved in five steps:
//
// w := W^{-T}*bz - Gs1*R1^{-T}*by
// u := R3^{-T}*Q2'*bx + Q3'*w
// W*z := Q3*u - w
// y := R1^{-1} * (Q1'*bx - Gs1'*(W*z))
// x := [ Q1, Q2 ] * [ R1^{-T}*by; R3^{-1}*u ]
minor := 0
if !checkpnt.MinorEmpty() {
minor = checkpnt.MinorTop()
}
// w := W^{-T} * bz in packed storage
scale(z, W, true, true)
pack(z, w, dims)
//checkpnt.Check("00solve_qr", minor)
// vv := [ Q1'*bx; R3^{-T}*Q2'*bx ]
blas.Copy(x, vv)
lapack.Ormqr(QA, tauA, vv, la.OptTrans)
lapack.Trtrs(Gs, vv, la.OptUpper, la.OptTrans, &la.IOpt{"n", n - p},
&la.IOpt{"offseta", Gs.Rows() * p}, &la.IOpt{"offsetb", p})
//checkpnt.Check("10solve_qr", minor)
// x[:p] := R1^{-T} * by
blas.Copy(y, x)
lapack.Trtrs(QA, x, la.OptUpper, la.OptTrans, &la.IOpt{"n", p})
//checkpnt.Check("20solve_qr", minor)
// w := w - Gs1 * x[:p]
// = W^{-T}*bz - Gs1*by
blas.GemvFloat(Gs, x, w, -1.0, 1.0, &la.IOpt{"n", p}, &la.IOpt{"m", cdim_pckd})
//checkpnt.Check("30solve_qr", minor)
// u := [ Q3'*w + v[p:]; 0 ]
// = [ Q3'*w + R3^{-T}*Q2'*bx; 0 ]
blas.Copy(w, u)
lapack.Ormqr(Gs, tauG, u, la.OptTrans, &la.IOpt{"k", n - p},
&la.IOpt{"offseta", Gs.Rows() * p}, &la.IOpt{"m", cdim_pckd})
blas.AxpyFloat(vv, u, 1.0, &la.IOpt{"offsetx", p}, &la.IOpt{"n", n - p})
blas.ScalFloat(u, 0.0, &la.IOpt{"offset", n - p})
//checkpnt.Check("40solve_qr", minor)
// x[p:] := R3^{-1} * u[:n-p]
blas.Copy(u, x, &la.IOpt{"offsety", p}, &la.IOpt{"n", n - p})
lapack.Trtrs(Gs, x, la.OptUpper, &la.IOpt{"n", n - p},
&la.IOpt{"offset", Gs.Rows() * p}, &la.IOpt{"offsetb", p})
//checkpnt.Check("50solve_qr", minor)
// x is now [ R1^{-T}*by; R3^{-1}*u[:n-p] ]
// x := [Q1 Q2]*x
lapack.Ormqr(QA, tauA, x)
//checkpnt.Check("60solve_qr", minor)
// u := [Q3, Q4] * u - w
// = Q3 * u[:n-p] - w
lapack.Ormqr(Gs, tauG, u, &la.IOpt{"k", n - p}, &la.IOpt{"m", cdim_pckd},
&la.IOpt{"offseta", Gs.Rows() * p})
blas.AxpyFloat(w, u, -1.0)
//checkpnt.Check("70solve_qr", minor)
// y := R1^{-1} * ( v[:p] - Gs1'*u )
// = R1^{-1} * ( Q1'*bx - Gs1'*u )
blas.Copy(vv, y, &la.IOpt{"n", p})
blas.GemvFloat(Gs, u, y, -1.0, 1.0, &la.IOpt{"m", cdim_pckd},
&la.IOpt{"n", p}, la.OptTrans)
lapack.Trtrs(QA, y, la.OptUpper, &la.IOpt{"n", p})
//checkpnt.Check("80solve_qr", minor)
unpack(u, z, dims)
checkpnt.Check("90solve_qr", minor)
return nil
}
return solve, err
}
return factor, nil
}