예제 #1
0
파일: misc.go 프로젝트: sguzwf/algorithm
func sinv(x, y *matrix.FloatMatrix, dims *sets.DimensionSet, mnl int) (err error) {
	/*DEBUGGED*/

	err = nil

	// For the nonlinear and 'l' blocks:
	//
	//     yk o\ xk = yk .\ xk.

	ind := mnl + dims.At("l")[0]
	blas.Tbsv(y, x, &la_.IOpt{"n", ind}, &la_.IOpt{"k", 0}, &la_.IOpt{"ldA", 1})

	// For the 'q' blocks:
	//
	//                        [ l0   -l1'              ]
	//     yk o\ xk = 1/a^2 * [                        ] * xk
	//                        [ -l1  (a*I + l1*l1')/l0 ]
	//
	// where yk = (l0, l1) and a = l0^2 - l1'*l1.

	for _, m := range dims.At("q") {
		aa := blas.Nrm2Float(y, &la_.IOpt{"n", m - 1}, &la_.IOpt{"offset", ind + 1})
		ee := y.GetIndex(ind)
		aa = (ee + aa) * (ee - aa)
		cc := x.GetIndex(ind)
		dd := blas.DotFloat(x, y, &la_.IOpt{"n", m - 1}, &la_.IOpt{"offsetx", ind + 1},
			&la_.IOpt{"offsety", ind + 1})
		x.SetIndex(ind, cc*ee-dd)
		blas.ScalFloat(x, aa/ee, &la_.IOpt{"n", m - 1}, &la_.IOpt{"offset", ind + 1})
		blas.AxpyFloat(y, x, dd/ee-cc, &la_.IOpt{"n", m - 1},
			&la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", ind + 1})
		blas.ScalFloat(x, 1.0/aa, &la_.IOpt{"n", m}, &la_.IOpt{"offset", ind})
		ind += m
	}

	// For the 's' blocks:
	//
	//     yk o\ xk =  xk ./ gamma
	//
	// where gammaij = .5 * (yk_i + yk_j).

	ind2 := ind
	for _, m := range dims.At("s") {
		for j := 0; j < m; j++ {
			u := matrix.FloatVector(y.FloatArray()[ind2+j : ind2+m])
			u.Add(y.GetIndex(ind2 + j))
			u.Scale(0.5)
			blas.Tbsv(u, x, &la_.IOpt{"n", m - j}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1},
				&la_.IOpt{"offsetx", ind + j*(m+1)})
		}
		ind += m * m
		ind2 += m
	}
	return
}
예제 #2
0
func (g *matrixFs) Gf(x, y *matrix.FloatMatrix, alpha, beta float64, trans linalg.Option) error {
	//
	// y := alpha*(-diag(x)) + beta*y
	//
	if linalg.Equal(trans, linalg.OptNoTrans) {
		blas.ScalFloat(y, beta)
		blas.AxpyFloat(x, y, -alpha, &linalg.IOpt{"incy", g.n + 1})
	} else {
		blas.ScalFloat(y, beta)
		blas.AxpyFloat(x, y, -alpha, &linalg.IOpt{"incx", g.n + 1})
	}
	return nil
}
예제 #3
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   Sets upper triangular part of the 's' components of x equal to zero
   and scales the strictly lower triangular part by 2.0.
*/
func trisc(x *matrix.FloatMatrix, dims *sets.DimensionSet, offset int) error {

	//m := dims.Sum("l", "q") + dims.SumSquared("s")
	ind := offset + dims.Sum("l", "q")

	for _, mk := range dims.At("s") {
		for j := 1; j < mk; j++ {
			blas.ScalFloat(x, 0.0, la_.IntOpt("n", mk-j), la_.IntOpt("inc", mk),
				la_.IntOpt("offset", ind+j*(mk+1)-1))
			blas.ScalFloat(x, 2.0, la_.IntOpt("n", mk-j), la_.IntOpt("offset", ind+mk*(j-1)+j))
		}
		ind += mk * mk
	}
	return nil
}
예제 #4
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   Copy x to y using packed storage.

   The vector x is an element of S, with the 's' components stored in
   unpacked storage.  On return, x is copied to y with the 's' components
   stored in packed storage and the off-diagonal entries scaled by
   sqrt(2).
*/
func pack(x, y *matrix.FloatMatrix, dims *sets.DimensionSet, opts ...la_.Option) (err error) {
	/*DEBUGGED*/
	err = nil
	mnl := la_.GetIntOpt("mnl", 0, opts...)
	offsetx := la_.GetIntOpt("offsetx", 0, opts...)
	offsety := la_.GetIntOpt("offsety", 0, opts...)

	nlq := mnl + dims.At("l")[0] + dims.Sum("q")
	blas.Copy(x, y, &la_.IOpt{"n", nlq}, &la_.IOpt{"offsetx", offsetx},
		&la_.IOpt{"offsety", offsety})

	iu, ip := offsetx+nlq, offsety+nlq
	for _, n := range dims.At("s") {
		for k := 0; k < n; k++ {
			blas.Copy(x, y, &la_.IOpt{"n", n - k}, &la_.IOpt{"offsetx", iu + k*(n+1)},
				&la_.IOpt{"offsety", ip})
			y.SetIndex(ip, (y.GetIndex(ip) / math.Sqrt(2.0)))
			ip += n - k
		}
		iu += n * n
	}
	np := dims.SumPacked("s")
	blas.ScalFloat(y, math.Sqrt(2.0), &la_.IOpt{"n", np}, &la_.IOpt{"offset", offsety + nlq})
	return
}
예제 #5
0
파일: gp.go 프로젝트: sguzwf/algorithm
func (gp *gpConvexProg) F1(x *matrix.FloatMatrix) (f, Df *matrix.FloatMatrix, err error) {
    f = nil
    Df = nil
    err = nil
    f = matrix.FloatZeros(gp.mnl+1, 1)
    Df = matrix.FloatZeros(gp.mnl+1, gp.n)
    y := gp.g.Copy()
    blas.GemvFloat(gp.F, x, y, 1.0, 1.0)

    for i, s := range gp.ind {
        start := s[0]
        stop := s[1]
        // yi := exp(yi) = exp(Fi*x+gi)
        ymax := maxvec(y.FloatArray()[start:stop])
        // ynew = exp(y[start:stop] - ymax)
        ynew := matrix.Exp(matrix.FloatVector(y.FloatArray()[start:stop]).Add(-ymax))
        y.SetIndexesFromArray(ynew.FloatArray(), matrix.Indexes(start, stop)...)

        // fi = log sum yi = log sum exp(Fi*x+gi)
        ysum := blas.AsumFloat(y, &la.IOpt{"n", stop - start}, &la.IOpt{"offset", start})
        f.SetIndex(i, ymax+math.Log(ysum))

        blas.ScalFloat(y, 1.0/ysum, &la.IOpt{"n", stop - start}, &la.IOpt{"offset", start})
        blas.GemvFloat(gp.F, y, Df, 1.0, 0.0, la.OptTrans, &la.IOpt{"m", stop - start},
            &la.IOpt{"incy", gp.mnl + 1}, &la.IOpt{"offseta", start},
            &la.IOpt{"offsetx", start}, &la.IOpt{"offsety", i})
    }
    return
}
예제 #6
0
파일: gp.go 프로젝트: sguzwf/algorithm
func (gp *gpConvexProg) F2(x, z *matrix.FloatMatrix) (f, Df, H *matrix.FloatMatrix, err error) {

    err = nil
    f = matrix.FloatZeros(gp.mnl+1, 1)
    Df = matrix.FloatZeros(gp.mnl+1, gp.n)
    H = matrix.FloatZeros(gp.n, gp.n)
    y := gp.g.Copy()
    Fsc := matrix.FloatZeros(gp.maxK, gp.n)
    blas.GemvFloat(gp.F, x, y, 1.0, 1.0)
    //fmt.Printf("y=\n%v\n", y.ToString("%.3f"))

    for i, s := range gp.ind {
        start := s[0]
        stop := s[1]

        // yi := exp(yi) = exp(Fi*x+gi)
        ymax := maxvec(y.FloatArray()[start:stop])
        ynew := matrix.Exp(matrix.FloatVector(y.FloatArray()[start:stop]).Add(-ymax))
        y.SetIndexesFromArray(ynew.FloatArray(), matrix.Indexes(start, stop)...)

        // fi = log sum yi = log sum exp(Fi*x+gi)
        ysum := blas.AsumFloat(y, &la.IOpt{"n", stop - start}, &la.IOpt{"offset", start})

        f.SetIndex(i, ymax+math.Log(ysum))
        blas.ScalFloat(y, 1.0/ysum, &la.IOpt{"n", stop - start}, &la.IOpt{"offset", start})
        blas.GemvFloat(gp.F, y, Df, 1.0, 0.0, la.OptTrans, &la.IOpt{"m", stop - start},
            &la.IOpt{"incy", gp.mnl + 1}, &la.IOpt{"offseta", start},
            &la.IOpt{"offsetx", start}, &la.IOpt{"offsety", i})

        Fsc.SetSubMatrix(0, 0, gp.F.GetSubMatrix(start, 0, stop-start))

        for k := start; k < stop; k++ {
            blas.AxpyFloat(Df, Fsc, -1.0, &la.IOpt{"n", gp.n},
                &la.IOpt{"incx", gp.mnl + 1}, &la.IOpt{"incy", Fsc.Rows()},
                &la.IOpt{"offsetx", i}, &la.IOpt{"offsety", k - start})
            blas.ScalFloat(Fsc, math.Sqrt(y.GetIndex(k)),
                &la.IOpt{"inc", Fsc.Rows()}, &la.IOpt{"offset", k - start})
        }
        // H += z[i]*Hi = z[i] *Fisc' * Fisc
        blas.SyrkFloat(Fsc, H, z.GetIndex(i), 1.0, la.OptTrans,
            &la.IOpt{"k", stop - start})
    }
    return
}
예제 #7
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   Scales the strictly lower triangular part of the 's' components of x
   by 0.5.

*/
func triusc(x *matrix.FloatMatrix, dims *sets.DimensionSet, offset int) error {

	//m := dims.Sum("l", "q") + dims.SumSquared("s")
	ind := offset + dims.Sum("l", "q")

	for _, mk := range dims.At("s") {
		for j := 1; j < mk; j++ {
			blas.ScalFloat(x, 0.5, &la_.IOpt{"n", mk - j}, &la_.IOpt{"offset", ind + mk*(j-1) + j})
		}
		ind += mk * mk
	}
	return nil
}
예제 #8
0
파일: misc.go 프로젝트: sguzwf/algorithm
// The product x := y o y.   The 's' components of y are diagonal and
// only the diagonals of x and y are stored.
func ssqr(x, y *matrix.FloatMatrix, dims *sets.DimensionSet, mnl int) (err error) {
	/*DEBUGGED*/
	blas.Copy(y, x)
	ind := mnl + dims.At("l")[0]
	err = blas.Tbmv(y, x, &la_.IOpt{"n", ind}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})
	if err != nil {
		return
	}

	for _, m := range dims.At("q") {
		v := blas.Nrm2Float(y, &la_.IOpt{"n", m}, &la_.IOpt{"offset", ind})
		x.SetIndex(ind, v*v)
		blas.ScalFloat(x, 2.0*y.GetIndex(ind), &la_.IOpt{"n", m - 1}, &la_.IOpt{"offset", ind + 1})
		ind += m
	}
	err = blas.Tbmv(y, x, &la_.IOpt{"n", dims.Sum("s")}, &la_.IOpt{"k", 0},
		&la_.IOpt{"lda", 1}, &la_.IOpt{"offseta", ind}, &la_.IOpt{"offsetx", ind})
	return
}
예제 #9
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   The vector x is an element of S, with the 's' components stored
   in unpacked storage and off-diagonal entries scaled by sqrt(2).
   On return, x is copied to y with the 's' components stored in
   unpacked storage.

*/
func unpack(x, y *matrix.FloatMatrix, dims *sets.DimensionSet, opts ...la_.Option) (err error) {
	/*DEBUGGED*/
	err = nil
	mnl := la_.GetIntOpt("mnl", 0, opts...)
	offsetx := la_.GetIntOpt("offsetx", 0, opts...)
	offsety := la_.GetIntOpt("offsety", 0, opts...)

	nlq := mnl + dims.At("l")[0] + dims.Sum("q")
	err = blas.Copy(x, y, &la_.IOpt{"n", nlq}, &la_.IOpt{"offsetx", offsetx},
		&la_.IOpt{"offsety", offsety})
	if err != nil {
		return
	}

	ip, iu := offsetx+nlq, offsety+nlq
	for _, n := range dims.At("s") {
		for k := 0; k < n; k++ {
			err = blas.Copy(x, y, &la_.IOpt{"n", n - k}, &la_.IOpt{"offsetx", ip},
				&la_.IOpt{"offsety", iu + k*(n+1)})
			if err != nil {
				return
			}

			ip += n - k
			blas.ScalFloat(y, 1.0/math.Sqrt(2.0),
				&la_.IOpt{"n", n - k - 1}, &la_.IOpt{"offset", iu + k*(n+1) + 1})
		}
		iu += n * n
	}
	/*
		nu := dims.SumSquared("s")
		fmt.Printf("-- UnPack: nu=%d, offset=%d\n", nu, offsety+nlq)
		err = blas.ScalFloat(y,
			&la_.IOpt{"n", nu}, &la_.IOpt{"offset", offsety+nlq})
	*/
	return
}
예제 #10
0
func (u *matrixVar) Scal(alpha float64) {
	blas.ScalFloat(u.val, alpha)
}
예제 #11
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   Applies Nesterov-Todd scaling or its inverse.

   Computes

        x := W*x        (trans is false 'N', inverse = false 'N')
        x := W^T*x      (trans is true  'T', inverse = false 'N')
        x := W^{-1}*x   (trans is false 'N', inverse = true  'T')
        x := W^{-T}*x   (trans is true  'T', inverse = true  'T').

   x is a dense float matrix.

   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].

   The 'dnl' and 'dnli' entries are optional, and only present when the
   function is called from the nonlinear solver.
*/
func scale(x *matrix.FloatMatrix, W *sets.FloatMatrixSet, trans, inverse bool) (err error) {
	/*DEBUGGED*/
	var wl []*matrix.FloatMatrix
	var w *matrix.FloatMatrix
	ind := 0
	err = nil

	// var minor int = 0
	//if ! checkpnt.MinorEmpty() {
	//	minor = checkpnt.MinorTop()
	//}

	//fmt.Printf("\n%d.%04d scaling x=\n%v\n", checkpnt.Major(), minor, x.ToString("%.17f"))

	// Scaling for nonlinear component xk is xk := dnl .* xk; inverse
	// scaling is xk ./ dnl = dnli .* xk, where dnl = W['dnl'],
	// dnli = W['dnli'].

	if wl = W.At("dnl"); wl != nil {
		if inverse {
			w = W.At("dnli")[0]
		} else {
			w = W.At("dnl")[0]
		}
		for k := 0; k < x.Cols(); k++ {
			err = blas.TbmvFloat(w, x, &la_.IOpt{"n", w.Rows()}, &la_.IOpt{"k", 0},
				&la_.IOpt{"lda", 1}, &la_.IOpt{"offsetx", k * x.Rows()})
			if err != nil {
				//fmt.Printf("1. TbmvFloat: %v\n", err)
				return
			}
		}
		ind += w.Rows()
	}

	//if ! checkpnt.MinorEmpty() {
	//    checkpnt.Check("000scale", minor)
	//}

	// Scaling for linear 'l' component xk is xk := d .* xk; inverse
	// scaling is xk ./ d = di .* xk, where d = W['d'], di = W['di'].

	if inverse {
		w = W.At("di")[0]
	} else {
		w = W.At("d")[0]
	}

	for k := 0; k < x.Cols(); k++ {
		err = blas.TbmvFloat(w, x, &la_.IOpt{"n", w.Rows()}, &la_.IOpt{"k", 0},
			&la_.IOpt{"lda", 1}, &la_.IOpt{"offsetx", k*x.Rows() + ind})
		if err != nil {
			//fmt.Printf("2. TbmvFloat: %v\n", err)
			return
		}
	}
	ind += w.Rows()

	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("010scale", minor)
	//}

	// Scaling for 'q' component is
	//
	//    xk := beta * (2*v*v' - J) * xk
	//        = beta * (2*v*(xk'*v)' - J*xk)
	//
	// where beta = W['beta'][k], v = W['v'][k], J = [1, 0; 0, -I].
	//
	//Inverse scaling is
	//
	//    xk := 1/beta * (2*J*v*v'*J - J) * xk
	//        = 1/beta * (-J) * (2*v*((-J*xk)'*v)' + xk).
	//wf := matrix.FloatZeros(x.Cols(), 1)
	w = matrix.FloatZeros(x.Cols(), 1)
	for k, v := range W.At("v") {
		m := v.Rows()
		if inverse {
			blas.ScalFloat(x, -1.0, &la_.IOpt{"offset", ind}, &la_.IOpt{"inc", x.Rows()})
		}
		err = blas.GemvFloat(x, v, w, 1.0, 0.0, la_.OptTrans, &la_.IOpt{"m", m},
			&la_.IOpt{"n", x.Cols()}, &la_.IOpt{"offsetA", ind},
			&la_.IOpt{"lda", x.Rows()})
		if err != nil {
			//fmt.Printf("3. GemvFloat: %v\n", err)
			return
		}

		err = blas.ScalFloat(x, -1.0, &la_.IOpt{"offset", ind}, &la_.IOpt{"inc", x.Rows()})
		if err != nil {
			return
		}

		err = blas.GerFloat(v, w, x, 2.0, &la_.IOpt{"m", m},
			&la_.IOpt{"n", x.Cols()}, &la_.IOpt{"lda", x.Rows()},
			&la_.IOpt{"offsetA", ind})
		if err != nil {
			//fmt.Printf("4. GerFloat: %v\n", err)
			return
		}

		var a float64
		if inverse {
			blas.ScalFloat(x, -1.0,
				&la_.IOpt{"offset", ind}, &la_.IOpt{"inc", x.Rows()})
			// a[i,j] := 1.0/W[i,j]
			a = 1.0 / W.At("beta")[0].GetIndex(k)
		} else {
			a = W.At("beta")[0].GetIndex(k)
		}
		for i := 0; i < x.Cols(); i++ {
			blas.ScalFloat(x, a, &la_.IOpt{"n", m}, &la_.IOpt{"offset", ind + i*x.Rows()})
		}
		ind += m
	}

	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("020scale", minor)
	//}

	// Scaling for 's' component xk is
	//
	//     xk := vec( r' * mat(xk) * r )  if trans = 'N'
	//     xk := vec( r * mat(xk) * r' )  if trans = 'T'.
	//
	// r is kth element of W['r'].
	//
	// Inverse scaling is
	//
	//     xk := vec( rti * mat(xk) * rti' )  if trans = 'N'
	//     xk := vec( rti' * mat(xk) * rti )  if trans = 'T'.
	//
	// rti is kth element of W['rti'].
	maxn := 0
	for _, r := range W.At("r") {
		if r.Rows() > maxn {
			maxn = r.Rows()
		}
	}
	a := matrix.FloatZeros(maxn, maxn)
	for k, v := range W.At("r") {
		t := trans
		var r *matrix.FloatMatrix
		if !inverse {
			r = v
			t = !trans
		} else {
			r = W.At("rti")[k]
		}

		n := r.Rows()
		for i := 0; i < x.Cols(); i++ {
			// scale diagonal of xk by 0.5
			blas.ScalFloat(x, 0.5, &la_.IOpt{"offset", ind + i*x.Rows()},
				&la_.IOpt{"inc", n + 1}, &la_.IOpt{"n", n})

			// a = r*tril(x) (t is 'N') or a = tril(x)*r  (t is 'T')
			blas.Copy(r, a)
			if !t {
				err = blas.TrmmFloat(x, a, 1.0, la_.OptRight, &la_.IOpt{"m", n},
					&la_.IOpt{"n", n}, &la_.IOpt{"lda", n}, &la_.IOpt{"ldb", n},
					&la_.IOpt{"offsetA", ind + i*x.Rows()})
				if err != nil {
					//fmt.Printf("5. TrmmFloat: %v\n", err)
					return
				}

				// x := (r*a' + a*r')  if t is 'N'
				err = blas.Syr2kFloat(r, a, x, 1.0, 0.0, la_.OptNoTrans, &la_.IOpt{"n", n},
					&la_.IOpt{"k", n}, &la_.IOpt{"ldb", n}, &la_.IOpt{"ldc", n},
					&la_.IOpt{"offsetC", ind + i*x.Rows()})
				if err != nil {
					//fmt.Printf("6. Syr2kFloat: %v\n", err)
					return
				}

			} else {
				err = blas.TrmmFloat(x, a, 1.0, la_.OptLeft, &la_.IOpt{"m", n},
					&la_.IOpt{"n", n}, &la_.IOpt{"lda", n}, &la_.IOpt{"ldb", n},
					&la_.IOpt{"offsetA", ind + i*x.Rows()})
				if err != nil {
					//fmt.Printf("7. TrmmFloat: %v\n", err)
					return
				}

				// x := (r'*a + a'*r)  if t is 'T'
				err = blas.Syr2kFloat(r, a, x, 1.0, 0.0, la_.OptTrans, &la_.IOpt{"n", n},
					&la_.IOpt{"k", n}, &la_.IOpt{"ldb", n}, &la_.IOpt{"ldc", n},
					&la_.IOpt{"offsetC", ind + i*x.Rows()})
				if err != nil {
					//fmt.Printf("8. Syr2kFloat: %v\n", err)
					return
				}
			}
		}
		ind += n * n
	}
	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("030scale", minor)
	//}
	return
}
예제 #12
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   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
}
예제 #13
0
파일: misc.go 프로젝트: sguzwf/algorithm
func updateScaling(W *sets.FloatMatrixSet, lmbda, s, z *matrix.FloatMatrix) (err error) {
	err = nil
	var stmp, ztmp *matrix.FloatMatrix
	/*
	   Nonlinear and 'l' blocks

	      d :=  d .* sqrt( s ./ z )
	      lmbda := lmbda .* sqrt(s) .* sqrt(z)
	*/
	mnl := 0
	dnlset := W.At("dnl")
	dnliset := W.At("dnli")
	dset := W.At("d")
	diset := W.At("di")
	beta := W.At("beta")[0]
	if dnlset != nil && dnlset[0].NumElements() > 0 {
		mnl = dnlset[0].NumElements()
	}
	ml := dset[0].NumElements()
	m := mnl + ml
	//fmt.Printf("ml=%d, mnl=%d, m=%d'n", ml, mnl, m)

	stmp = matrix.FloatVector(s.FloatArray()[:m])
	stmp.Apply(math.Sqrt)
	s.SetIndexesFromArray(stmp.FloatArray(), matrix.MakeIndexSet(0, m, 1)...)

	ztmp = matrix.FloatVector(z.FloatArray()[:m])
	ztmp.Apply(math.Sqrt)
	z.SetIndexesFromArray(ztmp.FloatArray(), matrix.MakeIndexSet(0, m, 1)...)

	// d := d .* s .* z
	if len(dnlset) > 0 {
		blas.TbmvFloat(s, dnlset[0], &la_.IOpt{"n", mnl}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})
		blas.TbsvFloat(z, dnlset[0], &la_.IOpt{"n", mnl}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})
		//dnliset[0].Apply(dnlset[0], func(a float64)float64 { return 1.0/a})
		//--dnliset[0] = matrix.Inv(dnlset[0])
		matrix.Set(dnliset[0], dnlset[0])
		dnliset[0].Inv()
	}
	blas.TbmvFloat(s, dset[0], &la_.IOpt{"n", ml},
		&la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1}, &la_.IOpt{"offseta", mnl})
	blas.TbsvFloat(z, dset[0], &la_.IOpt{"n", ml},
		&la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1}, &la_.IOpt{"offseta", mnl})
	//diset[0].Apply(dset[0], func(a float64)float64 { return 1.0/a})
	//--diset[0] = matrix.Inv(dset[0])
	matrix.Set(diset[0], dset[0])
	diset[0].Inv()

	// lmbda := s .* z
	blas.CopyFloat(s, lmbda, &la_.IOpt{"n", m})
	blas.TbmvFloat(z, lmbda, &la_.IOpt{"n", m}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})

	// 'q' blocks.
	// Let st and zt be the new variables in the old scaling:
	//
	//     st = s_k,   zt = z_k
	//
	// and a = sqrt(st' * J * st),  b = sqrt(zt' * J * zt).
	//
	// 1. Compute the hyperbolic Householder transformation 2*q*q' - J
	//    that maps st/a to zt/b.
	//
	//        c = sqrt( (1 + st'*zt/(a*b)) / 2 )
	//        q = (st/a + J*zt/b) / (2*c).
	//
	//    The new scaling point is
	//
	//        wk := betak * sqrt(a/b) * (2*v[k]*v[k]' - J) * q
	//
	//    with betak = W['beta'][k].
	//
	// 3. The scaled variable:
	//
	//        lambda_k0 = sqrt(a*b) * c
	//        lambda_k1 = sqrt(a*b) * ( (2vk*vk' - J) * (-d*q + u/2) )_1
	//
	//    where
	//
	//        u = st/a - J*zt/b
	//        d = ( vk0 * (vk'*u) + u0/2 ) / (2*vk0 *(vk'*q) - q0 + 1).
	//
	// 4. Update scaling
	//
	//        v[k] := wk^1/2
	//              = 1 / sqrt(2*(wk0 + 1)) * (wk + e).
	//        beta[k] *=  sqrt(a/b)

	ind := m
	for k, v := range W.At("v") {
		m = v.NumElements()

		// ln = sqrt( lambda_k' * J * lambda_k ) !! NOT USED!!
		jnrm2(lmbda, m, ind) // ?? NOT USED ??

		// a = sqrt( sk' * J * sk ) = sqrt( st' * J * st )
		// s := s / a = st / a
		aa := jnrm2(s, m, ind)
		blas.ScalFloat(s, 1.0/aa, &la_.IOpt{"n", m}, &la_.IOpt{"offset", ind})

		// b = sqrt( zk' * J * zk ) = sqrt( zt' * J * zt )
		// z := z / a = zt / b
		bb := jnrm2(z, m, ind)
		blas.ScalFloat(z, 1.0/bb, &la_.IOpt{"n", m}, &la_.IOpt{"offset", ind})

		// c = sqrt( ( 1 + (st'*zt) / (a*b) ) / 2 )
		cc := blas.DotFloat(s, z, &la_.IOpt{"offsetx", ind}, &la_.IOpt{"offsety", ind},
			&la_.IOpt{"n", m})
		cc = math.Sqrt((1.0 + cc) / 2.0)

		// vs = v' * st / a
		vs := blas.DotFloat(v, s, &la_.IOpt{"offsety", ind}, &la_.IOpt{"n", m})

		// vz = v' * J *zt / b
		vz := jdot(v, z, m, 0, ind)

		// vq = v' * q where q = (st/a + J * zt/b) / (2 * c)
		vq := (vs + vz) / 2.0 / cc

		// vq = v' * q where q = (st/a + J * zt/b) / (2 * c)
		vu := vs - vz
		// lambda_k0 = c
		lmbda.SetIndex(ind, cc)

		// wk0 = 2 * vk0 * (vk' * q) - q0
		wk0 := 2.0*v.GetIndex(0)*vq - (s.GetIndex(ind)+z.GetIndex(ind))/2.0/cc

		// d = (v[0] * (vk' * u) - u0/2) / (wk0 + 1)
		dd := (v.GetIndex(0)*vu - s.GetIndex(ind)/2.0 + z.GetIndex(ind)/2.0) / (wk0 + 1.0)

		// lambda_k1 = 2 * v_k1 * vk' * (-d*q + u/2) - d*q1 + u1/2
		blas.CopyFloat(v, lmbda, &la_.IOpt{"offsetx", 1}, &la_.IOpt{"offsety", ind + 1},
			&la_.IOpt{"n", m - 1})
		blas.ScalFloat(lmbda, (2.0 * (-dd*vq + 0.5*vu)),
			&la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", ind + 1}, &la_.IOpt{"n", m - 1})
		blas.AxpyFloat(s, lmbda, 0.5*(1.0-dd/cc),
			&la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", ind + 1}, &la_.IOpt{"n", m - 1})
		blas.AxpyFloat(z, lmbda, 0.5*(1.0+dd/cc),
			&la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", ind + 1}, &la_.IOpt{"n", m - 1})

		// Scale so that sqrt(lambda_k' * J * lambda_k) = sqrt(aa*bb).
		blas.ScalFloat(lmbda, math.Sqrt(aa*bb), &la_.IOpt{"offset", ind}, &la_.IOpt{"n", m})

		// v := (2*v*v' - J) * q
		//    = 2 * (v'*q) * v' - (J* st/a + zt/b) / (2*c)
		blas.ScalFloat(v, 2.0*vq)
		v.SetIndex(0, v.GetIndex(0)-(s.GetIndex(ind)/2.0/cc))
		blas.AxpyFloat(s, v, 0.5/cc, &la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", 1},
			&la_.IOpt{"n", m - 1})
		blas.AxpyFloat(z, v, -0.5/cc, &la_.IOpt{"offsetx", ind}, &la_.IOpt{"n", m})

		// v := v^{1/2} = 1/sqrt(2 * (v0 + 1)) * (v + e)
		v0 := v.GetIndex(0) + 1.0
		v.SetIndex(0, v0)
		blas.ScalFloat(v, 1.0/math.Sqrt(2.0*v0))

		// beta[k] *= ( aa / bb )**1/2
		bk := beta.GetIndex(k)
		beta.SetIndex(k, bk*math.Sqrt(aa/bb))

		ind += m
	}
	//fmt.Printf("-- end of q:\nz=\n%v\nlmbda=\n%v\n", z.ConvertToString(), lmbda.ConvertToString())
	//fmt.Printf("beta=\n%v\n", beta.ConvertToString())

	// 's' blocks
	//
	// Let st, zt be the updated variables in the old scaling:
	//
	//     st = Ls * Ls', zt = Lz * Lz'.
	//
	// where Ls and Lz are the 's' components of s, z.
	//
	// 1.  SVD Lz'*Ls = Uk * lambda_k^+ * Vk'.
	//
	// 2.  New scaling is
	//
	//         r[k] := r[k] * Ls * Vk * diag(lambda_k^+)^{-1/2}
	//         rti[k] := r[k] * Lz * Uk * diag(lambda_k^+)^{-1/2}.
	//

	maxr := 0
	for _, m := range W.At("r") {
		if m.Rows() > maxr {
			maxr = m.Rows()
		}
	}
	work := matrix.FloatZeros(maxr*maxr, 1)
	vlensum := 0
	for _, m := range W.At("v") {
		vlensum += m.NumElements()
	}
	ind = mnl + ml + vlensum
	ind2 := ind
	ind3 := 0
	rset := W.At("r")
	rtiset := W.At("rti")

	for k, _ := range rset {
		r := rset[k]
		rti := rtiset[k]
		m = r.Rows()
		//fmt.Printf("m=%d, r=\n%v\nrti=\n%v\n", m, r.ConvertToString(), rti.ConvertToString())

		// r := r*sk = r*Ls
		blas.GemmFloat(r, s, work, 1.0, 0.0, &la_.IOpt{"m", m}, &la_.IOpt{"n", m},
			&la_.IOpt{"k", m}, &la_.IOpt{"ldb", m}, &la_.IOpt{"ldc", m},
			&la_.IOpt{"offsetb", ind2})
		//fmt.Printf("1 work=\n%v\n", work.ConvertToString())
		blas.CopyFloat(work, r, &la_.IOpt{"n", m * m})

		// rti := rti*zk = rti*Lz
		blas.GemmFloat(rti, z, work, 1.0, 0.0, &la_.IOpt{"m", m}, &la_.IOpt{"n", m},
			&la_.IOpt{"k", m}, &la_.IOpt{"ldb", m}, &la_.IOpt{"ldc", m},
			&la_.IOpt{"offsetb", ind2})
		//fmt.Printf("2 work=\n%v\n", work.ConvertToString())
		blas.CopyFloat(work, rti, &la_.IOpt{"n", m * m})

		// SVD Lz'*Ls = U * lmbds^+ * V'; store U in sk and V' in zk. '
		blas.GemmFloat(z, s, work, 1.0, 0.0, la_.OptTransA, &la_.IOpt{"m", m},
			&la_.IOpt{"n", m}, &la_.IOpt{"k", m}, &la_.IOpt{"lda", m}, &la_.IOpt{"ldb", m},
			&la_.IOpt{"ldc", m}, &la_.IOpt{"offseta", ind2}, &la_.IOpt{"offsetb", ind2})
		//fmt.Printf("3 work=\n%v\n", work.ConvertToString())

		// U = s, Vt = z
		lapack.GesvdFloat(work, lmbda, s, z, la_.OptJobuAll, la_.OptJobvtAll,
			&la_.IOpt{"m", m}, &la_.IOpt{"n", m}, &la_.IOpt{"lda", m}, &la_.IOpt{"ldu", m},
			&la_.IOpt{"ldvt", m}, &la_.IOpt{"offsets", ind}, &la_.IOpt{"offsetu", ind2},
			&la_.IOpt{"offsetvt", ind2})

		// r := r*V
		blas.GemmFloat(r, z, work, 1.0, 0.0, la_.OptTransB, &la_.IOpt{"m", m},
			&la_.IOpt{"n", m}, &la_.IOpt{"k", m}, &la_.IOpt{"ldb", m}, &la_.IOpt{"ldc", m},
			&la_.IOpt{"offsetb", ind2})
		//fmt.Printf("4 work=\n%v\n", work.ConvertToString())
		blas.CopyFloat(work, r, &la_.IOpt{"n", m * m})

		// rti := rti*U
		blas.GemmFloat(rti, s, work, 1.0, 0.0, &la_.IOpt{"m", m}, &la_.IOpt{"n", m},
			&la_.IOpt{"k", m}, &la_.IOpt{"ldb", m}, &la_.IOpt{"ldc", m},
			&la_.IOpt{"offsetb", ind2})
		//fmt.Printf("5 work=\n%v\n", work.ConvertToString())
		blas.CopyFloat(work, rti, &la_.IOpt{"n", m * m})

		for i := 0; i < m; i++ {
			a := 1.0 / math.Sqrt(lmbda.GetIndex(ind+i))
			blas.ScalFloat(r, a, &la_.IOpt{"n", m}, &la_.IOpt{"offset", m * i})
			blas.ScalFloat(rti, a, &la_.IOpt{"n", m}, &la_.IOpt{"offset", m * i})
		}
		ind += m
		ind2 += m * m
		ind3 += m // !!NOT USED: ind3!!
	}

	//fmt.Printf("-- end of s:\nz=\n%v\nlmbda=\n%v\n", z.ConvertToString(), lmbda.ConvertToString())

	return

}
예제 #14
0
파일: misc.go 프로젝트: sguzwf/algorithm
/*
   Evaluates

       x := H(lambda^{1/2}) * x   (inverse is 'N')
       x := H(lambda^{-1/2}) * x  (inverse is 'I').

   H is the Hessian of the logarithmic barrier.

*/
func scale2(lmbda, x *matrix.FloatMatrix, dims *sets.DimensionSet, mnl int, inverse bool) (err error) {
	err = nil

	//var minor int = 0
	//if ! checkpnt.MinorEmpty() {
	//	minor = checkpnt.MinorTop()
	//}

	//fmt.Printf("\n%d.%04d scale2 x=\n%v\nlmbda=\n%v\n", checkpnt.Major(), minor,
	//	x.ToString("%.17f"), lmbda.ToString("%.17f"))

	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("000scale2", minor)
	//}

	// For the nonlinear and 'l' blocks,
	//
	//     xk := xk ./ l   (inverse is 'N')
	//     xk := xk .* l   (inverse is 'I')
	//
	// where l is lmbda[:mnl+dims['l']].
	ind := mnl + dims.Sum("l")
	if !inverse {
		blas.TbsvFloat(lmbda, x, &la_.IOpt{"n", ind}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})
	} else {
		blas.TbmvFloat(lmbda, x, &la_.IOpt{"n", ind}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})
	}

	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("010scale2", minor)
	//}

	// For 'q' blocks, if inverse is 'N',
	//
	//     xk := 1/a * [ l'*J*xk;
	//         xk[1:] - (xk[0] + l'*J*xk) / (l[0] + 1) * l[1:] ].
	//
	// If inverse is 'I',
	//
	//     xk := a * [ l'*xk;
	//         xk[1:] + (xk[0] + l'*xk) / (l[0] + 1) * l[1:] ].
	//
	// a = sqrt(lambda_k' * J * lambda_k), l = lambda_k / a.
	for _, m := range dims.At("q") {
		var lx, a, c, x0 float64
		a = jnrm2(lmbda, m, ind) //&la_.IOpt{"n", m}, &la_.IOpt{"offset", ind})
		if !inverse {
			lx = jdot(lmbda, x, m, ind, ind) //&la_.IOpt{"n", m}, &la_.IOpt{"offsetx", ind},
			//&la_.IOpt{"offsety", ind})
			lx /= a
		} else {
			lx = blas.DotFloat(lmbda, x, &la_.IOpt{"n", m}, &la_.IOpt{"offsetx", ind},
				&la_.IOpt{"offsety", ind})
			lx /= a
		}
		x0 = x.GetIndex(ind)
		x.SetIndex(ind, lx)
		c = (lx + x0) / (lmbda.GetIndex(ind)/a + 1.0) / a
		if !inverse {
			c *= -1.0
		}
		blas.AxpyFloat(lmbda, x, c, &la_.IOpt{"n", m - 1}, &la_.IOpt{"offsetx", ind + 1},
			&la_.IOpt{"offsety", ind + 1})
		if !inverse {
			a = 1.0 / a
		}
		blas.ScalFloat(x, a, &la_.IOpt{"offset", ind}, &la_.IOpt{"n", m})
		ind += m
	}

	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("020scale2", minor)
	//}

	// For the 's' blocks, if inverse is 'N',
	//
	//     xk := vec( diag(l)^{-1/2} * mat(xk) * diag(k)^{-1/2}).
	//
	// If inverse is true,
	//
	//     xk := vec( diag(l)^{1/2} * mat(xk) * diag(k)^{1/2}).
	//
	// where l is kth block of lambda.
	//
	// We scale upper and lower triangular part of mat(xk) because the
	// inverse operation will be applied to nonsymmetric matrices.
	ind2 := ind
	sdims := dims.At("s")
	for k := 0; k < len(sdims); k++ {
		m := sdims[k]
		scaleF := func(v, x float64) float64 {
			return math.Sqrt(v) * math.Sqrt(x)
		}
		for j := 0; j < m; j++ {
			c := matrix.FloatVector(lmbda.FloatArray()[ind2 : ind2+m])
			c.ApplyConst(lmbda.GetIndex(ind2+j), scaleF)
			if !inverse {
				blas.Tbsv(c, x, &la_.IOpt{"n", m}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1},
					&la_.IOpt{"offsetx", ind + j*m})
			} else {
				blas.Tbmv(c, x, &la_.IOpt{"n", m}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1},
					&la_.IOpt{"offsetx", ind + j*m})
			}
		}
		ind += m * m
		ind2 += m
	}

	//if ! checkpnt.MinorEmpty() {
	//	checkpnt.Check("030scale2", minor)
	//}
	return
}
예제 #15
0
파일: cp.go 프로젝트: sguzwf/algorithm
// Solves a convex optimization problem with a linear objective
//
//       minimize    f0(x)
//       subject to  fk(x) <= 0, k = 1, ..., mnl
//                   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 Cp(F ConvexProg, 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 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()})
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}
	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, x0.Rows())
	}
	if !G.SizeMatch(cdim, x0.Rows()) {
		estr := fmt.Sprintf("'G' must be of size (%d,%d)", cdim, x0.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, x0.Rows())
	}
	if A.Cols() != x0.Rows() {
		estr := fmt.Sprintf("'A' must have %d columns", x0.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
	}

	solvername := solopts.KKTSolverName
	if len(solvername) == 0 {
		if len(dims.At("q")) > 0 || len(dims.At("s")) > 0 {
			solvername = "chol"
		} else {
			solvername = "chol2"
		}
	}

	c_e := newEpigraph(x0, 1.0)
	blas.ScalFloat(c_e.m(), 0.0)
	//F_e := &cpProg{F}
	G_e := epMatrixG{G, dims}
	A_e := epMatrixA{A}
	b_e := matrixVar{b}

	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)
		if err != nil {
			return nil, err
		}
		// 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.GetSubMatrix(1, 0))
		}
	} else {
		err = errors.New(fmt.Sprintf("solver '%s' not known", solvername))
		return
	}

	return cp_problem(F, c_e, &G_e, h, &A_e, &b_e, dims, kktsolver, solopts, x0, mnl)
}
예제 #16
0
파일: kkt.go 프로젝트: sguzwf/algorithm
// 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
}
예제 #17
0
파일: misc.go 프로젝트: sguzwf/algorithm
// The product x := (y o x).  If diag is 'D', the 's' part of y is
// diagonal and only the diagonal is stored.
func sprod(x, y *matrix.FloatMatrix, dims *sets.DimensionSet, mnl int, opts ...la_.Option) (err error) {

	err = nil
	diag := la_.GetStringOpt("diag", "N", opts...)
	// For the nonlinear and 'l' blocks:
	//
	//     yk o xk = yk .* xk.
	ind := mnl + dims.At("l")[0]
	err = blas.Tbmv(y, x, &la_.IOpt{"n", ind}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1})
	if err != nil {
		return
	}
	//fmt.Printf("Sprod l:x=\n%v\n", x)

	// For 'q' blocks:
	//
	//               [ l0   l1'  ]
	//     yk o xk = [           ] * xk
	//               [ l1   l0*I ]
	//
	// where yk = (l0, l1).
	for _, m := range dims.At("q") {
		dd := blas.DotFloat(x, y, &la_.IOpt{"offsetx", ind}, &la_.IOpt{"offsety", ind},
			&la_.IOpt{"n", m})
		//fmt.Printf("dd=%v\n", dd)
		alpha := y.GetIndex(ind)
		//fmt.Printf("scal=%v\n", alpha)
		blas.ScalFloat(x, alpha, &la_.IOpt{"offset", ind + 1}, &la_.IOpt{"n", m - 1})
		alpha = x.GetIndex(ind)
		//fmt.Printf("axpy=%v\n", alpha)
		blas.AxpyFloat(y, x, alpha, &la_.IOpt{"offsetx", ind + 1}, &la_.IOpt{"offsety", ind + 1},
			&la_.IOpt{"n", m - 1})
		x.SetIndex(ind, dd)
		ind += m
	}
	//fmt.Printf("Sprod q :x=\n%v\n", x)

	// For the 's' blocks:
	//
	//    yk o sk = .5 * ( Yk * mat(xk) + mat(xk) * Yk )
	//
	// where Yk = mat(yk) if diag is 'N' and Yk = diag(yk) if diag is 'D'.

	if diag[0] == 'N' {
		// DEBUGGED
		maxm := maxdim(dims.At("s"))
		A := matrix.FloatZeros(maxm, maxm)
		for _, m := range dims.At("s") {
			blas.Copy(x, A, &la_.IOpt{"offsetx", ind}, &la_.IOpt{"n", m * m})
			for i := 0; i < m-1; i++ { // i < m-1 --> i < m
				symm(A, m, 0)
				symm(y, m, ind)
			}
			err = blas.Syr2kFloat(A, y, x, 0.5, 0.0, &la_.IOpt{"n", m}, &la_.IOpt{"k", m},
				&la_.IOpt{"lda", m}, &la_.IOpt{"ldb", m}, &la_.IOpt{"ldc", m},
				&la_.IOpt{"offsetb", ind}, &la_.IOpt{"offsetc", ind})
			if err != nil {
				return
			}
			ind += m * m
		}
		//fmt.Printf("Sprod diag=N s:x=\n%v\n", x)

	} else {
		ind2 := ind
		for _, m := range dims.At("s") {
			for i := 0; i < m; i++ {
				// original: u = 0.5 * ( y[ind2+i:ind2+m] + y[ind2+i] )
				// creates matrix of elements: [ind2+i ... ind2+m] then
				// element wisely adds y[ind2+i] and scales by 0.5
				iset := matrix.MakeIndexSet(ind2+i, ind2+m, 1)
				u := matrix.FloatVector(y.GetIndexes(iset...))
				u.Add(y.GetIndex(ind2 + i))
				u.Scale(0.5)
				err = blas.Tbmv(u, x, &la_.IOpt{"n", m - i}, &la_.IOpt{"k", 0}, &la_.IOpt{"lda", 1},
					&la_.IOpt{"offsetx", ind + i*(m+1)})
				if err != nil {
					return
				}
			}
			ind += m * m
			ind2 += m
		}
		//fmt.Printf("Sprod diag=T s:x=\n%v\n", x)
	}
	return
}
예제 #18
0
파일: cp.go 프로젝트: sguzwf/algorithm
func (u *epigraph) Scal(alpha float64) {
	blas.ScalFloat(u.m(), alpha)
	u.set(u.t() * alpha)
}
예제 #19
0
파일: cp.go 프로젝트: sguzwf/algorithm
// Solves a convex optimization problem with a linear objective
//
//       minimize    f0(x)
//       subject to  fk(x) <= 0, k = 1, ..., mnl
//                   G*x   <= h
//                   A*x    = b.
//
// using custom solver for KKT equations.
//
func CpCustomKKT(F ConvexProg, 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 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()})
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}
	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, x0.Rows())
	}
	if !G.SizeMatch(cdim, x0.Rows()) {
		estr := fmt.Sprintf("'G' must be of size (%d,%d)", cdim, x0.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, x0.Rows())
	}
	if A.Cols() != x0.Rows() {
		estr := fmt.Sprintf("'A' must have %d columns", x0.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
	}

	if kktsolver == nil {
		err = errors.New("'kktsolver' must be non-nil function.")
		return
	}

	c_e := newEpigraph(x0, 1.0)
	blas.ScalFloat(x0, 0.0)
	G_e := epMatrixG{G, dims}
	A_e := epMatrixA{A}
	b_e := matrixVar{b}

	return cp_problem(F, c_e, &G_e, h, &A_e, &b_e, dims, kktsolver, solopts, x0, mnl)
}
예제 #20
0
파일: coneqp.go 프로젝트: sguzwf/algorithm
func coneqp_solver(P MatrixVarP, q MatrixVariable, G MatrixVarG, h *matrix.FloatMatrix,
	A MatrixVarA, b MatrixVariable, dims *sets.DimensionSet, kktsolver KKTConeSolverVar,
	solopts *SolverOptions, initvals *sets.FloatMatrixSet) (sol *Solution, err error) {

	err = nil
	EXPON := 3
	STEP := 0.99

	sol = &Solution{Unknown,
		nil,
		0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0}

	//var kktsolver func(*sets.FloatMatrixSet)(KKTFunc, error) = nil
	var refinement int
	var correction bool = true

	feasTolerance := FEASTOL
	absTolerance := ABSTOL
	relTolerance := RELTOL
	maxIter := MAXITERS
	if solopts.FeasTol > 0.0 {
		feasTolerance = solopts.FeasTol
	}
	if solopts.AbsTol > 0.0 {
		absTolerance = solopts.AbsTol
	}
	if solopts.RelTol > 0.0 {
		relTolerance = solopts.RelTol
	}
	if solopts.MaxIter > 0 {
		maxIter = solopts.MaxIter
	}
	if q == nil {
		err = errors.New("'q' must be non-nil MatrixVariable with one column")
		return
	}

	if h == nil {
		h = matrix.FloatZeros(0, 1)
	}
	if h.Cols() != 1 {
		err = errors.New("'h' must be non-nil matrix with one column")
		return
	}
	if dims == nil {
		dims = sets.NewDimensionSet("l", "q", "s")
		dims.Set("l", []int{h.Rows()})
	}

	err = checkConeQpDimensions(dims)
	if err != nil {
		return
	}

	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
	}

	// Data for kth 'q' constraint are found in rows indq[k]:indq[k+1] of G.
	indq := make([]int, 0)
	indq = append(indq, dims.At("l")[0])
	for _, k := range dims.At("q") {
		indq = append(indq, indq[len(indq)-1]+k)
	}

	// Data for kth 's' constraint are found in rows inds[k]:inds[k+1] of G.
	inds := make([]int, 0)
	inds = append(inds, indq[len(indq)-1])
	for _, k := range dims.At("s") {
		inds = append(inds, inds[len(inds)-1]+k*k)
	}

	if P == nil {
		err = errors.New("'P' must be non-nil MatrixVarP interface.")
		return
	}
	fP := func(u, v MatrixVariable, alpha, beta float64) error {
		return P.Pf(u, v, alpha, beta)
	}

	if G == nil {
		err = errors.New("'G' must be non-nil MatrixG interface.")
		return
	}
	fG := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return G.Gf(x, y, alpha, beta, trans)
	}

	// Check A and set defaults if it is nil
	fA := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return A.Af(x, y, alpha, beta, trans)
	}

	// Check b and set defaults if it is nil
	if b == nil {
		err = errors.New("'b' must be non-nil MatrixVariable interface.")
		return
	}

	// kktsolver(W) returns a routine for solving 3x3 block KKT system
	//
	//     [ 0   A'  G'*W^{-1} ] [ ux ]   [ bx ]
	//     [ A   0   0         ] [ uy ] = [ by ].
	//     [ G   0   -W'       ] [ uz ]   [ bz ]

	if kktsolver == nil {
		err = errors.New("nil kktsolver not allowed.")
		return
	}

	ws3 := matrix.FloatZeros(cdim, 1)
	wz3 := matrix.FloatZeros(cdim, 1)
	checkpnt.AddMatrixVar("ws3", ws3)
	checkpnt.AddMatrixVar("wz3", wz3)

	//
	res := func(ux, uy MatrixVariable, uz, us *matrix.FloatMatrix, vx, vy MatrixVariable, vz, vs *matrix.FloatMatrix, W *sets.FloatMatrixSet, lmbda *matrix.FloatMatrix) (err error) {
		// Evaluates residual in Newton equations:
		//
		//      [ vx ]    [ vx ]   [ 0     ]   [ P  A'  G' ]   [ ux        ]
		//      [ vy ] := [ vy ] - [ 0     ] - [ A  0   0  ] * [ uy        ]
		//      [ vz ]    [ vz ]   [ W'*us ]   [ G  0   0  ]   [ W^{-1}*uz ]
		//
		//      vs := vs - lmbda o (uz + us).

		// vx := vx - P*ux - A'*uy - G'*W^{-1}*uz
		minor := checkpnt.MinorTop()
		checkpnt.Check("00res", minor)
		fP(ux, vx, -1.0, 1.0)
		fA(uy, vx, -1.0, 1.0, la.OptTrans)
		blas.Copy(uz, wz3)
		scale(wz3, W, true, false)
		fG(&matrixVar{wz3}, vx, -1.0, 1.0, la.OptTrans)
		// vy := vy - A*ux
		fA(ux, vy, -1.0, 1.0, la.OptNoTrans)
		checkpnt.Check("50res", minor)

		// vz := vz - G*ux - W'*us
		fG(ux, &matrixVar{vz}, -1.0, 1.0, la.OptNoTrans)
		blas.Copy(us, ws3)
		scale(ws3, W, true, false)
		blas.AxpyFloat(ws3, vz, -1.0)

		// vs := vs - lmbda o (uz + us)
		blas.Copy(us, ws3)
		blas.AxpyFloat(uz, ws3, 1.0)
		sprod(ws3, lmbda, dims, 0, la.OptDiag)
		blas.AxpyFloat(ws3, vs, -1.0)
		checkpnt.Check("90res", minor)
		return
	}

	resx0 := math.Max(1.0, math.Sqrt(q.Dot(q)))
	resy0 := math.Max(1.0, math.Sqrt(b.Dot(b)))
	resz0 := math.Max(1.0, snrm2(h, dims, 0))
	//fmt.Printf("resx0: %.17f, resy0: %.17f, resz0: %.17f\n", resx0, resy0, resz0)

	var x, y, dx, dy, rx, ry MatrixVariable
	var z, s, ds, dz, rz *matrix.FloatMatrix
	var lmbda, lmbdasq, sigs, sigz *matrix.FloatMatrix
	var W *sets.FloatMatrixSet
	var f, f3 KKTFuncVar
	var resx, resy, resz, step, sigma, mu, eta float64
	var gap, pcost, dcost, relgap, pres, dres, f0 float64

	if cdim == 0 {
		// Solve
		//
		//     [ P  A' ] [ x ]   [ -q ]
		//     [       ] [   ] = [    ].
		//     [ A  0  ] [ y ]   [  b ]
		//
		Wtmp := sets.NewFloatSet("d", "di", "beta", "v", "r", "rti")
		Wtmp.Set("d", matrix.FloatZeros(0, 1))
		Wtmp.Set("di", matrix.FloatZeros(0, 1))
		f3, err = kktsolver(Wtmp)
		if err != nil {
			s := fmt.Sprintf("kkt error: %s", err)
			err = errors.New("2: Rank(A) < p or Rank(([P; A; G;]) < n : " + s)
			return
		}
		x = q.Copy()
		x.Scal(0.0)
		y = b.Copy()
		f3(x, y, matrix.FloatZeros(0, 1))

		// dres = || P*x + q + A'*y || / resx0
		rx = q.Copy()
		fP(x, rx, 1.0, 1.0)
		pcost = 0.5 * (x.Dot(rx) + x.Dot(q))
		fA(y, rx, 1.0, 1.0, la.OptTrans)
		dres = math.Sqrt(rx.Dot(rx) / resx0)

		ry = b.Copy()
		fA(x, ry, 1.0, -1.0, la.OptNoTrans)
		pres = math.Sqrt(ry.Dot(ry) / resy0)

		relgap = 0.0
		if pcost == 0.0 {
			relgap = math.NaN()
		}

		sol.Result = sets.NewFloatSet("x", "y", "s", "z")
		sol.Result.Set("x", x.Matrix())
		sol.Result.Set("y", y.Matrix())
		sol.Result.Set("s", matrix.FloatZeros(0, 1))
		sol.Result.Set("z", matrix.FloatZeros(0, 1))
		sol.Status = Optimal
		sol.Gap = 0.0
		sol.RelativeGap = relgap
		sol.PrimalObjective = pcost
		sol.DualObjective = pcost
		sol.PrimalInfeasibility = pres
		sol.DualInfeasibility = dres
		sol.PrimalSlack = 0.0
		sol.DualSlack = 0.0
		return
	}
	x = q.Copy()
	y = b.Copy()
	s = matrix.FloatZeros(cdim, 1)
	z = matrix.FloatZeros(cdim, 1)

	checkpnt.AddVerifiable("x", x)
	checkpnt.AddVerifiable("y", y)
	checkpnt.AddMatrixVar("s", s)
	checkpnt.AddMatrixVar("z", z)

	var ts, tz, nrms, nrmz float64

	if initvals == nil {
		// Factor
		//
		//     [ 0   A'  G' ]
		//     [ A   0   0  ].
		//     [ G   0  -I  ]
		//
		W = sets.NewFloatSet("d", "di", "v", "beta", "r", "rti")
		W.Set("d", matrix.FloatOnes(dims.At("l")[0], 1))
		W.Set("di", matrix.FloatOnes(dims.At("l")[0], 1))
		W.Set("beta", matrix.FloatOnes(len(dims.At("q")), 1))

		for _, n := range dims.At("q") {
			vm := matrix.FloatZeros(n, 1)
			vm.SetIndex(0, 1.0)
			W.Append("v", vm)
		}
		for _, n := range dims.At("s") {
			W.Append("r", matrix.FloatIdentity(n))
			W.Append("rti", matrix.FloatIdentity(n))
		}
		checkpnt.AddScaleVar(W)
		f, err = kktsolver(W)
		if err != nil {
			s := fmt.Sprintf("kkt error: %s", err)
			err = errors.New("3: Rank(A) < p or Rank([P; G; A]) < n : " + s)
			return
		}
		// Solve
		//
		//     [ P   A'  G' ]   [ x ]   [ -q ]
		//     [ A   0   0  ] * [ y ] = [  b ].
		//     [ G   0  -I  ]   [ z ]   [  h ]
		mCopy(q, x)
		x.Scal(-1.0)
		mCopy(b, y)
		blas.Copy(h, z)
		checkpnt.Check("00init", 1)
		err = f(x, y, z)
		if err != nil {
			s := fmt.Sprintf("kkt error: %s", err)
			err = errors.New("4: Rank(A) < p or Rank([P; G; A]) < n : " + s)
			return
		}
		blas.Copy(z, s)
		blas.ScalFloat(s, -1.0)
		checkpnt.Check("05init", 1)

		nrms = snrm2(s, dims, 0)
		ts, _ = maxStep(s, dims, 0, nil)
		//fmt.Printf("nrms = %.7f, ts = %.7f\n", nrms, ts)
		if ts >= -1e-8*math.Max(nrms, 1.0) {
			// a = 1.0 + ts
			a := 1.0 + ts
			is := make([]int, 0)
			// indexes s[:dims['l']]
			is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			// indexes s[indq[:-1]]
			is = append(is, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			// indexes s[ind:ind+m*m:m+1] (diagonal)
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				s.SetIndex(k, a+s.GetIndex(k))
			}
		}

		nrmz = snrm2(z, dims, 0)
		tz, _ = maxStep(z, dims, 0, nil)
		//fmt.Printf("nrmz = %.7f, tz = %.7f\n", nrmz, tz)
		if tz >= -1e-8*math.Max(nrmz, 1.0) {
			a := 1.0 + tz
			is := make([]int, 0)
			is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			is = append(is, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				z.SetIndex(k, a+z.GetIndex(k))
			}
		}

	} else {
		ix := initvals.At("x")[0]
		if ix != nil {
			mCopy(&matrixVar{ix}, x)
		} else {
			x.Scal(0.0)
		}

		is := initvals.At("s")[0]
		if is != nil {
			blas.Copy(is, s)
		} else {
			iset := make([]int, 0)
			iset = append(iset, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			iset = append(iset, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				iset = append(iset, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range iset {
				s.SetIndex(k, 1.0)
			}
		}

		iy := initvals.At("y")[0]
		if iy != nil {
			mCopy(&matrixVar{iy}, y)
		} else {
			y.Scal(0.0)
		}

		iz := initvals.At("z")[0]
		if iz != nil {
			blas.Copy(iz, z)
		} else {
			iset := make([]int, 0)
			iset = append(iset, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			iset = append(iset, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				iset = append(iset, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range iset {
				z.SetIndex(k, 1.0)
			}
		}
	}

	rx = q.Copy()
	ry = b.Copy()
	rz = matrix.FloatZeros(cdim, 1)
	dx = x.Copy()
	dy = y.Copy()
	dz = matrix.FloatZeros(cdim, 1)
	ds = matrix.FloatZeros(cdim, 1)
	lmbda = matrix.FloatZeros(cdim_diag, 1)
	lmbdasq = matrix.FloatZeros(cdim_diag, 1)
	sigs = matrix.FloatZeros(dims.Sum("s"), 1)
	sigz = matrix.FloatZeros(dims.Sum("s"), 1)

	checkpnt.AddVerifiable("rx", rx)
	checkpnt.AddVerifiable("ry", ry)
	checkpnt.AddVerifiable("dx", dx)
	checkpnt.AddVerifiable("dy", dy)
	//checkpnt.AddMatrixVar("rs", rs)
	checkpnt.AddMatrixVar("rz", rz)
	checkpnt.AddMatrixVar("ds", ds)
	checkpnt.AddMatrixVar("dz", dz)
	checkpnt.AddMatrixVar("lmbda", lmbda)
	checkpnt.AddMatrixVar("lmbdasq", lmbdasq)

	//var resx, resy, resz, step, sigma, mu, eta float64
	//var gap, pcost, dcost, relgap, pres, dres, f0 float64
	checkpnt.AddFloatVar("resx", &resx)
	checkpnt.AddFloatVar("resy", &resy)
	checkpnt.AddFloatVar("resz", &resz)
	checkpnt.AddFloatVar("step", &step)
	checkpnt.AddFloatVar("gap", &gap)
	checkpnt.AddFloatVar("dcost", &dcost)
	checkpnt.AddFloatVar("pcost", &pcost)
	checkpnt.AddFloatVar("dres", &dres)
	checkpnt.AddFloatVar("pres", &pres)
	checkpnt.AddFloatVar("relgap", &relgap)
	checkpnt.AddFloatVar("sigma", &sigma)

	var WS fVarClosure

	gap = sdot(s, z, dims, 0)
	for iter := 0; iter < maxIter+1; iter++ {
		checkpnt.MajorNext()
		checkpnt.Check("loopstart", 10)

		// f0 = (1/2)*x'*P*x + q'*x + r and  rx = P*x + q + A'*y + G'*z.
		mCopy(q, rx)
		fP(x, rx, 1.0, 1.0)
		f0 = 0.5 * (x.Dot(rx) + x.Dot(q))
		fA(y, rx, 1.0, 1.0, la.OptTrans)
		fG(&matrixVar{z}, rx, 1.0, 1.0, la.OptTrans)
		resx = math.Sqrt(rx.Dot(rx))

		// ry = A*x - b
		mCopy(b, ry)
		fA(x, ry, 1.0, -1.0, la.OptNoTrans)
		resy = math.Sqrt(ry.Dot(ry))

		// rz = s + G*x - h
		blas.Copy(s, rz)
		blas.AxpyFloat(h, rz, -1.0)
		fG(x, &matrixVar{rz}, 1.0, 1.0, la.OptNoTrans)
		resz = snrm2(rz, dims, 0)
		//fmt.Printf("resx: %.17f, resy: %.17f, resz: %.17f\n", resx, resy, resz)

		// Statistics for stopping criteria.

		// pcost = (1/2)*x'*P*x + q'*x
		// dcost = (1/2)*x'*P*x + q'*x + y'*(A*x-b) + z'*(G*x-h) '
		//       = (1/2)*x'*P*x + q'*x + y'*(A*x-b) + z'*(G*x-h+s) - z'*s
		//       = (1/2)*x'*P*x + q'*x + y'*ry + z'*rz - gap
		pcost = f0
		dcost = f0 + y.Dot(ry) + sdot(z, rz, dims, 0) - gap
		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}
		pres = math.Max(resy/resy0, resz/resz0)
		dres = resx / resx0

		if solopts.ShowProgress {
			if iter == 0 {
				// show headers of something
				fmt.Printf("% 10s% 12s% 10s% 8s% 7s\n",
					"pcost", "dcost", "gap", "pres", "dres")
			}
			// show something
			fmt.Printf("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e\n",
				iter, pcost, dcost, gap, pres, dres)
		}
		checkpnt.Check("stoptest", 100)

		if pres <= feasTolerance && dres <= feasTolerance &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance)) ||
			iter == maxIter {

			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				symm(z, m, ind)
				ind += m * m
			}
			ts, _ = maxStep(s, dims, 0, nil)
			tz, _ = maxStep(z, dims, 0, nil)
			if iter == maxIter {
				// terminated on max iterations.
				sol.Status = Unknown
				err = errors.New("Terminated (maximum iterations reached)")
				fmt.Printf("Terminated (maximum iterations reached)\n")
				return
			}
			// optimal solution found
			//fmt.Print("Optimal solution.\n")
			err = nil
			sol.Result = sets.NewFloatSet("x", "y", "s", "z")
			sol.Result.Set("x", x.Matrix())
			sol.Result.Set("y", y.Matrix())
			sol.Result.Set("s", s)
			sol.Result.Set("z", z)
			sol.Status = Optimal
			sol.Gap = gap
			sol.RelativeGap = relgap
			sol.PrimalObjective = pcost
			sol.DualObjective = dcost
			sol.PrimalInfeasibility = pres
			sol.DualInfeasibility = dres
			sol.PrimalSlack = -ts
			sol.DualSlack = -tz
			sol.PrimalResidualCert = math.NaN()
			sol.DualResidualCert = math.NaN()
			sol.Iterations = iter
			return
		}

		// Compute initial scaling W and scaled iterates:
		//
		//     W * z = W^{-T} * s = lambda.
		//
		// lmbdasq = lambda o lambda.
		if iter == 0 {
			W, err = computeScaling(s, z, lmbda, dims, 0)
			checkpnt.AddScaleVar(W)
		}
		ssqr(lmbdasq, lmbda, dims, 0)

		f3, err = kktsolver(W)
		if err != nil {
			if iter == 0 {
				s := fmt.Sprintf("kkt error: %s", err)
				err = errors.New("5: Rank(A) < p or Rank([P; A; G]) < n : " + s)
				return
			} else {
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(s, m, ind)
					symm(z, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, 0, nil)
				tz, _ = maxStep(z, dims, 0, nil)
				// terminated (singular KKT matrix)
				fmt.Printf("Terminated (singular KKT matrix).\n")
				err = errors.New("Terminated (singular KKT matrix).")
				sol.Result = sets.NewFloatSet("x", "y", "s", "z")
				sol.Result.Set("x", x.Matrix())
				sol.Result.Set("y", y.Matrix())
				sol.Result.Set("s", s)
				sol.Result.Set("z", z)
				sol.Status = Unknown
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.Iterations = iter
				return
			}
		}
		// f4_no_ir(x, y, z, s) solves
		//
		//     [ 0     ]   [ P  A'  G' ]   [ ux        ]   [ bx ]
		//     [ 0     ] + [ A  0   0  ] * [ uy        ] = [ by ]
		//     [ W'*us ]   [ G  0   0  ]   [ W^{-1}*uz ]   [ bz ]
		//
		//     lmbda o (uz + us) = bs.
		//
		// On entry, x, y, z, s contain bx, by, bz, bs.
		// On exit, they contain ux, uy, uz, us.

		f4_no_ir := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) error {
			// Solve
			//
			//     [ P A' G'   ] [ ux        ]    [ bx                    ]
			//     [ A 0  0    ] [ uy        ] =  [ by                    ]
			//     [ G 0 -W'*W ] [ W^{-1}*uz ]    [ bz - W'*(lmbda o\ bs) ]
			//
			//     us = lmbda o\ bs - uz.
			//
			// On entry, x, y, z, s  contains bx, by, bz, bs.
			// On exit they contain x, y, z, s.

			minor := checkpnt.MinorTop()
			checkpnt.Check("f4_no_ir_start", minor)
			// s := lmbda o\ s
			//    = lmbda o\ bs
			sinv(s, lmbda, dims, 0)

			// z := z - W'*s
			//    = bz - W'*(lambda o\ bs)
			blas.Copy(s, ws3)
			scale(ws3, W, true, false)
			blas.AxpyFloat(ws3, z, -1.0)

			checkpnt.Check("f4_no_ir_f3", minor+50)
			err := f3(x, y, z)
			if err != nil {
				return err
			}
			checkpnt.Check("f4_no_ir_f3", minor+60)

			// s := s - z
			//    = lambda o\ bs - uz.
			blas.AxpyFloat(z, s, -1.0)
			checkpnt.Check("f4_no_ir_f3", minor+90)
			return nil
		}

		if iter == 0 {
			if refinement > 0 || solopts.Debug {
				WS.wx = q.Copy()
				WS.wy = y.Copy()
				WS.ws = matrix.FloatZeros(cdim, 1)
				WS.wz = matrix.FloatZeros(cdim, 1)
				checkpnt.AddVerifiable("wx", WS.wx)
				checkpnt.AddVerifiable("wy", WS.wy)
				checkpnt.AddMatrixVar("ws", WS.ws)
				checkpnt.AddMatrixVar("wz", WS.wz)
			}
			if refinement > 0 {
				WS.wx2 = q.Copy()
				WS.wy2 = y.Copy()
				WS.ws2 = matrix.FloatZeros(cdim, 1)
				WS.wz2 = matrix.FloatZeros(cdim, 1)
				checkpnt.AddVerifiable("wx2", WS.wx2)
				checkpnt.AddVerifiable("wy2", WS.wy2)
				checkpnt.AddMatrixVar("ws2", WS.ws2)
				checkpnt.AddMatrixVar("wz2", WS.wz2)
			}
		}

		f4 := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) (err error) {
			minor := checkpnt.MinorTop()
			checkpnt.Check("f4start", minor)
			err = nil
			if refinement > 0 || solopts.Debug {
				mCopy(x, WS.wx)
				mCopy(y, WS.wy)
				blas.Copy(z, WS.wz)
				blas.Copy(s, WS.ws)
			}

			checkpnt.MinorPush(minor + 100)
			err = f4_no_ir(x, y, z, s)
			checkpnt.MinorPop()

			for i := 0; i < refinement; i++ {
				mCopy(WS.wx, WS.wx2)
				mCopy(WS.wy, WS.wy2)
				blas.Copy(WS.wz, WS.wz2)
				blas.Copy(WS.ws, WS.ws2)

				checkpnt.MinorPush(minor + (i+1)*300)
				res(x, y, z, s, WS.wx2, WS.wy2, WS.wz2, WS.ws2, W, lmbda)
				checkpnt.MinorPop()

				checkpnt.MinorPush(minor + (i+1)*500)
				f4_no_ir(WS.wx2, WS.wy2, WS.wz2, WS.ws2)
				checkpnt.MinorPop()

				WS.wx2.Axpy(x, 1.0)
				WS.wy2.Axpy(y, 1.0)
				blas.AxpyFloat(WS.wz2, z, 1.0)
				blas.AxpyFloat(WS.ws2, s, 1.0)
			}
			checkpnt.Check("f4end", minor+1500)
			return
		}

		//var mu, sigma, eta float64
		mu = gap / float64(dims.Sum("l", "s")+len(dims.At("q")))
		sigma, eta = 0.0, 0.0

		for i := 0; i < 2; i++ {
			// Solve
			//
			//     [ 0     ]   [ P  A' G' ]   [ dx        ]
			//     [ 0     ] + [ A  0  0  ] * [ dy        ] = -(1 - eta) * r
			//     [ W'*ds ]   [ G  0  0  ]   [ W^{-1}*dz ]
			//
			//     lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e (i=0)
			//     lmbda o (dz + ds) = -lmbda o lmbda - dsa o dza
			//                         + sigma*mu*e (i=1) where dsa, dza
			//                         are the solution for i=0.

			minor_base := (i + 1) * 2000
			// ds = -lmbdasq + sigma * mu * e  (if i is 0)
			//    = -lmbdasq - dsa o dza + sigma * mu * e  (if i is 1),
			//    where ds, dz are solution for i is 0.
			blas.ScalFloat(ds, 0.0)
			if correction && i == 1 {
				blas.AxpyFloat(ws3, ds, -1.0)
			}
			blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", dims.Sum("l", "q")})
			ind := dims.At("l")[0]
			ds.Add(sigma*mu, matrix.MakeIndexSet(0, ind, 1)...)
			for _, m := range dims.At("q") {
				ds.SetIndex(ind, sigma*mu+ds.GetIndex(ind))
				ind += m
			}
			ind2 := ind
			for _, m := range dims.At("s") {
				blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", m}, &la.IOpt{"incy", m + 1},
					&la.IOpt{"offsetx", ind2}, &la.IOpt{"offsety", ind})
				ds.Add(sigma*mu, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
				ind2 += m
			}

			checkpnt.Check("00loop01", minor_base)

			// (dx, dy, dz) := -(1 - eta) * (rx, ry, rz)
			//blas.ScalFloat(dx, 0.0)
			//blas.AxpyFloat(rx, dx, -1.0+eta)
			dx.Scal(0.0)
			rx.Axpy(dx, -1.0+eta)
			dy.Scal(0.0)
			ry.Axpy(dy, -1.0+eta)
			blas.ScalFloat(dz, 0.0)
			blas.AxpyFloat(rz, dz, -1.0+eta)

			//fmt.Printf("== Calling f4 %d\n", i)
			//fmt.Printf("dx=\n%v\n", dx.ToString("%.17f"))
			//fmt.Printf("ds=\n%v\n", ds.ToString("%.17f"))
			//fmt.Printf("dz=\n%v\n", dz.ToString("%.17f"))
			//fmt.Printf("== Entering f4 %d\n", i)
			checkpnt.MinorPush(minor_base)
			err = f4(dx, dy, dz, ds)
			checkpnt.MinorPop()
			if err != nil {
				if iter == 0 {
					s := fmt.Sprintf("kkt error: %s", err)
					err = errors.New("6: Rank(A) < p or Rank([P; A; G]) < n : " + s)
					return
				} else {
					ind = dims.Sum("l", "q")
					for _, m := range dims.At("s") {
						symm(s, m, ind)
						symm(z, m, ind)
						ind += m * m
					}
					ts, _ = maxStep(s, dims, 0, nil)
					tz, _ = maxStep(z, dims, 0, nil)
					return
				}
			}

			dsdz := sdot(ds, dz, dims, 0)
			if correction && i == 0 {
				blas.Copy(ds, ws3)
				sprod(ws3, dz, dims, 0)
			}

			// Maximum step to boundary.
			//
			// If i is 1, also compute eigenvalue decomposition of the 's'
			// blocks in ds, dz.  The eigenvectors Qs, Qz are stored in
			// dsk, dzk.  The eigenvalues are stored in sigs, sigz.
			scale2(lmbda, ds, dims, 0, false)
			scale2(lmbda, dz, dims, 0, false)
			checkpnt.Check("maxstep", minor_base+1500)
			if i == 0 {
				ts, _ = maxStep(ds, dims, 0, nil)
				tz, _ = maxStep(dz, dims, 0, nil)
			} else {
				ts, _ = maxStep(ds, dims, 0, sigs)
				tz, _ = maxStep(dz, dims, 0, sigz)
			}
			t := maxvec([]float64{0.0, ts, tz})
			//fmt.Printf("== t=%.17f from %v\n", t, []float64{ts, tz})
			if t == 0.0 {
				step = 1.0
			} else {
				if i == 0 {
					step = math.Min(1.0, 1.0/t)
				} else {
					step = math.Min(1.0, STEP/t)
				}
			}
			if i == 0 {
				m := math.Max(0.0, 1.0-step+dsdz/gap*(step*step))
				sigma = math.Pow(math.Min(1.0, m), float64(EXPON))
				eta = 0.0
			}
			//fmt.Printf("== step=%.17f sigma=%.17f dsdz=%.17f\n", step, sigma, dsdz)

		}

		checkpnt.Check("updatexy", 8000)
		dx.Axpy(x, step)
		dy.Axpy(y, step)
		//fmt.Printf("x=\n%v\n", x.ConvertToString())
		//fmt.Printf("y=\n%v\n", y.ConvertToString())
		//fmt.Printf("ds=\n%v\n", ds.ConvertToString())
		//fmt.Printf("dz=\n%v\n", dz.ConvertToString())

		// We will now replace the 'l' and 'q' blocks of ds and dz with
		// the updated iterates in the current scaling.
		// We also replace the 's' blocks of ds and dz with the factors
		// Ls, Lz in a factorization Ls*Ls', Lz*Lz' of the updated variables
		// in the current scaling.

		// ds := e + step*ds for nonlinear, 'l' and 'q' blocks.
		// dz := e + step*dz for nonlinear, 'l' and 'q' blocks.
		blas.ScalFloat(ds, step, &la.IOpt{"n", dims.Sum("l", "q")})
		blas.ScalFloat(dz, step, &la.IOpt{"n", dims.Sum("l", "q")})
		ind := dims.At("l")[0]
		is := matrix.MakeIndexSet(0, ind, 1)
		ds.Add(1.0, is...)
		dz.Add(1.0, is...)
		for _, m := range dims.At("q") {
			ds.SetIndex(ind, 1.0+ds.GetIndex(ind))
			dz.SetIndex(ind, 1.0+dz.GetIndex(ind))
			ind += m
		}
		checkpnt.Check("updatedsdz", 8010)

		// ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
		//
		// This replaces the 'l' and 'q' components of ds and dz with the
		// updated variables in the current scaling.
		// The 's' components of ds and dz are replaced with
		//
		// diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
		// diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
		scale2(lmbda, ds, dims, 0, true)
		scale2(lmbda, dz, dims, 0, true)
		checkpnt.Check("scale2", 8030)

		// sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
		// sigz := ( e + step*sigz ) ./ lambda for 's' blocks.
		blas.ScalFloat(sigs, step)
		blas.ScalFloat(sigz, step)
		sigs.Add(1.0)
		sigz.Add(1.0)
		sdimsum := dims.Sum("s")
		qdimsum := dims.Sum("l", "q")
		blas.TbsvFloat(lmbda, sigs, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})
		blas.TbsvFloat(lmbda, sigz, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})

		ind2 := qdimsum
		ind3 := 0
		sdims := dims.At("s")

		for k := 0; k < len(sdims); k++ {
			m := sdims[k]
			for i := 0; i < m; i++ {
				a := math.Sqrt(sigs.GetIndex(ind3 + i))
				blas.ScalFloat(ds, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
				a = math.Sqrt(sigz.GetIndex(ind3 + i))
				blas.ScalFloat(dz, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
			}
			ind2 += m * m
			ind3 += m
		}

		checkpnt.Check("updatescaling", 8050)
		err = updateScaling(W, lmbda, ds, dz)
		checkpnt.Check("afterscaling", 8060)

		// Unscale s, z, tau, kappa (unscaled variables are used only to
		// compute feasibility residuals).
		ind = dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, s, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(s, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, s, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(s, W, true, false)

		ind = dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, z, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(z, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, z, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(z, W, false, true)

		gap = blas.DotFloat(lmbda, lmbda)
		checkpnt.Check("eol", 8900)
		//fmt.Printf("== gap = %.17f\n", gap)
	}
	return
}
예제 #21
0
파일: conelp.go 프로젝트: sguzwf/algorithm
func conelp_solver(c MatrixVariable, G MatrixVarG, h *matrix.FloatMatrix,
	A MatrixVarA, b MatrixVariable, dims *sets.DimensionSet, kktsolver KKTConeSolverVar,
	solopts *SolverOptions, primalstart, dualstart *sets.FloatMatrixSet) (sol *Solution, err error) {

	err = nil
	const EXPON = 3
	const STEP = 0.99

	sol = &Solution{Unknown,
		nil,
		0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0}

	var refinement int

	if solopts.Refinement > 0 {
		refinement = solopts.Refinement
	} else {
		refinement = 0
		if len(dims.At("q")) > 0 || len(dims.At("s")) > 0 {
			refinement = 1
		}
	}
	feasTolerance := FEASTOL
	absTolerance := ABSTOL
	relTolerance := RELTOL
	maxIter := MAXITERS
	if solopts.FeasTol > 0.0 {
		feasTolerance = solopts.FeasTol
	}
	if solopts.AbsTol > 0.0 {
		absTolerance = solopts.AbsTol
	}
	if solopts.RelTol > 0.0 {
		relTolerance = solopts.RelTol
	}
	if solopts.MaxIter > 0 {
		maxIter = solopts.MaxIter
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}

	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
	}

	// Data for kth 'q' constraint are found in rows indq[k]:indq[k+1] of G.
	indq := make([]int, 0)
	indq = append(indq, dims.At("l")[0])
	for _, k := range dims.At("q") {
		indq = append(indq, indq[len(indq)-1]+k)
	}

	// Data for kth 's' constraint are found in rows inds[k]:inds[k+1] of G.
	inds := make([]int, 0)
	inds = append(inds, indq[len(indq)-1])
	for _, k := range dims.At("s") {
		inds = append(inds, inds[len(inds)-1]+k*k)
	}

	Gf := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return G.Gf(x, y, alpha, beta, trans)
	}

	Af := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return A.Af(x, y, alpha, beta, trans)
	}

	// kktsolver(W) returns a routine for solving 3x3 block KKT system
	//
	//     [ 0   A'  G'*W^{-1} ] [ ux ]   [ bx ]
	//     [ A   0   0         ] [ uy ] = [ by ].
	//     [ G   0   -W'       ] [ uz ]   [ bz ]

	if kktsolver == nil {
		err = errors.New("nil kktsolver not allowed.")
		return
	}

	// res() evaluates residual in 5x5 block KKT system
	//
	//     [ vx   ]    [ 0         ]   [ 0   A'  G'  c ] [ ux        ]
	//     [ vy   ]    [ 0         ]   [-A   0   0   b ] [ uy        ]
	//     [ vz   ] += [ W'*us     ] - [-G   0   0   h ] [ W^{-1}*uz ]
	//     [ vtau ]    [ dg*ukappa ]   [-c' -b' -h'  0 ] [ utau/dg   ]
	//
	//           vs += lmbda o (dz + ds)
	//       vkappa += lmbdg * (dtau + dkappa).
	ws3 := matrix.FloatZeros(cdim, 1)
	wz3 := matrix.FloatZeros(cdim, 1)
	checkpnt.AddMatrixVar("ws3", ws3)
	checkpnt.AddMatrixVar("wz3", wz3)

	//
	res := func(ux, uy MatrixVariable, uz, utau, us, ukappa *matrix.FloatMatrix,
		vx, vy MatrixVariable, vz, vtau, vs, vkappa *matrix.FloatMatrix,
		W *sets.FloatMatrixSet, dg float64, lmbda *matrix.FloatMatrix) (err error) {

		err = nil
		// vx := vx - A'*uy - G'*W^{-1}*uz - c*utau/dg
		Af(uy, vx, -1.0, 1.0, la.OptTrans)
		//fmt.Printf("post-Af vx=\n%v\n", vx)
		blas.Copy(uz, wz3)
		scale(wz3, W, false, true)
		Gf(&matrixVar{wz3}, vx, -1.0, 1.0, la.OptTrans)
		//blas.AxpyFloat(c, vx, -utau.Float()/dg)
		c.Axpy(vx, -utau.Float()/dg)

		// vy := vy + A*ux - b*utau/dg
		Af(ux, vy, 1.0, 1.0, la.OptNoTrans)
		//blas.AxpyFloat(b, vy, -utau.Float()/dg)
		b.Axpy(vy, -utau.Float()/dg)

		// vz := vz + G*ux - h*utau/dg + W'*us
		Gf(ux, &matrixVar{vz}, 1.0, 1.0, la.OptNoTrans)
		blas.AxpyFloat(h, vz, -utau.Float()/dg)
		blas.Copy(us, ws3)
		scale(ws3, W, true, false)
		blas.AxpyFloat(ws3, vz, 1.0)

		// vtau := vtau + c'*ux + b'*uy + h'*W^{-1}*uz + dg*ukappa
		var vtauplus float64 = dg*ukappa.Float() + c.Dot(ux) +
			b.Dot(uy) + sdot(h, wz3, dims, 0)
		vtau.SetValue(vtau.Float() + vtauplus)

		// vs := vs + lmbda o (uz + us)
		blas.Copy(us, ws3)
		blas.AxpyFloat(uz, ws3, 1.0)
		sprod(ws3, lmbda, dims, 0, &la.SOpt{"diag", "D"})
		blas.AxpyFloat(ws3, vs, 1.0)

		// vkappa += vkappa + lmbdag * (utau + ukappa)
		lscale := lmbda.GetIndex(-1)
		var vkplus float64 = lscale * (utau.Float() + ukappa.Float())
		vkappa.SetValue(vkappa.Float() + vkplus)
		return
	}

	resx0 := math.Max(1.0, math.Sqrt(c.Dot(c)))
	resy0 := math.Max(1.0, math.Sqrt(b.Dot(b)))
	resz0 := math.Max(1.0, snrm2(h, dims, 0))

	// select initial points

	//fmt.Printf("** initial resx0=%.4f, resy0=%.4f, resz0=%.4f \n", resx0, resy0, resz0)

	x := c.Copy()
	//blas.ScalFloat(x, 0.0)
	x.Scal(0.0)
	y := b.Copy()
	//blas.ScalFloat(y, 0.0)
	y.Scal(0.0)

	s := matrix.FloatZeros(cdim, 1)
	z := matrix.FloatZeros(cdim, 1)
	dx := c.Copy()
	dy := b.Copy()
	ds := matrix.FloatZeros(cdim, 1)
	dz := matrix.FloatZeros(cdim, 1)
	// these are singleton matrix
	dkappa := matrix.FloatValue(0.0)
	dtau := matrix.FloatValue(0.0)

	checkpnt.AddVerifiable("x", x)
	checkpnt.AddMatrixVar("s", s)
	checkpnt.AddMatrixVar("z", z)
	checkpnt.AddVerifiable("dx", dx)
	checkpnt.AddMatrixVar("ds", ds)
	checkpnt.AddMatrixVar("dz", dz)
	checkpnt.Check("00init", 1)

	var W *sets.FloatMatrixSet
	var f KKTFuncVar
	if primalstart == nil || dualstart == nil {
		// Factor
		//
		//     [ 0   A'  G' ]
		//     [ A   0   0  ].
		//     [ G   0  -I  ]
		//
		W = sets.NewFloatSet("d", "di", "v", "beta", "r", "rti")
		dd := dims.At("l")[0]
		mat := matrix.FloatOnes(dd, 1)
		W.Set("d", mat)
		mat = matrix.FloatOnes(dd, 1)
		W.Set("di", mat)
		dq := len(dims.At("q"))
		W.Set("beta", matrix.FloatOnes(dq, 1))

		for _, n := range dims.At("q") {
			vm := matrix.FloatZeros(n, 1)
			vm.SetIndex(0, 1.0)
			W.Append("v", vm)
		}
		for _, n := range dims.At("s") {
			W.Append("r", matrix.FloatIdentity(n))
			W.Append("rti", matrix.FloatIdentity(n))
		}
		f, err = kktsolver(W)
		if err != nil {
			fmt.Printf("kktsolver error: %s\n", err)
			return
		}
		checkpnt.AddScaleVar(W)
	}

	checkpnt.Check("05init", 5)
	if primalstart == nil {
		// minimize    || G * x - h ||^2
		// subject to  A * x = b
		//
		// by solving
		//
		//     [ 0   A'  G' ]   [ x  ]   [ 0 ]
		//     [ A   0   0  ] * [ dy ] = [ b ].
		//     [ G   0  -I  ]   [ -s ]   [ h ]
		//blas.ScalFloat(x, 0.0)
		//blas.CopyFloat(b, dy)
		checkpnt.MinorPush(5)
		x.Scal(0.0)
		mCopy(b, dy)
		blas.CopyFloat(h, s)

		err = f(x, dy, s)
		if err != nil {
			fmt.Printf("f(x,dy,s): %s\n", err)
			return
		}
		blas.ScalFloat(s, -1.0)
		//fmt.Printf("initial s=\n%v\n", s.ToString("%.5f"))
		checkpnt.MinorPop()
	} else {
		mCopy(&matrixVar{primalstart.At("x")[0]}, x)
		blas.Copy(primalstart.At("s")[0], s)
	}

	// ts = min{ t | s + t*e >= 0 }
	ts, _ := maxStep(s, dims, 0, nil)
	if ts >= 0 && primalstart != nil {
		err = errors.New("initial s is not positive")
		return
	}
	//fmt.Printf("initial ts=%.5f\n", ts)
	checkpnt.Check("10init", 10)

	if dualstart == nil {
		// minimize   || z ||^2
		// subject to G'*z + A'*y + c = 0
		//
		// by solving
		//
		//     [ 0   A'  G' ] [ dx ]   [ -c ]
		//     [ A   0   0  ] [ y  ] = [  0 ].
		//     [ G   0  -I  ] [ z  ]   [  0 ]
		//blas.Copy(c, dx)
		//blas.ScalFloat(dx, -1.0)
		//blas.ScalFloat(y, 0.0)
		checkpnt.MinorPush(10)
		mCopy(c, dx)
		dx.Scal(-1.0)
		y.Scal(0.0)
		blas.ScalFloat(z, 0.0)
		err = f(dx, y, z)
		if err != nil {
			fmt.Printf("f(dx,y,z): %s\n", err)
			return
		}
		//fmt.Printf("initial z=\n%v\n", z.ToString("%.5f"))
		checkpnt.MinorPop()
	} else {
		if len(dualstart.At("y")) > 0 {
			mCopy(&matrixVar{dualstart.At("y")[0]}, y)
		}
		blas.Copy(dualstart.At("z")[0], z)
	}

	// ts = min{ t | z + t*e >= 0 }
	tz, _ := maxStep(z, dims, 0, nil)
	if tz >= 0 && dualstart != nil {
		err = errors.New("initial z is not positive")
		return
	}
	//fmt.Printf("initial tz=%.5f\n", tz)

	nrms := snrm2(s, dims, 0)
	nrmz := snrm2(z, dims, 0)

	gap := 0.0
	pcost := 0.0
	dcost := 0.0
	relgap := 0.0

	checkpnt.Check("20init", 0)

	if primalstart == nil && dualstart == nil {
		gap = sdot(s, z, dims, 0)
		pcost = c.Dot(x)
		dcost = -b.Dot(y) - sdot(h, z, dims, 0)
		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}
		if ts <= 0 && tz < 0 &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance)) {
			// Constructed initial points happen to be feasible and optimal

			ind := dims.At("l")[0] + dims.Sum("q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				symm(z, m, ind)
				ind += m * m
			}

			// rx = A'*y + G'*z + c
			rx := c.Copy()
			Af(y, rx, 1.0, 1.0, la.OptTrans)
			Gf(&matrixVar{z}, rx, 1.0, 1.0, la.OptTrans)
			resx := math.Sqrt(rx.Dot(rx))
			// ry = b - A*x
			ry := b.Copy()
			Af(x, ry, -1.0, -1.0, la.OptNoTrans)
			resy := math.Sqrt(ry.Dot(ry))
			// rz = s + G*x - h
			rz := matrix.FloatZeros(cdim, 1)

			Gf(x, &matrixVar{rz}, 1.0, 0.0, la.OptNoTrans)
			blas.AxpyFloat(s, rz, 1.0)
			blas.AxpyFloat(h, rz, -1.0)
			resz := snrm2(rz, dims, 0)

			pres := math.Max(resy/resy0, resz/resz0)
			dres := resx / resx0
			cx := c.Dot(x)
			by := b.Dot(y)
			hz := sdot(h, z, dims, 0)

			//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
			sol.Result = sets.NewFloatSet("x", "y", "s", "x")
			sol.Result.Append("x", x.Matrix())
			sol.Result.Append("y", y.Matrix())
			sol.Result.Append("s", s)
			sol.Result.Append("z", z)
			sol.Status = Optimal
			sol.Gap = gap
			sol.RelativeGap = relgap
			sol.PrimalObjective = cx
			sol.DualObjective = -(by + hz)
			sol.PrimalInfeasibility = pres
			sol.DualInfeasibility = dres
			sol.PrimalSlack = -ts
			sol.DualSlack = -tz

			return
		}

		if ts >= -1e-8*math.Max(nrms, 1.0) {
			a := 1.0 + ts
			is := make([]int, 0)
			// indexes s[:dims['l']]
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			// indexes s[indq[:-1]]
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			// indexes s[ind:ind+m*m:m+1] (diagonal)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				s.SetIndex(k, a+s.GetIndex(k))
			}
		}

		if tz >= -1e-8*math.Max(nrmz, 1.0) {
			a := 1.0 + tz
			is := make([]int, 0)
			// indexes z[:dims['l']]
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			// indexes z[indq[:-1]]
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			// indexes z[ind:ind+m*m:m+1] (diagonal)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				z.SetIndex(k, a+z.GetIndex(k))
			}
		}
	} else if primalstart == nil && dualstart != nil {
		if ts >= -1e-8*math.Max(nrms, 1.0) {
			a := 1.0 + ts
			is := make([]int, 0)
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				s.SetIndex(k, a+s.GetIndex(k))
			}
		}
	} else if primalstart != nil && dualstart == nil {
		if tz >= -1e-8*math.Max(nrmz, 1.0) {
			a := 1.0 + tz
			is := make([]int, 0)
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				z.SetIndex(k, a+z.GetIndex(k))
			}
		}
	}

	tau := matrix.FloatValue(1.0)
	kappa := matrix.FloatValue(1.0)
	wkappa3 := matrix.FloatValue(0.0)

	rx := c.Copy()
	hrx := c.Copy()
	ry := b.Copy()
	hry := b.Copy()
	rz := matrix.FloatZeros(cdim, 1)
	hrz := matrix.FloatZeros(cdim, 1)
	sigs := matrix.FloatZeros(dims.Sum("s"), 1)
	sigz := matrix.FloatZeros(dims.Sum("s"), 1)
	lmbda := matrix.FloatZeros(cdim_diag+1, 1)
	lmbdasq := matrix.FloatZeros(cdim_diag+1, 1)

	gap = sdot(s, z, dims, 0)

	var x1, y1 MatrixVariable
	var z1 *matrix.FloatMatrix
	var dg, dgi float64
	var th *matrix.FloatMatrix
	var WS fVarClosure
	var f3 KKTFuncVar
	var cx, by, hz, rt float64
	var hresx, resx, hresy, resy, hresz, resz float64
	var dres, pres, dinfres, pinfres float64

	// check point variables
	checkpnt.AddMatrixVar("lmbda", lmbda)
	checkpnt.AddMatrixVar("lmbdasq", lmbdasq)
	checkpnt.AddVerifiable("rx", rx)
	checkpnt.AddVerifiable("ry", ry)
	checkpnt.AddMatrixVar("rz", rz)
	checkpnt.AddFloatVar("resx", &resx)
	checkpnt.AddFloatVar("resy", &resy)
	checkpnt.AddFloatVar("resz", &resz)
	checkpnt.AddFloatVar("hresx", &hresx)
	checkpnt.AddFloatVar("hresy", &hresy)
	checkpnt.AddFloatVar("hresz", &hresz)
	checkpnt.AddFloatVar("cx", &cx)
	checkpnt.AddFloatVar("by", &by)
	checkpnt.AddFloatVar("hz", &hz)
	checkpnt.AddFloatVar("gap", &gap)
	checkpnt.AddFloatVar("pres", &pres)
	checkpnt.AddFloatVar("dres", &dres)

	for iter := 0; iter < maxIter+1; iter++ {
		checkpnt.MajorNext()
		checkpnt.Check("loop-start", 100)

		// hrx = -A'*y - G'*z
		Af(y, hrx, -1.0, 0.0, la.OptTrans)
		Gf(&matrixVar{z}, hrx, -1.0, 1.0, la.OptTrans)
		hresx = math.Sqrt(hrx.Dot(hrx))

		// rx = hrx - c*tau
		//    = -A'*y - G'*z - c*tau
		mCopy(hrx, rx)
		c.Axpy(rx, -tau.Float())
		resx = math.Sqrt(rx.Dot(rx)) / tau.Float()

		// hry = A*x
		Af(x, hry, 1.0, 0.0, la.OptNoTrans)
		hresy = math.Sqrt(hry.Dot(hry))

		// ry = hry - b*tau
		//    = A*x - b*tau
		mCopy(hry, ry)
		b.Axpy(ry, -tau.Float())
		resy = math.Sqrt(ry.Dot(ry)) / tau.Float()

		// hrz = s + G*x
		Gf(x, &matrixVar{hrz}, 1.0, 0.0, la.OptNoTrans)
		blas.AxpyFloat(s, hrz, 1.0)
		hresz = snrm2(hrz, dims, 0)

		// rz = hrz - h*tau
		//    = s + G*x - h*tau
		blas.ScalFloat(rz, 0.0)
		blas.AxpyFloat(hrz, rz, 1.0)
		blas.AxpyFloat(h, rz, -tau.Float())
		resz = snrm2(rz, dims, 0) / tau.Float()

		// rt = kappa + c'*x + b'*y + h'*z '
		cx = c.Dot(x)
		by = b.Dot(y)
		hz = sdot(h, z, dims, 0)
		rt = kappa.Float() + cx + by + hz

		// Statistics for stopping criteria
		pcost = cx / tau.Float()
		dcost = -(by + hz) / tau.Float()

		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}

		pres = math.Max(resy/resy0, resz/resz0)
		dres = resx / resx0
		pinfres = math.NaN()
		if hz+by < 0.0 {
			pinfres = hresx / resx0 / (-hz - by)
		}
		dinfres = math.NaN()
		if cx < 0.0 {
			dinfres = math.Max(hresy/resy0, hresz/resz0) / (-cx)
		}

		if solopts.ShowProgress {
			if iter == 0 {
				// show headers of something
				fmt.Printf("% 10s% 12s% 10s% 8s% 7s % 5s\n",
					"pcost", "dcost", "gap", "pres", "dres", "k/t")
			}
			// show something
			fmt.Printf("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e% 7.0e\n",
				iter, pcost, dcost, gap, pres, dres, kappa.GetIndex(0)/tau.GetIndex(0))
		}

		checkpnt.Check("isready", 200)

		if (pres <= feasTolerance && dres <= feasTolerance &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance))) ||
			iter == maxIter {
			// done
			x.Scal(1.0 / tau.Float())
			y.Scal(1.0 / tau.Float())
			blas.ScalFloat(s, 1.0/tau.Float())
			blas.ScalFloat(z, 1.0/tau.Float())
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				symm(z, m, ind)
				ind += m * m
			}
			ts, _ = maxStep(s, dims, 0, nil)
			tz, _ = maxStep(z, dims, 0, nil)
			if iter == maxIter {
				// MaxIterations exceeded
				if solopts.ShowProgress {
					fmt.Printf("No solution. Max iterations exceeded\n")
				}
				err = errors.New("No solution. Max iterations exceeded")
				//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
				sol.Result = sets.NewFloatSet("x", "y", "s", "x")
				sol.Result.Append("x", x.Matrix())
				sol.Result.Append("y", y.Matrix())
				sol.Result.Append("s", s)
				sol.Result.Append("z", z)
				sol.Status = Unknown
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.PrimalResidualCert = pinfres
				sol.DualResidualCert = dinfres
				sol.Iterations = iter
				return
			} else {
				// Optimal
				if solopts.ShowProgress {
					fmt.Printf("Optimal solution.\n")
				}
				err = nil
				//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
				sol.Result = sets.NewFloatSet("x", "y", "s", "x")
				sol.Result.Append("x", x.Matrix())
				sol.Result.Append("y", y.Matrix())
				sol.Result.Append("s", s)
				sol.Result.Append("z", z)
				sol.Status = Optimal
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.PrimalResidualCert = math.NaN()
				sol.DualResidualCert = math.NaN()
				sol.Iterations = iter
				return
			}
		} else if !math.IsNaN(pinfres) && pinfres <= feasTolerance {
			// Primal Infeasible
			if solopts.ShowProgress {
				fmt.Printf("Primal infeasible.\n")
			}
			err = errors.New("Primal infeasible")
			y.Scal(1.0 / (-hz - by))
			blas.ScalFloat(z, 1.0/(-hz-by))
			//sol.X = nil; sol.Y = nil; sol.S = nil; sol.Z = nil
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(z, m, ind)
				ind += m * m
			}
			tz, _ = maxStep(z, dims, 0, nil)
			sol.Status = PrimalInfeasible
			sol.Result = sets.NewFloatSet("x", "y", "s", "x")
			sol.Result.Append("x", nil)
			sol.Result.Append("y", nil)
			sol.Result.Append("s", nil)
			sol.Result.Append("z", nil)
			sol.Gap = math.NaN()
			sol.RelativeGap = math.NaN()
			sol.PrimalObjective = math.NaN()
			sol.DualObjective = 1.0
			sol.PrimalInfeasibility = math.NaN()
			sol.DualInfeasibility = math.NaN()
			sol.PrimalSlack = math.NaN()
			sol.DualSlack = -tz
			sol.PrimalResidualCert = pinfres
			sol.DualResidualCert = math.NaN()
			sol.Iterations = iter
			return
		} else if !math.IsNaN(dinfres) && dinfres <= feasTolerance {
			// Dual Infeasible
			if solopts.ShowProgress {
				fmt.Printf("Dual infeasible.\n")
			}
			err = errors.New("Primal infeasible")
			x.Scal(1.0 / (-cx))
			blas.ScalFloat(s, 1.0/(-cx))
			//sol.X = nil; sol.Y = nil; sol.S = nil; sol.Z = nil
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				ind += m * m
			}
			ts, _ = maxStep(s, dims, 0, nil)
			sol.Status = PrimalInfeasible
			sol.Result = sets.NewFloatSet("x", "y", "s", "x")
			sol.Result.Append("x", nil)
			sol.Result.Append("y", nil)
			sol.Result.Append("s", nil)
			sol.Result.Append("z", nil)
			sol.Gap = math.NaN()
			sol.RelativeGap = math.NaN()
			sol.PrimalObjective = 1.0
			sol.DualObjective = math.NaN()
			sol.PrimalInfeasibility = math.NaN()
			sol.DualInfeasibility = math.NaN()
			sol.PrimalSlack = -ts
			sol.DualSlack = math.NaN()
			sol.PrimalResidualCert = math.NaN()
			sol.DualResidualCert = dinfres
			sol.Iterations = iter
			return
		}

		// Compute initial scaling W:
		//
		//     W * z = W^{-T} * s = lambda
		//     dg * tau = 1/dg * kappa = lambdag.
		if iter == 0 {
			//fmt.Printf("compute scaling: lmbda=\n%v\n", lmbda.ToString("%.17f"))
			//fmt.Printf("s=\n%v\n", s.ToString("%.17f"))
			//fmt.Printf("z=\n%v\n", z.ToString("%.17f"))
			W, err = computeScaling(s, z, lmbda, dims, 0)
			checkpnt.AddScaleVar(W)

			//     dg = sqrt( kappa / tau )
			//     dgi = sqrt( tau / kappa )
			//     lambda_g = sqrt( tau * kappa )
			//
			// lambda_g is stored in the last position of lmbda.

			dg = math.Sqrt(kappa.Float() / tau.Float())
			dgi = math.Sqrt(float64(tau.Float() / kappa.Float()))
			lmbda.SetIndex(-1, math.Sqrt(float64(tau.Float()*kappa.Float())))
			//fmt.Printf("lmbda=\n%v\n", lmbda.ToString("%.17f"))
			//W.Print()
			checkpnt.Check("compute_scaling", 300)
		}
		// lmbdasq := lmbda o lmbda
		ssqr(lmbdasq, lmbda, dims, 0)
		lmbdasq.SetIndex(-1, lmbda.GetIndex(-1)*lmbda.GetIndex(-1))

		// f3(x, y, z) solves
		//
		//     [ 0  A'  G'   ] [ ux        ]   [ bx ]
		//     [ A  0   0    ] [ uy        ] = [ by ].
		//     [ G  0  -W'*W ] [ W^{-1}*uz ]   [ bz ]
		//
		// On entry, x, y, z contain bx, by, bz.
		// On exit, they contain ux, uy, uz.
		//
		// Also solve
		//
		//     [ 0   A'  G'    ] [ x1        ]          [ c ]
		//     [-A   0   0     ]*[ y1        ] = -dgi * [ b ].
		//     [-G   0   W'*W  ] [ W^{-1}*z1 ]          [ h ]

		f3, err = kktsolver(W)
		if err != nil {
			fmt.Printf("kktsolver error=%v\n", err)
			return
		}
		if iter == 0 {
			x1 = c.Copy()
			y1 = b.Copy()
			z1 = matrix.FloatZeros(cdim, 1)
			checkpnt.AddVerifiable("x1", x1)
			checkpnt.AddMatrixVar("z1", z1)
		}
		mCopy(c, x1)
		x1.Scal(-1.0)
		mCopy(b, y1)
		blas.Copy(h, z1)
		err = f3(x1, y1, z1)
		//fmt.Printf("f3 result: x1=\n%v\nf3 result: z1=\n%v\n", x1, z1)
		x1.Scal(dgi)
		y1.Scal(dgi)
		blas.ScalFloat(z1, dgi)

		if err != nil {
			if iter == 0 && primalstart != nil && dualstart != nil {
				err = errors.New("Rank(A) < p or Rank([G; A]) < n")
				return
			} else {
				t_ := 1.0 / tau.Float()
				x.Scal(t_)
				y.Scal(t_)
				blas.ScalFloat(s, t_)
				blas.ScalFloat(z, t_)
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(s, m, ind)
					symm(z, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, 0, nil)
				tz, _ = maxStep(z, dims, 0, nil)
				err = errors.New("Terminated (singular KKT matrix).")
				//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
				sol.Result = sets.NewFloatSet("x", "y", "s", "x")
				sol.Result.Append("x", x.Matrix())
				sol.Result.Append("y", y.Matrix())
				sol.Result.Append("s", s)
				sol.Result.Append("z", z)
				sol.Status = Unknown
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.Iterations = iter
				return
			}
		}

		// f6_no_ir(x, y, z, tau, s, kappa) solves
		//
		//    [ 0         ]   [  0   A'  G'  c ] [ ux        ]    [ bx   ]
		//    [ 0         ]   [ -A   0   0   b ] [ uy        ]    [ by   ]
		//    [ W'*us     ] - [ -G   0   0   h ] [ W^{-1}*uz ] = -[ bz   ]
		//    [ dg*ukappa ]   [ -c' -b' -h'  0 ] [ utau/dg   ]    [ btau ]
		//
		//    lmbda o (uz + us) = -bs
		//    lmbdag * (utau + ukappa) = -bkappa.
		//
		// On entry, x, y, z, tau, s, kappa contain bx, by, bz, btau,
		// bkappa.  On exit, they contain ux, uy, uz, utau, ukappa.

		// th = W^{-T} * h
		if iter == 0 {
			th = matrix.FloatZeros(cdim, 1)
			checkpnt.AddMatrixVar("th", th)
		}

		blas.Copy(h, th)
		scale(th, W, true, true)

		f6_no_ir := func(x, y MatrixVariable, z, tau, s, kappa *matrix.FloatMatrix) (err error) {
			// Solve
			//
			// [  0   A'  G'    0   ] [ ux        ]
			// [ -A   0   0     b   ] [ uy        ]
			// [ -G   0   W'*W  h   ] [ W^{-1}*uz ]
			// [ -c' -b' -h'    k/t ] [ utau/dg   ]
			//
			//   [ bx                    ]
			//   [ by                    ]
			// = [ bz - W'*(lmbda o\ bs) ]
			//   [ btau - bkappa/tau     ]
			//
			// us = -lmbda o\ bs - uz
			// ukappa = -bkappa/lmbdag - utau.

			// First solve
			//
			// [ 0  A' G'   ] [ ux        ]   [  bx                    ]
			// [ A  0  0    ] [ uy        ] = [ -by                    ]
			// [ G  0 -W'*W ] [ W^{-1}*uz ]   [ -bz + W'*(lmbda o\ bs) ]

			minor := checkpnt.MinorTop()
			err = nil
			// y := -y = -by
			y.Scal(-1.0)

			// s := -lmbda o\ s = -lmbda o\ bs
			err = sinv(s, lmbda, dims, 0)
			blas.ScalFloat(s, -1.0)

			// z := -(z + W'*s) = -bz + W'*(lambda o\ bs)
			blas.Copy(s, ws3)
			checkpnt.Check("prescale", minor+5)
			checkpnt.MinorPush(minor + 5)
			err = scale(ws3, W, true, false)
			checkpnt.MinorPop()
			if err != nil {
				fmt.Printf("scale error: %s\n", err)
			}
			blas.AxpyFloat(ws3, z, 1.0)
			blas.ScalFloat(z, -1.0)

			checkpnt.Check("f3-call", minor+20)
			checkpnt.MinorPush(minor + 20)
			err = f3(x, y, z)
			checkpnt.MinorPop()
			checkpnt.Check("f3-return", minor+40)

			// Combine with solution of
			//
			// [ 0   A'  G'    ] [ x1         ]          [ c ]
			// [-A   0   0     ] [ y1         ] = -dgi * [ b ]
			// [-G   0   W'*W  ] [ W^{-1}*dzl ]          [ h ]
			//
			// to satisfy
			//
			// -c'*x - b'*y - h'*W^{-1}*z + dg*tau = btau - bkappa/tau. '

			// , kappa[0] := -kappa[0] / lmbd[-1] = -bkappa / lmbdag
			kap_ := kappa.Float()
			tau_ := tau.Float()
			kap_ = -kap_ / lmbda.GetIndex(-1)
			// tau[0] = tau[0] + kappa[0] / dgi = btau[0] - bkappa / tau
			tau_ = tau_ + kap_/dgi

			//tau[0] = dgi * ( tau[0] + xdot(c,x) + ydot(b,y) +
			//    misc.sdot(th, z, dims) ) / (1.0 + misc.sdot(z1, z1, dims))
			//tau_ = tau_ + blas.DotFloat(c, x) + blas.DotFloat(b, y) + sdot(th, z, dims, 0)
			tau_ += c.Dot(x)
			tau_ += b.Dot(y)
			tau_ += sdot(th, z, dims, 0)
			tau_ = dgi * tau_ / (1.0 + sdot(z1, z1, dims, 0))
			tau.SetValue(tau_)
			x1.Axpy(x, tau_)
			y1.Axpy(y, tau_)
			blas.AxpyFloat(z1, z, tau_)

			blas.AxpyFloat(z, s, -1.0)
			kap_ = kap_ - tau_
			kappa.SetValue(kap_)
			return
		}

		// f6(x, y, z, tau, s, kappa) solves the same system as f6_no_ir,
		// but applies iterative refinement. Following variables part of f6-closure
		// and ~ 12 is the limit. We wrap them to a structure.

		if iter == 0 {
			if refinement > 0 || solopts.Debug {
				WS.wx = c.Copy()
				WS.wy = b.Copy()
				WS.wz = matrix.FloatZeros(cdim, 1)
				WS.ws = matrix.FloatZeros(cdim, 1)
				WS.wtau = matrix.FloatValue(0.0)
				WS.wkappa = matrix.FloatValue(0.0)
				checkpnt.AddVerifiable("wx", WS.wx)
				checkpnt.AddMatrixVar("wz", WS.wz)
				checkpnt.AddMatrixVar("ws", WS.ws)
			}
			if refinement > 0 {
				WS.wx2 = c.Copy()
				WS.wy2 = b.Copy()
				WS.wz2 = matrix.FloatZeros(cdim, 1)
				WS.ws2 = matrix.FloatZeros(cdim, 1)
				WS.wtau2 = matrix.FloatValue(0.0)
				WS.wkappa2 = matrix.FloatValue(0.0)
				checkpnt.AddVerifiable("wx2", WS.wx2)
				checkpnt.AddMatrixVar("wz2", WS.wz2)
				checkpnt.AddMatrixVar("ws2", WS.ws2)
			}
		}

		f6 := func(x, y MatrixVariable, z, tau, s, kappa *matrix.FloatMatrix) error {
			var err error = nil
			minor := checkpnt.MinorTop()
			checkpnt.Check("startf6", minor+100)
			if refinement > 0 || solopts.Debug {
				mCopy(x, WS.wx)
				mCopy(y, WS.wy)
				blas.Copy(z, WS.wz)
				blas.Copy(s, WS.ws)
				WS.wtau.SetValue(tau.Float())
				WS.wkappa.SetValue(kappa.Float())
			}
			checkpnt.Check("pref6_no_ir", minor+200)
			err = f6_no_ir(x, y, z, tau, s, kappa)
			checkpnt.Check("postf6_no_ir", minor+399)
			for i := 0; i < refinement; i++ {
				mCopy(WS.wx, WS.wx2)
				mCopy(WS.wy, WS.wy2)
				blas.Copy(WS.wz, WS.wz2)
				blas.Copy(WS.ws, WS.ws2)
				WS.wtau2.SetValue(WS.wtau.Float())
				WS.wkappa2.SetValue(WS.wkappa.Float())
				checkpnt.Check("res-call", minor+400)

				checkpnt.MinorPush(minor + 400)
				err = res(x, y, z, tau, s, kappa, WS.wx2, WS.wy2, WS.wz2, WS.wtau2, WS.ws2, WS.wkappa2, W, dg, lmbda)
				checkpnt.MinorPop()

				checkpnt.Check("refine_pref6_no_ir", minor+500)
				checkpnt.MinorPush(minor + 500)
				err = f6_no_ir(WS.wx2, WS.wy2, WS.wz2, WS.wtau2, WS.ws2, WS.wkappa2)
				checkpnt.MinorPop()

				checkpnt.Check("refine_postf6_no_ir", minor+600)
				WS.wx2.Axpy(x, 1.0)
				WS.wy2.Axpy(y, 1.0)
				blas.AxpyFloat(WS.wz2, z, 1.0)
				blas.AxpyFloat(WS.ws2, s, 1.0)
				tau.SetValue(tau.Float() + WS.wtau2.Float())
				kappa.SetValue(kappa.Float() + WS.wkappa2.Float())
			}
			if solopts.Debug {
				checkpnt.MinorPush(minor + 700)
				res(x, y, z, tau, s, kappa, WS.wx, WS.wy, WS.wz, WS.wtau, WS.ws, WS.wkappa, W, dg, lmbda)
				checkpnt.MinorPop()
				fmt.Printf("KKT residuals\n")
				fmt.Printf("    'x'    : %.6e\n", math.Sqrt(WS.wx.Dot(WS.wx)))
				fmt.Printf("    'y'    : %.6e\n", math.Sqrt(WS.wy.Dot(WS.wy)))
				fmt.Printf("    'z'    : %.6e\n", snrm2(WS.wz, dims, 0))
				fmt.Printf("    'tau'  : %.6e\n", math.Abs(WS.wtau.Float()))
				fmt.Printf("    's'    : %.6e\n", snrm2(WS.ws, dims, 0))
				fmt.Printf("    'kappa': %.6e\n", math.Abs(WS.wkappa.Float()))
			}
			return err
		}

		var nrm float64 = blas.Nrm2Float(lmbda)
		mu := math.Pow(nrm, 2.0) / (1.0 + float64(cdim_diag))
		sigma := 0.0
		var step, tt, tk float64

		for i := 0; i < 2; i++ {
			// Solve
			//
			// [ 0         ]   [  0   A'  G'  c ] [ dx        ]
			// [ 0         ]   [ -A   0   0   b ] [ dy        ]
			// [ W'*ds     ] - [ -G   0   0   h ] [ W^{-1}*dz ]
			// [ dg*dkappa ]   [ -c' -b' -h'  0 ] [ dtau/dg   ]
			//
			//               [ rx   ]
			//               [ ry   ]
			// = - (1-sigma) [ rz   ]
			//               [ rtau ]
			//
			// lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e
			// lmbdag * (dtau + dkappa) = - kappa * tau + sigma*mu
			//
			// ds = -lmbdasq if i is 0
			//    = -lmbdasq - dsa o dza + sigma*mu*e if i is 1
			// dkappa = -lambdasq[-1] if i is 0
			//        = -lambdasq[-1] - dkappaa*dtaua + sigma*mu if i is 1.
			ind := dims.Sum("l", "q")
			ind2 := ind
			blas.Copy(lmbdasq, ds, &la.IOpt{"n", ind})
			blas.ScalFloat(ds, 0.0, &la.IOpt{"offset", ind})
			for _, m := range dims.At("s") {
				blas.Copy(lmbdasq, ds, &la.IOpt{"n", m}, &la.IOpt{"offsetx", ind2},
					&la.IOpt{"offsety", ind}, &la.IOpt{"incy", m + 1})
				ind += m * m
				ind2 += m
			}
			// dkappa[0] = lmbdasq[-1]
			dkappa.SetValue(lmbdasq.GetIndex(-1))

			if i == 1 {
				blas.AxpyFloat(ws3, ds, 1.0)
				ind = dims.Sum("l", "q")
				is := make([]int, 0)
				// indexes: [:dims['l']]
				if dims.At("l")[0] > 0 {
					is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
				}
				// ...[indq[:-1]]
				if len(indq) > 1 {
					is = append(is, indq[:len(indq)-1]...)
				}
				// ...[ind : ind+m*m : m+1] (diagonal)
				for _, m := range dims.At("s") {
					is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
					ind += m * m
				}
				//ds.Add(-sigma*mu, is...)
				for _, k := range is {
					ds.SetIndex(k, ds.GetIndex(k)-sigma*mu)
				}

				dk_ := dkappa.Float()
				wk_ := wkappa3.Float()
				dkappa.SetValue(dk_ + wk_ - sigma*mu)
			}
			// (dx, dy, dz, dtau) = (1-sigma)*(rx, ry, rz, rt)
			mCopy(rx, dx)
			dx.Scal(1.0 - sigma)
			mCopy(ry, dy)
			dy.Scal(1.0 - sigma)
			blas.Copy(rz, dz)
			blas.ScalFloat(dz, 1.0-sigma)
			// dtau[0] = (1.0 - sigma) * rt
			dtau.SetValue((1.0 - sigma) * rt)

			checkpnt.Check("pref6", (1+i)*1000)
			checkpnt.MinorPush((1 + i) * 1000)
			err = f6(dx, dy, dz, dtau, ds, dkappa)
			checkpnt.MinorPop()
			checkpnt.Check("postf6", (1+i)*1000+800)

			// Save ds o dz and dkappa * dtau for Mehrotra correction
			if i == 0 {
				blas.Copy(ds, ws3)
				sprod(ws3, dz, dims, 0)
				wkappa3.SetValue(dtau.Float() * dkappa.Float())
			}

			// Maximum step to boundary.
			//
			// If i is 1, also compute eigenvalue decomposition of the 's'
			// blocks in ds, dz.  The eigenvectors Qs, Qz are stored in
			// dsk, dzk.  The eigenvalues are stored in sigs, sigz.
			var ts, tz float64

			checkpnt.MinorPush((1+i)*1000 + 900)
			scale2(lmbda, ds, dims, 0, false)
			scale2(lmbda, dz, dims, 0, false)
			checkpnt.MinorPop()
			checkpnt.Check("post-scale2", (1+i)*1000+990)
			if i == 0 {
				ts, _ = maxStep(ds, dims, 0, nil)
				tz, _ = maxStep(dz, dims, 0, nil)
			} else {
				ts, _ = maxStep(ds, dims, 0, sigs)
				tz, _ = maxStep(dz, dims, 0, sigz)
			}
			dt_ := dtau.Float()
			dk_ := dkappa.Float()
			tt = -dt_ / lmbda.GetIndex(-1)
			tk = -dk_ / lmbda.GetIndex(-1)
			t := maxvec([]float64{0.0, ts, tz, tt, tk})
			if t == 0.0 {
				step = 1.0
			} else {
				if i == 0 {
					step = math.Min(1.0, 1.0/t)
				} else {
					step = math.Min(1.0, STEP/t)
				}
			}
			if i == 0 {
				// sigma = (1 - step)^3
				sigma = (1.0 - step) * (1.0 - step) * (1.0 - step)
				//sigma = math.Pow((1.0 - step), EXPON)
			}
		}
		//fmt.Printf("** tau = %.17f, kappa = %.17f\n", tau.Float(), kappa.Float())
		//fmt.Printf("** step = %.17f, sigma = %.17f\n", step, sigma)

		checkpnt.Check("update-xy", 7000)
		// Update x, y
		dx.Axpy(x, step)
		dy.Axpy(y, step)

		// Replace 'l' and 'q' blocks of ds and dz with the updated
		// variables in the current scaling.
		// Replace 's' blocks of ds and dz with the factors Ls, Lz in a
		// factorization Ls*Ls', Lz*Lz' of the updated variables in the
		// current scaling.
		//
		// ds := e + step*ds for 'l' and 'q' blocks.
		// dz := e + step*dz for 'l' and 'q' blocks.
		blas.ScalFloat(ds, step, &la.IOpt{"n", dims.Sum("l", "q")})
		blas.ScalFloat(dz, step, &la.IOpt{"n", dims.Sum("l", "q")})

		is := make([]int, 0)
		is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
		is = append(is, indq[:len(indq)-1]...)
		for _, k := range is {
			ds.SetIndex(k, 1.0+ds.GetIndex(k))
			dz.SetIndex(k, 1.0+dz.GetIndex(k))
		}
		checkpnt.Check("update-dsdz", 7500)

		// ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
		//
		// This replaces the 'l' and 'q' components of ds and dz with the
		// updated variables in the current scaling.
		// The 's' components of ds and dz are replaced with
		//
		// diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
		// diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
		checkpnt.MinorPush(7500)
		scale2(lmbda, ds, dims, 0, true)
		scale2(lmbda, dz, dims, 0, true)
		checkpnt.MinorPop()

		// sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
		// sigz := ( e + step*sigz ) ./ lambda for 's' blocks.
		blas.ScalFloat(sigs, step)
		blas.ScalFloat(sigz, step)
		sigs.Add(1.0)
		sigz.Add(1.0)
		sdimsum := dims.Sum("s")
		qdimsum := dims.Sum("l", "q")
		blas.TbsvFloat(lmbda, sigs, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})
		blas.TbsvFloat(lmbda, sigz, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})

		ind2 := qdimsum
		ind3 := 0
		sdims := dims.At("s")
		for k := 0; k < len(sdims); k++ {
			m := sdims[k]
			for i := 0; i < m; i++ {
				a := math.Sqrt(sigs.GetIndex(ind3 + i))
				blas.ScalFloat(ds, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
				a = math.Sqrt(sigz.GetIndex(ind3 + i))
				blas.ScalFloat(dz, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
			}
			ind2 += m * m
			ind3 += m
		}

		checkpnt.Check("pre-update-scaling", 7700)
		err = updateScaling(W, lmbda, ds, dz)
		checkpnt.Check("post-update-scaling", 7800)

		// For kappa, tau block:
		//
		//     dg := sqrt( (kappa + step*dkappa) / (tau + step*dtau) )
		//         = dg * sqrt( (1 - step*tk) / (1 - step*tt) )
		//
		//     lmbda[-1] := sqrt((tau + step*dtau) * (kappa + step*dkappa))
		//                = lmbda[-1] * sqrt(( 1 - step*tt) * (1 - step*tk))
		dg *= math.Sqrt(1.0-step*tk) / math.Sqrt(1.0-step*tt)
		dgi = 1.0 / dg
		a := math.Sqrt(1.0-step*tk) * math.Sqrt(1.0-step*tt)
		lmbda.SetIndex(-1, a*lmbda.GetIndex(-1))

		// Unscale s, z, tau, kappa (unscaled variables are used only to
		// compute feasibility residuals).
		ind := dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, s, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(s, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, s, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(s, W, true, false)

		ind = dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, z, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(z, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, z, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(z, W, false, true)

		kappa.SetValue(lmbda.GetIndex(-1) / dgi)
		tau.SetValue(lmbda.GetIndex(-1) * dgi)
		g := blas.Nrm2Float(lmbda, &la.IOpt{"n", lmbda.Rows() - 1}) / tau.Float()
		gap = g * g
		checkpnt.Check("end-of-loop", 8000)
		//fmt.Printf(" ** kappa=%.10f, tau=%.10f, gap=%.10f\n", kappa.Float(), tau.Float(), gap)

	}
	return
}
예제 #22
0
파일: cp.go 프로젝트: sguzwf/algorithm
// Solves a convex optimization problem with a linear objective
//
//       minimize    f0(x)
//       subject to  fk(x) <= 0, k = 1, ..., mnl
//                   G*x   <= h
//                   A*x    = b.
//
// using custom solver for KKT equations and constraint equations G and A.
//
func CpCustomMatrix(F ConvexProg, 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 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()})
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}
	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
	}

	var G_e MatrixVarG = nil
	if G == nil {
		G_e = &epMatrixG{matrix.FloatZeros(0, x0.Rows()), dims}
	} else {
		G_e = &epiMatrixG{G, dims}
	}

	var A_e MatrixVarA = nil
	if A == nil {
		A_e = &epMatrixA{matrix.FloatZeros(0, x0.Rows())}
	} else {
		A_e = &epiMatrixA{A}
	}

	// 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 kktsolver == nil {
		err = errors.New("'kktsolver' must be non-nil function.")
		return
	}

	c_e := newEpigraph(x0, 1.0)
	blas.ScalFloat(c_e.m(), 0.0)
	b_e := matrixVar{b}

	return cp_problem(F, c_e, G_e, h, A_e, &b_e, dims, kktsolver, solopts, x0, mnl)

}
예제 #23
0
파일: cpl.go 프로젝트: sguzwf/algorithm
// Internal CPL solver for CP and CLP problems. Everything is wrapped to proper interfaces
func cpl_solver(F ConvexVarProg, c MatrixVariable, G MatrixVarG, h *matrix.FloatMatrix,
	A MatrixVarA, b MatrixVariable, dims *sets.DimensionSet, kktsolver KKTCpSolverVar,
	solopts *SolverOptions, x0 MatrixVariable, mnl int) (sol *Solution, err error) {

	const (
		STEP              = 0.99
		BETA              = 0.5
		ALPHA             = 0.01
		EXPON             = 3
		MAX_RELAXED_ITERS = 8
	)

	var refinement int

	sol = &Solution{Unknown,
		nil,
		0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0}

	feasTolerance := FEASTOL
	absTolerance := ABSTOL
	relTolerance := RELTOL
	maxIter := MAXITERS
	if solopts.FeasTol > 0.0 {
		feasTolerance = solopts.FeasTol
	}
	if solopts.AbsTol > 0.0 {
		absTolerance = solopts.AbsTol
	}
	if solopts.RelTol > 0.0 {
		relTolerance = solopts.RelTol
	}
	if solopts.Refinement > 0 {
		refinement = solopts.Refinement
	} else {
		refinement = 1
	}
	if solopts.MaxIter > 0 {
		maxIter = solopts.MaxIter
	}

	if x0 == nil {
		mnl, x0, err = F.F0()
		if err != nil {
			return
		}
	}

	if c == nil {
		err = errors.New("Must define objective.")
		return
	}

	if h == nil {
		h = matrix.FloatZeros(0, 1)
	}
	if dims == nil {
		err = errors.New("Problem dimensions not defined.")
		return
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}

	cdim := dims.Sum("l", "q") + dims.SumSquared("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 {
		err = errors.New("'G' must be non-nil MatrixG interface.")
		return
	}
	fG := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return G.Gf(x, y, alpha, beta, trans)
	}

	// Check A and set defaults if it is nil
	if A == nil {
		err = errors.New("'A' must be non-nil MatrixA interface.")
		return
	}
	fA := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return A.Af(x, y, alpha, beta, trans)
	}

	if b == nil {
		err = errors.New("'b' must be non-nil MatrixVariable interface.")
		return
	}

	if kktsolver == nil {
		err = errors.New("nil kktsolver not allowed.")
		return
	}

	x := x0.Copy()
	y := b.Copy()
	y.Scal(0.0)
	z := matrix.FloatZeros(mnl+cdim, 1)
	s := matrix.FloatZeros(mnl+cdim, 1)
	ind := mnl + dims.At("l")[0]
	z.SetIndexes(1.0, matrix.MakeIndexSet(0, ind, 1)...)
	s.SetIndexes(1.0, matrix.MakeIndexSet(0, ind, 1)...)
	for _, m := range dims.At("q") {
		z.SetIndexes(1.0, ind)
		s.SetIndexes(1.0, ind)
		ind += m
	}
	for _, m := range dims.At("s") {
		iset := matrix.MakeIndexSet(ind, ind+m*m, m+1)
		z.SetIndexes(1.0, iset...)
		s.SetIndexes(1.0, iset...)
		ind += m * m
	}

	rx := x0.Copy()
	ry := b.Copy()
	dx := x.Copy()
	dy := y.Copy()
	rznl := matrix.FloatZeros(mnl, 1)
	rzl := matrix.FloatZeros(cdim, 1)
	dz := matrix.FloatZeros(mnl+cdim, 1)
	ds := matrix.FloatZeros(mnl+cdim, 1)
	lmbda := matrix.FloatZeros(mnl+cdim_diag, 1)
	lmbdasq := matrix.FloatZeros(mnl+cdim_diag, 1)
	sigs := matrix.FloatZeros(dims.Sum("s"), 1)
	sigz := matrix.FloatZeros(dims.Sum("s"), 1)

	dz2 := matrix.FloatZeros(mnl+cdim, 1)
	ds2 := matrix.FloatZeros(mnl+cdim, 1)

	newx := x.Copy()
	newy := y.Copy()
	newrx := x0.Copy()

	newz := matrix.FloatZeros(mnl+cdim, 1)
	news := matrix.FloatZeros(mnl+cdim, 1)
	newrznl := matrix.FloatZeros(mnl, 1)

	rx0 := rx.Copy()
	ry0 := ry.Copy()
	rznl0 := matrix.FloatZeros(mnl, 1)
	rzl0 := matrix.FloatZeros(cdim, 1)

	x0, dx0 := x.Copy(), dx.Copy()
	y0, dy0 := y.Copy(), dy.Copy()

	z0 := matrix.FloatZeros(mnl+cdim, 1)
	dz0 := matrix.FloatZeros(mnl+cdim, 1)
	dz20 := matrix.FloatZeros(mnl+cdim, 1)

	s0 := matrix.FloatZeros(mnl+cdim, 1)
	ds0 := matrix.FloatZeros(mnl+cdim, 1)
	ds20 := matrix.FloatZeros(mnl+cdim, 1)

	checkpnt.AddMatrixVar("z", z)
	checkpnt.AddMatrixVar("s", s)
	checkpnt.AddMatrixVar("dz", dz)
	checkpnt.AddMatrixVar("ds", ds)
	checkpnt.AddMatrixVar("rznl", rznl)
	checkpnt.AddMatrixVar("rzl", rzl)
	checkpnt.AddMatrixVar("lmbda", lmbda)
	checkpnt.AddMatrixVar("lmbdasq", lmbdasq)
	checkpnt.AddMatrixVar("z0", z0)
	checkpnt.AddMatrixVar("dz0", dz0)
	checkpnt.AddVerifiable("c", c)
	checkpnt.AddVerifiable("x", x)
	checkpnt.AddVerifiable("rx", rx)
	checkpnt.AddVerifiable("dx", dx)
	checkpnt.AddVerifiable("newrx", newrx)
	checkpnt.AddVerifiable("newx", newx)
	checkpnt.AddVerifiable("x0", x0)
	checkpnt.AddVerifiable("dx0", dx0)
	checkpnt.AddVerifiable("rx0", rx0)
	checkpnt.AddVerifiable("y", y)
	checkpnt.AddVerifiable("dy", dy)

	W0 := sets.NewFloatSet("d", "di", "dnl", "dnli", "v", "r", "rti", "beta")
	W0.Set("dnl", matrix.FloatZeros(mnl, 1))
	W0.Set("dnli", matrix.FloatZeros(mnl, 1))
	W0.Set("d", matrix.FloatZeros(dims.At("l")[0], 1))
	W0.Set("di", matrix.FloatZeros(dims.At("l")[0], 1))
	W0.Set("beta", matrix.FloatZeros(len(dims.At("q")), 1))
	for _, n := range dims.At("q") {
		W0.Append("v", matrix.FloatZeros(n, 1))
	}
	for _, n := range dims.At("s") {
		W0.Append("r", matrix.FloatZeros(n, n))
		W0.Append("rti", matrix.FloatZeros(n, n))
	}
	lmbda0 := matrix.FloatZeros(mnl+dims.Sum("l", "q", "s"), 1)
	lmbdasq0 := matrix.FloatZeros(mnl+dims.Sum("l", "q", "s"), 1)

	var f MatrixVariable = nil
	var Df MatrixVarDf = nil
	var H MatrixVarH = nil

	var ws3, wz3, wz2l, wz2nl *matrix.FloatMatrix
	var ws, wz, wz2, ws2 *matrix.FloatMatrix
	var wx, wx2, wy, wy2 MatrixVariable
	var gap, gap0, theta1, theta2, theta3, ts, tz, phi, phi0, mu, sigma, eta float64
	var resx, resy, reszl, resznl, pcost, dcost, dres, pres, relgap float64
	var resx0, resznl0, dres0, pres0 float64
	var dsdz, dsdz0, step, step0, dphi, dphi0, sigma0, eta0 float64
	var newresx, newresznl, newgap, newphi float64
	var W *sets.FloatMatrixSet
	var f3 KKTFuncVar

	checkpnt.AddFloatVar("gap", &gap)
	checkpnt.AddFloatVar("pcost", &pcost)
	checkpnt.AddFloatVar("dcost", &dcost)
	checkpnt.AddFloatVar("pres", &pres)
	checkpnt.AddFloatVar("dres", &dres)
	checkpnt.AddFloatVar("relgap", &relgap)
	checkpnt.AddFloatVar("step", &step)
	checkpnt.AddFloatVar("dsdz", &dsdz)
	checkpnt.AddFloatVar("resx", &resx)
	checkpnt.AddFloatVar("resy", &resy)
	checkpnt.AddFloatVar("reszl", &reszl)
	checkpnt.AddFloatVar("resznl", &resznl)

	// Declare fDf and fH here, they bind to Df and H as they are already declared.
	// ??really??

	var fDf func(u, v MatrixVariable, alpha, beta float64, trans la.Option) error = nil
	var fH func(u, v MatrixVariable, alpha, beta float64) error = nil

	relaxed_iters := 0
	for iters := 0; iters <= maxIter+1; iters++ {
		checkpnt.MajorNext()
		checkpnt.Check("loopstart", 10)

		checkpnt.MinorPush(10)
		if refinement != 0 || solopts.Debug {
			f, Df, H, err = F.F2(x, matrix.FloatVector(z.FloatArray()[:mnl]))
			fDf = func(u, v MatrixVariable, alpha, beta float64, trans la.Option) error {
				return Df.Df(u, v, alpha, beta, trans)
			}
			fH = func(u, v MatrixVariable, alpha, beta float64) error {
				return H.Hf(u, v, alpha, beta)
			}
		} else {
			f, Df, err = F.F1(x)
			fDf = func(u, v MatrixVariable, alpha, beta float64, trans la.Option) error {
				return Df.Df(u, v, alpha, beta, trans)
			}
		}
		checkpnt.MinorPop()

		gap = sdot(s, z, dims, mnl)

		// these are helpers, copies of parts of z,s
		z_mnl := matrix.FloatVector(z.FloatArray()[:mnl])
		z_mnl2 := matrix.FloatVector(z.FloatArray()[mnl:])
		s_mnl := matrix.FloatVector(s.FloatArray()[:mnl])
		s_mnl2 := matrix.FloatVector(s.FloatArray()[mnl:])

		// rx = c + A'*y + Df'*z[:mnl] + G'*z[mnl:]
		// -- y, rx MatrixArg
		mCopy(c, rx)
		fA(y, rx, 1.0, 1.0, la.OptTrans)
		fDf(&matrixVar{z_mnl}, rx, 1.0, 1.0, la.OptTrans)
		fG(&matrixVar{z_mnl2}, rx, 1.0, 1.0, la.OptTrans)
		resx = math.Sqrt(rx.Dot(rx))

		// rznl = s[:mnl] + f
		blas.Copy(s_mnl, rznl)
		blas.AxpyFloat(f.Matrix(), rznl, 1.0)
		resznl = blas.Nrm2Float(rznl)

		// rzl = s[mnl:] + G*x - h
		blas.Copy(s_mnl2, rzl)
		blas.AxpyFloat(h, rzl, -1.0)
		fG(x, &matrixVar{rzl}, 1.0, 1.0, la.OptNoTrans)
		reszl = snrm2(rzl, dims, 0)

		// Statistics for stopping criteria
		// pcost = c'*x
		// dcost = c'*x + y'*(A*x-b) + znl'*f(x) + zl'*(G*x-h)
		//       = c'*x + y'*(A*x-b) + znl'*(f(x)+snl) + zl'*(G*x-h+sl)
		//         - z'*s
		//       = c'*x + y'*ry + znl'*rznl + zl'*rzl - gap
		//pcost = blas.DotFloat(c, x)
		pcost = c.Dot(x)
		dcost = pcost + blas.DotFloat(y.Matrix(), ry.Matrix()) + blas.DotFloat(z_mnl, rznl)
		dcost += sdot(z_mnl2, rzl, dims, 0) - gap

		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}
		pres = math.Sqrt(resy*resy + resznl*resznl + reszl*reszl)
		dres = resx
		if iters == 0 {
			resx0 = math.Max(1.0, resx)
			resznl0 = math.Max(1.0, resznl)
			pres0 = math.Max(1.0, pres)
			dres0 = math.Max(1.0, dres)
			gap0 = gap
			theta1 = 1.0 / gap0
			theta2 = 1.0 / resx0
			theta3 = 1.0 / resznl0
		}
		phi = theta1*gap + theta2*resx + theta3*resznl
		pres = pres / pres0
		dres = dres / dres0

		if solopts.ShowProgress {
			if iters == 0 {
				// some headers
				fmt.Printf("% 10s% 12s% 10s% 8s% 7s\n",
					"pcost", "dcost", "gap", "pres", "dres")
			}
			fmt.Printf("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e\n",
				iters, pcost, dcost, gap, pres, dres)
		}

		checkpnt.Check("checkgap", 50)
		// Stopping criteria
		if (pres <= feasTolerance && dres <= feasTolerance &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance))) ||
			iters == maxIter {

			if iters == maxIter {
				s := "Terminated (maximum number of iterations reached)"
				if solopts.ShowProgress {
					fmt.Printf(s + "\n")
				}
				err = errors.New(s)
				sol.Status = Unknown
			} else {
				err = nil
				sol.Status = Optimal
			}
			sol.Result = sets.NewFloatSet("x", "y", "znl", "zl", "snl", "sl")
			sol.Result.Set("x", x.Matrix())
			sol.Result.Set("y", y.Matrix())
			sol.Result.Set("znl", matrix.FloatVector(z.FloatArray()[:mnl]))
			sol.Result.Set("zl", matrix.FloatVector(z.FloatArray()[mnl:]))
			sol.Result.Set("sl", matrix.FloatVector(s.FloatArray()[mnl:]))
			sol.Result.Set("snl", matrix.FloatVector(s.FloatArray()[:mnl]))
			sol.Gap = gap
			sol.RelativeGap = relgap
			sol.PrimalObjective = pcost
			sol.DualObjective = dcost
			sol.PrimalInfeasibility = pres
			sol.DualInfeasibility = dres
			sol.PrimalSlack = -ts
			sol.DualSlack = -tz
			return
		}

		// Compute initial scaling W:
		//
		//     W * z = W^{-T} * s = lambda.
		//
		// lmbdasq = lambda o lambda
		if iters == 0 {
			W, _ = computeScaling(s, z, lmbda, dims, mnl)
			checkpnt.AddScaleVar(W)
		}
		ssqr(lmbdasq, lmbda, dims, mnl)
		checkpnt.Check("lmbdasq", 90)

		// f3(x, y, z) solves
		//
		//     [ H   A'  GG'*W^{-1} ] [ ux ]   [ bx ]
		//     [ A   0   0          ] [ uy ] = [ by ].
		//     [ GG  0  -W'         ] [ uz ]   [ bz ]
		//
		// On entry, x, y, z contain bx, by, bz.
		// On exit, they contain ux, uy, uz.
		checkpnt.MinorPush(95)
		f3, err = kktsolver(W, x, z_mnl)
		checkpnt.MinorPop()
		checkpnt.Check("f3", 100)
		if err != nil {
			// ?? z_mnl is really copy of z[:mnl] ... should we copy here back to z??
			singular_kkt_matrix := false
			if iters == 0 {
				err = errors.New("Rank(A) < p or Rank([H(x); A; Df(x); G] < n")
				return
			} else if relaxed_iters > 0 && relaxed_iters < MAX_RELAXED_ITERS {
				// The arithmetic error may be caused by a relaxed line
				// search in the previous iteration.  Therefore we restore
				// the last saved state and require a standard line search.
				phi, gap = phi0, gap0
				mu = gap / float64(mnl+dims.Sum("l", "s")+len(dims.At("q")))
				blas.Copy(W0.At("dnl")[0], W.At("dnl")[0])
				blas.Copy(W0.At("dnli")[0], W.At("dnli")[0])
				blas.Copy(W0.At("d")[0], W.At("d")[0])
				blas.Copy(W0.At("di")[0], W.At("di")[0])
				blas.Copy(W0.At("beta")[0], W.At("beta")[0])
				for k, _ := range dims.At("q") {
					blas.Copy(W0.At("v")[k], W.At("v")[k])
				}
				for k, _ := range dims.At("s") {
					blas.Copy(W0.At("r")[k], W.At("r")[k])
					blas.Copy(W0.At("rti")[k], W.At("rti")[k])
				}
				//blas.Copy(x0, x)
				//x0.CopyTo(x)
				mCopy(x0, x)
				//blas.Copy(y0, y)
				mCopy(y0, y)
				blas.Copy(s0, s)
				blas.Copy(z0, z)
				blas.Copy(lmbda0, lmbda)
				blas.Copy(lmbdasq0, lmbdasq) // ???
				//blas.Copy(rx0, rx)
				//rx0.CopyTo(rx)
				mCopy(rx0, rx)
				//blas.Copy(ry0, ry)
				mCopy(ry0, ry)
				//resx = math.Sqrt(blas.DotFloat(rx, rx))
				resx = math.Sqrt(rx.Dot(rx))
				blas.Copy(rznl0, rznl)
				blas.Copy(rzl0, rzl)
				resznl = blas.Nrm2Float(rznl)

				relaxed_iters = -1

				// How about z_mnl here???
				checkpnt.MinorPush(120)
				f3, err = kktsolver(W, x, z_mnl)
				checkpnt.MinorPop()
				if err != nil {
					singular_kkt_matrix = true
				}
			} else {
				singular_kkt_matrix = true
			}

			if singular_kkt_matrix {
				msg := "Terminated (singular KKT matrix)."
				if solopts.ShowProgress {
					fmt.Printf(msg + "\n")
				}
				zl := matrix.FloatVector(z.FloatArray()[mnl:])
				sl := matrix.FloatVector(s.FloatArray()[mnl:])
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(sl, m, ind)
					symm(zl, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, mnl, nil)
				tz, _ = maxStep(z, dims, mnl, nil)

				err = errors.New(msg)
				sol.Status = Unknown
				sol.Result = sets.NewFloatSet("x", "y", "znl", "zl", "snl", "sl")
				sol.Result.Set("x", x.Matrix())
				sol.Result.Set("y", y.Matrix())
				sol.Result.Set("znl", matrix.FloatVector(z.FloatArray()[:mnl]))
				sol.Result.Set("zl", zl)
				sol.Result.Set("sl", sl)
				sol.Result.Set("snl", matrix.FloatVector(s.FloatArray()[:mnl]))
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				return
			}
		}

		// f4_no_ir(x, y, z, s) solves
		//
		//     [ 0     ]   [ H   A'  GG' ] [ ux        ]   [ bx ]
		//     [ 0     ] + [ A   0   0   ] [ uy        ] = [ by ]
		//     [ W'*us ]   [ GG  0   0   ] [ W^{-1}*uz ]   [ bz ]
		//
		//     lmbda o (uz + us) = bs.
		//
		// On entry, x, y, z, x, contain bx, by, bz, bs.
		// On exit, they contain ux, uy, uz, us.

		if iters == 0 {
			ws3 = matrix.FloatZeros(mnl+cdim, 1)
			wz3 = matrix.FloatZeros(mnl+cdim, 1)
			checkpnt.AddMatrixVar("ws3", ws3)
			checkpnt.AddMatrixVar("wz3", wz3)
		}

		f4_no_ir := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) (err error) {
			// Solve
			//
			//     [ H  A'  GG'  ] [ ux        ]   [ bx                    ]
			//     [ A  0   0    ] [ uy        ] = [ by                    ]
			//     [ GG 0  -W'*W ] [ W^{-1}*uz ]   [ bz - W'*(lmbda o\ bs) ]
			//
			//     us = lmbda o\ bs - uz.

			err = nil
			// s := lmbda o\ s
			//    = lmbda o\ bs
			sinv(s, lmbda, dims, mnl)

			// z := z - W'*s
			//    = bz - W' * (lambda o\ bs)
			blas.Copy(s, ws3)

			scale(ws3, W, true, false)
			blas.AxpyFloat(ws3, z, -1.0)

			// Solve for ux, uy, uz
			err = f3(x, y, z)

			// s := s - z
			//    = lambda o\ bs - z.
			blas.AxpyFloat(z, s, -1.0)
			return
		}

		if iters == 0 {
			wz2nl = matrix.FloatZeros(mnl, 1)
			wz2l = matrix.FloatZeros(cdim, 1)
			checkpnt.AddMatrixVar("wz2nl", wz2nl)
			checkpnt.AddMatrixVar("wz2l", wz2l)
		}

		res := func(ux, uy MatrixVariable, uz, us *matrix.FloatMatrix, vx, vy MatrixVariable, vz, vs *matrix.FloatMatrix) (err error) {

			// Evaluates residuals in Newton equations:
			//
			//     [ vx ]     [ 0     ]   [ H  A' GG' ] [ ux        ]
			//     [ vy ] -=  [ 0     ] + [ A  0  0   ] [ uy        ]
			//     [ vz ]     [ W'*us ]   [ GG 0  0   ] [ W^{-1}*uz ]
			//
			//     vs -= lmbda o (uz + us).
			err = nil
			minor := checkpnt.MinorTop()
			// vx := vx - H*ux - A'*uy - GG'*W^{-1}*uz
			fH(ux, vx, -1.0, 1.0)
			fA(uy, vx, -1.0, 1.0, la.OptTrans)
			blas.Copy(uz, wz3)
			scale(wz3, W, false, true)
			wz3_nl := matrix.FloatVector(wz3.FloatArray()[:mnl])
			wz3_l := matrix.FloatVector(wz3.FloatArray()[mnl:])
			fDf(&matrixVar{wz3_nl}, vx, -1.0, 1.0, la.OptTrans)
			fG(&matrixVar{wz3_l}, vx, -1.0, 1.0, la.OptTrans)

			checkpnt.Check("10res", minor+10)

			// vy := vy - A*ux
			fA(ux, vy, -1.0, 1.0, la.OptNoTrans)

			// vz := vz - W'*us - GG*ux
			err = fDf(ux, &matrixVar{wz2nl}, 1.0, 0.0, la.OptNoTrans)
			checkpnt.Check("15res", minor+10)
			blas.AxpyFloat(wz2nl, vz, -1.0)
			fG(ux, &matrixVar{wz2l}, 1.0, 0.0, la.OptNoTrans)
			checkpnt.Check("20res", minor+10)
			blas.AxpyFloat(wz2l, vz, -1.0, &la.IOpt{"offsety", mnl})
			blas.Copy(us, ws3)
			scale(ws3, W, true, false)
			blas.AxpyFloat(ws3, vz, -1.0)

			checkpnt.Check("30res", minor+10)

			// vs -= lmbda o (uz + us)
			blas.Copy(us, ws3)
			blas.AxpyFloat(uz, ws3, 1.0)
			sprod(ws3, lmbda, dims, mnl, &la.SOpt{"diag", "D"})
			blas.AxpyFloat(ws3, vs, -1.0)

			checkpnt.Check("90res", minor+10)
			return
		}

		// f4(x, y, z, s) solves the same system as f4_no_ir, but applies
		// iterative refinement.

		if iters == 0 {
			if refinement > 0 || solopts.Debug {
				wx = c.Copy()
				wy = b.Copy()
				wz = z.Copy()
				ws = s.Copy()
				checkpnt.AddVerifiable("wx", wx)
				checkpnt.AddMatrixVar("ws", ws)
				checkpnt.AddMatrixVar("wz", wz)
			}
			if refinement > 0 {
				wx2 = c.Copy()
				wy2 = b.Copy()
				wz2 = matrix.FloatZeros(mnl+cdim, 1)
				ws2 = matrix.FloatZeros(mnl+cdim, 1)
				checkpnt.AddVerifiable("wx2", wx2)
				checkpnt.AddMatrixVar("ws2", ws2)
				checkpnt.AddMatrixVar("wz2", wz2)
			}
		}

		f4 := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) (err error) {
			if refinement > 0 || solopts.Debug {
				mCopy(x, wx)
				mCopy(y, wy)
				blas.Copy(z, wz)
				blas.Copy(s, ws)
			}
			minor := checkpnt.MinorTop()
			checkpnt.Check("0_f4", minor+100)
			checkpnt.MinorPush(minor + 100)

			err = f4_no_ir(x, y, z, s)

			checkpnt.MinorPop()
			checkpnt.Check("1_f4", minor+200)
			for i := 0; i < refinement; i++ {
				mCopy(wx, wx2)
				mCopy(wy, wy2)
				blas.Copy(wz, wz2)
				blas.Copy(ws, ws2)

				checkpnt.Check("2_f4", minor+(1+i)*200)
				checkpnt.MinorPush(minor + (1+i)*200)

				res(x, y, z, s, wx2, wy2, wz2, ws2)
				checkpnt.MinorPop()
				checkpnt.Check("3_f4", minor+(1+i)*200+100)

				err = f4_no_ir(wx2, wy2, wz2, ws2)
				checkpnt.MinorPop()
				checkpnt.Check("4_f4", minor+(1+i)*200+199)
				wx2.Axpy(x, 1.0)
				wy2.Axpy(y, 1.0)
				blas.AxpyFloat(wz2, z, 1.0)
				blas.AxpyFloat(ws2, s, 1.0)
			}
			if solopts.Debug {
				res(x, y, z, s, wx, wy, wz, ws)
				fmt.Printf("KKT residuals:\n")
			}
			return
		}

		sigma, eta = 0.0, 0.0

		for i := 0; i < 2; i++ {
			minor := (i + 2) * 1000
			checkpnt.MinorPush(minor)
			checkpnt.Check("loop01", minor)

			// Solve
			//
			//     [ 0     ]   [ H  A' GG' ] [ dx        ]
			//     [ 0     ] + [ A  0  0   ] [ dy        ] = -(1 - eta)*r
			//     [ W'*ds ]   [ GG 0  0   ] [ W^{-1}*dz ]
			//
			//     lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e.
			//

			mu = gap / float64(mnl+dims.Sum("l", "s")+len(dims.At("q")))
			blas.ScalFloat(ds, 0.0)
			blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", mnl + dims.Sum("l", "q")})

			ind = mnl + dims.At("l")[0]
			iset := matrix.MakeIndexSet(0, ind, 1)
			ds.Add(sigma*mu, iset...)
			for _, m := range dims.At("q") {
				ds.Add(sigma*mu, ind)
				ind += m
			}
			ind2 := ind
			for _, m := range dims.At("s") {
				blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", m}, &la.IOpt{"offsetx", ind2},
					&la.IOpt{"offsety", ind}, &la.IOpt{"incy", m + 1})
				ds.Add(sigma*mu, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
				ind2 += m
			}

			dx.Scal(0.0)
			rx.Axpy(dx, -1.0+eta)
			dy.Scal(0.0)
			ry.Axpy(dy, -1.0+eta)
			dz.Scale(0.0)
			blas.AxpyFloat(rznl, dz, -1.0+eta)
			blas.AxpyFloat(rzl, dz, -1.0+eta, &la.IOpt{"offsety", mnl})
			//fmt.Printf("dx=\n%v\n", dx)
			//fmt.Printf("dz=\n%v\n", dz.ToString("%.7f"))
			//fmt.Printf("ds=\n%v\n", ds.ToString("%.7f"))

			checkpnt.Check("pref4", minor)
			checkpnt.MinorPush(minor)
			err = f4(dx, dy, dz, ds)
			if err != nil {
				if iters == 0 {
					s := fmt.Sprintf("Rank(A) < p or Rank([H(x); A; Df(x); G] < n (%s)", err)
					err = errors.New(s)
					return
				}
				msg := "Terminated (singular KKT matrix)."
				if solopts.ShowProgress {
					fmt.Printf(msg + "\n")
				}
				zl := matrix.FloatVector(z.FloatArray()[mnl:])
				sl := matrix.FloatVector(s.FloatArray()[mnl:])
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(sl, m, ind)
					symm(zl, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, mnl, nil)
				tz, _ = maxStep(z, dims, mnl, nil)

				err = errors.New(msg)
				sol.Status = Unknown
				sol.Result = sets.NewFloatSet("x", "y", "znl", "zl", "snl", "sl")
				sol.Result.Set("x", x.Matrix())
				sol.Result.Set("y", y.Matrix())
				sol.Result.Set("znl", matrix.FloatVector(z.FloatArray()[:mnl]))
				sol.Result.Set("zl", zl)
				sol.Result.Set("sl", sl)
				sol.Result.Set("snl", matrix.FloatVector(s.FloatArray()[:mnl]))
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				return
			}

			checkpnt.MinorPop()
			checkpnt.Check("postf4", minor+400)

			// Inner product ds'*dz and unscaled steps are needed in the
			// line search.
			dsdz = sdot(ds, dz, dims, mnl)
			blas.Copy(dz, dz2)
			scale(dz2, W, false, true)
			blas.Copy(ds, ds2)
			scale(ds2, W, true, false)

			checkpnt.Check("dsdz", minor+400)

			// Maximum steps to boundary.
			//
			// Also compute the eigenvalue decomposition of 's' blocks in
			// ds, dz.  The eigenvectors Qs, Qz are stored in ds, dz.
			// The eigenvalues are stored in sigs, sigz.

			scale2(lmbda, ds, dims, mnl, false)
			ts, _ = maxStep(ds, dims, mnl, sigs)
			scale2(lmbda, dz, dims, mnl, false)
			tz, _ = maxStep(dz, dims, mnl, sigz)
			t := maxvec([]float64{0.0, ts, tz})
			if t == 0 {
				step = 1.0
			} else {
				step = math.Min(1.0, STEP/t)
			}

			checkpnt.Check("maxstep", minor+400)

			var newDf MatrixVarDf = nil
			var newf MatrixVariable = nil

			// Backtrack until newx is in domain of f.
			backtrack := true
			for backtrack {
				mCopy(x, newx)
				dx.Axpy(newx, step)
				newf, newDf, err = F.F1(newx)
				if newf != nil {
					backtrack = false
				} else {
					step *= BETA
				}
			}

			// Merit function
			//
			//     phi = theta1 * gap + theta2 * norm(rx) +
			//         theta3 * norm(rznl)
			//
			// and its directional derivative dphi.

			phi = theta1*gap + theta2*resx + theta3*resznl
			if i == 0 {
				dphi = -phi
			} else {
				dphi = -theta1*(1-sigma)*gap - theta2*(1-eta)*resx - theta3*(1-eta)*resznl
			}

			var newfDf func(x, y MatrixVariable, a, b float64, trans la.Option) error

			// Line search
			backtrack = true
			for backtrack {
				mCopy(x, newx)
				dx.Axpy(newx, step)
				mCopy(y, newy)
				dy.Axpy(newy, step)
				blas.Copy(z, newz)
				blas.AxpyFloat(dz2, newz, step)
				blas.Copy(s, news)
				blas.AxpyFloat(ds2, news, step)

				newf, newDf, err = F.F1(newx)
				newfDf = func(u, v MatrixVariable, a, b float64, trans la.Option) error {
					return newDf.Df(u, v, a, b, trans)
				}

				// newrx = c + A'*newy + newDf'*newz[:mnl] + G'*newz[mnl:]
				newz_mnl := matrix.FloatVector(newz.FloatArray()[:mnl])
				newz_ml := matrix.FloatVector(newz.FloatArray()[mnl:])
				//blas.Copy(c, newrx)
				//c.CopyTo(newrx)
				mCopy(c, newrx)
				fA(newy, newrx, 1.0, 1.0, la.OptTrans)
				newfDf(&matrixVar{newz_mnl}, newrx, 1.0, 1.0, la.OptTrans)
				fG(&matrixVar{newz_ml}, newrx, 1.0, 1.0, la.OptTrans)
				newresx = math.Sqrt(newrx.Dot(newrx))

				// newrznl = news[:mnl] + newf
				news_mnl := matrix.FloatVector(news.FloatArray()[:mnl])
				//news_ml := matrix.FloatVector(news.FloatArray()[mnl:])
				blas.Copy(news_mnl, newrznl)
				blas.AxpyFloat(newf.Matrix(), newrznl, 1.0)
				newresznl = blas.Nrm2Float(newrznl)

				newgap = (1.0-(1.0-sigma)*step)*gap + step*step*dsdz
				newphi = theta1*newgap + theta2*newresx + theta3*newresznl

				if i == 0 {
					if newgap <= (1.0-ALPHA*step)*gap &&
						(relaxed_iters > 0 && relaxed_iters < MAX_RELAXED_ITERS ||
							newphi <= phi+ALPHA*step*dphi) {
						backtrack = false
						sigma = math.Min(newgap/gap, math.Pow((newgap/gap), EXPON))
						//fmt.Printf("break 1: sigma=%.7f\n", sigma)
						eta = 0.0
					} else {
						step *= BETA
					}
				} else {
					if relaxed_iters == -1 || (relaxed_iters == 0 && MAX_RELAXED_ITERS == 0) {
						// Do a standard line search.
						if newphi <= phi+ALPHA*step*dphi {
							relaxed_iters = 0
							backtrack = false
							//fmt.Printf("break 2 : newphi=%.7f\n", newphi)
						} else {
							step *= BETA
						}
					} else if relaxed_iters == 0 && relaxed_iters < MAX_RELAXED_ITERS {
						if newphi <= phi+ALPHA*step*dphi {
							// Relaxed l.s. gives sufficient decrease.
							relaxed_iters = 0
						} else {
							// Save state.
							phi0, dphi0, gap0 = phi, dphi, gap
							step0 = step

							blas.Copy(W.At("dnl")[0], W0.At("dnl")[0])
							blas.Copy(W.At("dnli")[0], W0.At("dnli")[0])
							blas.Copy(W.At("d")[0], W0.At("d")[0])
							blas.Copy(W.At("di")[0], W0.At("di")[0])
							blas.Copy(W.At("beta")[0], W0.At("beta")[0])
							for k, _ := range dims.At("q") {
								blas.Copy(W.At("v")[k], W0.At("v")[k])
							}
							for k, _ := range dims.At("s") {
								blas.Copy(W.At("r")[k], W0.At("r")[k])
								blas.Copy(W.At("rti")[k], W0.At("rti")[k])
							}
							mCopy(x, x0)
							mCopy(y, y0)
							mCopy(dx, dx0)
							mCopy(dy, dy0)
							blas.Copy(s, s0)
							blas.Copy(z, z0)
							blas.Copy(ds, ds0)
							blas.Copy(dz, dz0)
							blas.Copy(ds2, ds20)
							blas.Copy(dz2, dz20)
							blas.Copy(lmbda, lmbda0)
							blas.Copy(lmbdasq, lmbdasq0) // ???
							mCopy(rx, rx0)
							mCopy(ry, ry0)
							blas.Copy(rznl, rznl0)
							blas.Copy(rzl, rzl0)
							dsdz0 = dsdz
							sigma0, eta0 = sigma, eta
							relaxed_iters = 1
						}
						backtrack = false
						//fmt.Printf("break 3 : newphi=%.7f\n", newphi)

					} else if relaxed_iters >= 0 && relaxed_iters < MAX_RELAXED_ITERS &&
						MAX_RELAXED_ITERS > 0 {
						if newphi <= phi0+ALPHA*step0*dphi0 {
							// Relaxed l.s. gives sufficient decrease.
							relaxed_iters = 0
						} else {
							// Relaxed line search
							relaxed_iters += 1
						}
						backtrack = false
						//fmt.Printf("break 4 : newphi=%.7f\n", newphi)

					} else if relaxed_iters == MAX_RELAXED_ITERS && MAX_RELAXED_ITERS > 0 {
						if newphi <= phi0+ALPHA*step0*dphi0 {
							// Series of relaxed line searches ends
							// with sufficient decrease w.r.t. phi0.
							backtrack = false
							relaxed_iters = 0
							//fmt.Printf("break 5 : newphi=%.7f\n", newphi)
						} else if newphi >= phi0 {
							// Resume last saved line search
							phi, dphi, gap = phi0, dphi0, gap0
							step = step0
							blas.Copy(W0.At("dnl")[0], W.At("dnl")[0])
							blas.Copy(W0.At("dnli")[0], W.At("dnli")[0])
							blas.Copy(W0.At("d")[0], W.At("d")[0])
							blas.Copy(W0.At("di")[0], W.At("di")[0])
							blas.Copy(W0.At("beta")[0], W.At("beta")[0])
							for k, _ := range dims.At("q") {
								blas.Copy(W0.At("v")[k], W.At("v")[k])
							}
							for k, _ := range dims.At("s") {
								blas.Copy(W0.At("r")[k], W.At("r")[k])
								blas.Copy(W0.At("rti")[k], W.At("rti")[k])
							}
							mCopy(x, x0)
							mCopy(y, y0)
							mCopy(dx, dx0)
							mCopy(dy, dy0)
							blas.Copy(s, s0)
							blas.Copy(z, z0)
							blas.Copy(ds2, ds20)
							blas.Copy(dz2, dz20)
							blas.Copy(lmbda, lmbda0)
							blas.Copy(lmbdasq, lmbdasq0) // ???
							mCopy(rx, rx0)
							mCopy(ry, ry0)
							blas.Copy(rznl, rznl0)
							blas.Copy(rzl, rzl0)
							dsdz = dsdz0
							sigma, eta = sigma0, eta0
							relaxed_iters = -1

						} else if newphi <= phi+ALPHA*step*dphi {
							// Series of relaxed line searches ends
							// with sufficient decrease w.r.t. phi0.
							backtrack = false
							relaxed_iters = -1
							//fmt.Printf("break 6 : newphi=%.7f\n", newphi)
						}
					}
				}
			} // end of line search

			checkpnt.Check("eol", minor+900)

		} // end for [0,1]

		// Update x, y
		dx.Axpy(x, step)
		dy.Axpy(y, step)
		checkpnt.Check("updatexy", 5000)

		// Replace nonlinear, 'l' and 'q' blocks of ds and dz with the
		// updated variables in the current scaling.
		// Replace 's' blocks of ds and dz with the factors Ls, Lz in a
		// factorization Ls*Ls', Lz*Lz' of the updated variables in the
		// current scaling.

		// ds := e + step*ds for nonlinear, 'l' and 'q' blocks.
		// dz := e + step*dz for nonlinear, 'l' and 'q' blocks.
		blas.ScalFloat(ds, step, &la.IOpt{"n", mnl + dims.Sum("l", "q")})
		blas.ScalFloat(dz, step, &la.IOpt{"n", mnl + dims.Sum("l", "q")})
		ind := mnl + dims.At("l")[0]
		is := matrix.MakeIndexSet(0, ind, 1)
		ds.Add(1.0, is...)
		dz.Add(1.0, is...)
		for _, m := range dims.At("q") {
			ds.SetIndex(ind, 1.0+ds.GetIndex(ind))
			dz.SetIndex(ind, 1.0+dz.GetIndex(ind))
			ind += m
		}
		checkpnt.Check("updatedsdz", 5100)

		// ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
		//
		// This replaces the 'l' and 'q' components of ds and dz with the
		// updated variables in the current scaling.
		// The 's' components of ds and dz are replaced with
		//
		// diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
		// diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
		scale2(lmbda, ds, dims, mnl, true)
		scale2(lmbda, dz, dims, mnl, true)

		checkpnt.Check("scale2", 5200)

		// sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
		// sigz := ( e + step*sigz ) ./ lambda for 's' blocks.
		blas.ScalFloat(sigs, step)
		blas.ScalFloat(sigz, step)
		sigs.Add(1.0)
		sigz.Add(1.0)
		sdimsum := dims.Sum("s")
		qdimsum := dims.Sum("l", "q")
		blas.TbsvFloat(lmbda, sigs, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", mnl + qdimsum})
		blas.TbsvFloat(lmbda, sigz, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", mnl + qdimsum})

		checkpnt.Check("sigs", 5300)

		ind2 := mnl + qdimsum
		ind3 := 0
		sdims := dims.At("s")

		for k := 0; k < len(sdims); k++ {
			m := sdims[k]
			for i := 0; i < m; i++ {
				a := math.Sqrt(sigs.GetIndex(ind3 + i))
				blas.ScalFloat(ds, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
				a = math.Sqrt(sigz.GetIndex(ind3 + i))
				blas.ScalFloat(dz, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
			}
			ind2 += m * m
			ind3 += m
		}

		checkpnt.Check("scaling", 5400)
		err = updateScaling(W, lmbda, ds, dz)
		checkpnt.Check("postscaling", 5500)

		// Unscale s, z, tau, kappa (unscaled variables are used only to
		// compute feasibility residuals).
		ind = mnl + dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, s, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(s, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, s, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(s, W, true, false)
		checkpnt.Check("unscale_s", 5600)

		ind = mnl + dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, z, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(z, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, z, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(z, W, false, true)
		checkpnt.Check("unscale_z", 5700)

		gap = blas.DotFloat(lmbda, lmbda)

	}
	return
}
예제 #24
0
파일: kkt.go 프로젝트: sguzwf/algorithm
// 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
}