python类bmat()的实例源码

def pad(mat, padrow, padcol):
    """
    Add additional rows/columns to a numpy.matrix `mat`. The new rows/columns
    will be initialized with zeros.
    """
    if padrow < 0:
        padrow = 0
    if padcol < 0:
        padcol = 0
    rows, cols = mat.shape
    return numpy.bmat([[mat, numpy.matrix(numpy.zeros((rows, padcol)))],
                      [numpy.matrix(numpy.zeros((padrow, cols + padcol)))]])

def __init__(self, nb_steps, duration, omega2, state_estimator, com_target,
                 stance, tube_radius=0.02, tube_margin=0.01):
        start_com = state_estimator.com
        start_comd = state_estimator.comd
        tube = COMTube(
            start_com, com_target.p, stance, tube_radius, tube_margin)
        dt = duration / nb_steps
        I = eye(3)
        A = asarray(bmat([[I, dt * I], [zeros((3, 3)), I]]))
        B = asarray(bmat([[.5 * dt ** 2 * I], [dt * I]]))
        x_init = hstack([start_com, start_comd])
        x_goal = hstack([com_target.p, com_target.pd])
        C1, e1 = tube.primal_hrep
        D2, e2 = tube.dual_hrep
        C1 = hstack([C1, zeros(C1.shape)])
        C2 = zeros((D2.shape[0], A.shape[1]))
        D1 = zeros((C1.shape[0], B.shape[1]))
        C = vstack([C1, C2])
        D = vstack([D1, D2])
        e = hstack([e1, e2])
        lmpc = LinearPredictiveControl(
            A, B, C, D, e, x_init, x_goal, nb_steps, wxt=1., wxc=1e-1, wu=1e-1)
        lmpc.build()
        try:
            lmpc.solve()
            U = lmpc.U
            P = lmpc.X[:-1, 0:3]
            V = lmpc.X[:-1, 3:6]
            Z = P + (gravity - U) / omega2
            preview = ZMPPreviewBuffer([stance])
            preview.update(P, V, Z, [dt] * nb_steps, omega2)
        except ValueError as e:
            print "%s error:" % type(self).__name__, e
            preview = None
        self.lmpc = lmpc
        self.preview = preview
        self.tube = tube

def build(self, rows):
        return np.bmat(rows)

def test_diag():
    n0, n1, n2, n3 = 4, 5, 6, 7
    A = npr.randn(n0, n1)
    B = npr.randn(n2, n3)

    K_ = np.bmat((
        (A, np.zeros((n0, n3))),
        (np.zeros((n2, n1)), B)
    ))

    K = block_diag((A, B))

    assert np.allclose(K_, K)

def test_linear_operator():
    npr.seed(0)

    nx, nineq, neq = 4, 6, 7
    Q = npr.randn(nx, nx)
    G = npr.randn(nineq, nx)
    A = npr.randn(neq, nx)
    D = np.diag(npr.rand(nineq))

    K_ = np.bmat((
        (Q, np.zeros((nx, nineq)), G.T, A.T),
        (np.zeros((nineq, nx)), D, np.eye(nineq), np.zeros((nineq, neq))),
        (G, np.eye(nineq), np.zeros((nineq, nineq + neq))),
        (A, np.zeros((neq, nineq + nineq + neq)))
    ))

    Q_lo = sla.aslinearoperator(Q)
    G_lo = sla.aslinearoperator(G)
    A_lo = sla.aslinearoperator(A)
    D_lo = sla.aslinearoperator(D)

    K = block((
        (Q_lo,    0,    G.T,    A.T),
        (0,    D_lo,    'I',      0),
        (G_lo,  'I',      0,      0),
        (A_lo,    0,      0,      0)
    ), arrtype=sla.LinearOperator)

    w1 = np.random.randn(K_.shape[1])
    assert np.allclose(K_.dot(w1), K.dot(w1))
    w2 = np.random.randn(K_.shape[0])
    assert np.allclose(K_.T.dot(w2), K.H.dot(w2))
    W = np.random.randn(*K_.shape)
    assert np.allclose(K_.dot(W), K.dot(W))

def pad(mat, padrow, padcol):
    """
    Add additional rows/columns to a np.matrix `mat`. The new rows/columns
    will be initialized with zeros.
    """
    if padrow < 0:
        padrow = 0
    if padcol < 0:
        padcol = 0
    rows, cols = mat.shape
    return np.bmat([[mat, np.matrix(np.zeros((rows, padcol)))],
                      [np.matrix(np.zeros((padrow, cols + padcol)))]])

def doPhysics(rbd, force, torque, dtime):
    globalcom = rbd.rotmat.dot(rbd.com)+rbd.pos
    globalinertiatensor = rbd.rotmat.dot(rbd.inertiatensor).dot(rbd.rotmat.transpose())
    globalcom_hat = rm.hat(globalcom)
    # si = spatial inertia
    Isi00 = rbd.mass * np.eye(3)
    Isi01 = rbd.mass * globalcom_hat.transpose()
    Isi10 = rbd.mass * globalcom_hat
    Isi11 = rbd.mass * globalcom_hat.dot(globalcom_hat.transpose()) + globalinertiatensor
    Isi = np.bmat([[Isi00, Isi01], [Isi10, Isi11]])
    vw = np.bmat([rbd.linearv, rbd.angularw]).T
    pl = Isi*vw
    # print np.ravel(pl[0:3])
    # print np.ravel(pl[3:6])
    ft = np.bmat([force, torque]).T
    angularw_hat = rm.hat(rbd.angularw)
    linearv_hat = rm.hat(rbd.linearv)
    vwhat_mat = np.bmat([[angularw_hat, np.zeros((3,3))], [linearv_hat, angularw_hat]])
    dvw = Isi.I*(ft-vwhat_mat*Isi*vw)
    # print dvw
    rbd.dlinearv = np.ravel(dvw[0:3])
    rbd.dangularw = np.ravel(dvw[3:6])

    rbd.linearv = rbd.linearv + rbd.dlinearv * dtime
    rbd.angularw = rbd.angularw + rbd.dangularw * dtime

    return [np.ravel(pl[0:3]), np.ravel(pl[3:6])]

def _build_cov_mat_datapoints(self, axis):
        '''
        Builds the Cov_mat for the data points for the given axis. The cov_mat will take in account
        if 2 datasets are the same and correlate them, if self.corelate_datasets is True.


        '''
        dummy2 = []
        __querry_dummy2 = []
        for i,fit in enumerate(self.fit_list):
            # Create mask to store points where a non 0 matrix is needed.
            __querry = [False] * len(self.fit_list)
            # Diagonal entrys are never 0
            __querry[i] = True
            dummy2.append([0] * len(self.fit_list))
            # Check if datsets are correlated. If so change non diagonal elements.
            if self.corelate_datasets:
                if np.allclose(self.fit_list[i].dataset.get_data(axis), self.fit_list[i-1].dataset.get_data(axis),
                               atol=0, rtol=1e-4):
                    __querry[i-1] = True
            __querry_dummy2.append(__querry)

        for i,list in enumerate(dummy2):
            for j,entry in enumerate(list):
                if __querry_dummy2[i][j]:
                    dummy2[i][j] = self.fit_list[i].current_cov_mat
                else:
                    dummy2[i][j] = np.zeros((self.fit_list[i].dataset.get_size(), self.fit_list[j].dataset.get_size()))
        return np.bmat(dummy2)

def _get_weights(self, X, Y):
        """
        This private method, given the original control points and the deformed
        ones, returns the matrix with the weights and the polynomial terms, that
        is :math:`W`, :math:`c^T` and :math:`Q^T`. The shape is
        (n_control_points+1+3)-by-3.

        :param numpy.ndarray X: it is an n_control_points-by-3 array with the
            coordinates of the original interpolation control points before the
            deformation.
        :param numpy.ndarray Y: it is an n_control_points-by-3 array with the
            coordinates of the interpolation control points after the
            deformation.

        :return: weights: the matrix with the weights and the polynomial terms.
        :rtype: numpy.matrix
        """
        n_points = X.shape[0]
        dim = X.shape[1]
        identity = np.ones((n_points, 1))
        dist = self._distance_matrix(X, X)
        H = np.bmat([
            [dist, identity, X], 
            [identity.T, np.zeros((1, 1)), np.zeros((1, dim))],
            [X.T, np.zeros((dim, 1)), np.zeros((dim, dim))]
        ])
        rhs = np.bmat([[Y], [np.zeros((1, dim))], [np.zeros((dim, dim))]])
        weights = np.linalg.solve(H, rhs)
        return weights

def perform(self):
        """
        This method performs the deformation of the mesh points. After the
        execution it sets `self.modified_mesh_points`.
        """
        n_points = self.original_mesh_points.shape[0]
        dist = self._distance_matrix(
            self.original_mesh_points, self.parameters.original_control_points
        )
        identity = np.ones((n_points, 1))
        H = np.bmat([[dist, identity, self.original_mesh_points]])
        self.modified_mesh_points = np.asarray(np.dot(H, self.weights))

def pad(mat, padrow, padcol):
    """
    Add additional rows/columns to a numpy.matrix `mat`. The new rows/columns
    will be initialized with zeros.
    """
    if padrow < 0:
        padrow = 0
    if padcol < 0:
        padcol = 0
    rows, cols = mat.shape
    return numpy.bmat([[mat, numpy.matrix(numpy.zeros((rows, padcol)))],
                      [numpy.matrix(numpy.zeros((padrow, cols + padcol)))]])

def crossEntrGrad(y, trueY, G):
    k,n = G.shape
    assert(len(y) == n)
    y_ = np.copy(y)
    eps = 1e-8
    y_ = np.clip(y_, eps, 1.-eps)

    # H = np.bmat([[np.diag(1./y_ + 1./(1.-y_)), G.T, np.zeros((n,1))],
    #              [G, np.zeros((k,k)), -np.ones((k,1))],
    #              [np.zeros((1,n)), -np.ones((1,k)), np.zeros((1,1))]])

    # c = -np.linalg.solve(H, np.concatenate([trueY/y_-(1-trueY)/(1-y_), np.zeros(k+1)]))
    # b = np.concatenate([trueY/y_-(1-trueY)/(1-y_), np.zeros(k+1)])
    # cy, clam, ct = np.split(c, [n, n+k])
    # cy[(y == 0) | (y == 1)] = 0

    z = 1./y_ + 1./(1.-y_)
    zinv = 1./z
    G_zinv = G*zinv
    G_zinv_GT = np.dot(G_zinv, G.T)
    H = np.bmat([[G_zinv_GT, np.ones((k,1))], [np.ones((1,k)), np.zeros((1,1))]])
    dl = trueY/y_-(1-trueY)/(1-y_)
    b = np.concatenate([np.dot(G_zinv, dl), np.zeros(1)])
    clamt = np.linalg.solve(H, b)
    clam, ct = np.split(clamt, [k])
    cy = zinv*dl - np.dot((G*zinv).T, clam)
    cy[(y == 0) | (y == 1)] = 0
    return cy, clam, ct

def mseGrad_full(y, trueY, G):
    k,n = G.shape
    assert(len(y) == n)
    I = np.where((y > 1e-8) & (1.-y > 1e-8))
    z = np.ones_like(y)
    z[I] = (1./y[I] + 1./(1.-y[I]))
    H = np.bmat([[np.diag(z), G.T, np.zeros((n,1))],
                 [G, np.zeros((k,k)), -np.ones((k,1))],
                 [np.zeros((1,n)), -np.ones((1,k)), np.zeros((1,1))]])

    c = -np.linalg.solve(H, np.concatenate([(y - trueY), np.zeros(k+1)]))
    return np.split(c, [n, n+k])

def mseGrad(y, trueY, G):
    try:
        k,n = G.shape
    except:
        import IPython; IPython.embed(); sys.exit(-1)
    assert(len(y) == n)

    # y_ = np.copy(y)
    # eps = 1e-8
    # y_ = np.clip(y_, eps, 1.-eps)

    I = np.where((y > 1e-8) & (1.-y > 1e-8))
    # print(len(I[0]))
    z = np.ones_like(y)
    z[I] = (1./y[I] + 1./(1.-y[I]))
    z = 1./y + 1./(1.-y)
    zinv = 1./z
    G_zinv = G*zinv
    G_zinv_GT = np.dot(G_zinv, G.T)
    H = np.bmat([[G_zinv_GT, np.ones((k,1))], [np.ones((1,k)), np.zeros((1,1))]])

    # Different scaling than the MSE plots.
    dl = -(y-trueY)

    b = np.concatenate([np.dot(G_zinv, dl), np.zeros(1)])
    clamt = np.linalg.solve(H, b)
    clam, ct = np.split(clamt, [k])
    cy = zinv*dl - np.dot((G*zinv).T, clam)
    cy[(y == 0) | (y == 1)] = 0
    return cy, clam, ct

def pad(mat, padrow, padcol):
    """
    Add additional rows/columns to a numpy.matrix `mat`. The new rows/columns
    will be initialized with zeros.
    """
    if padrow < 0:
        padrow = 0
    if padcol < 0:
        padcol = 0
    rows, cols = mat.shape
    return numpy.bmat([[mat, numpy.matrix(numpy.zeros((rows, padcol)))],
                      [numpy.matrix(numpy.zeros((padrow, cols + padcol)))]])

def generate_data_ajive_fig2(seed=None):
    """
    Samples the data from AJIVE figure 2. Note here we use rows as observations
    i.e. data matrices are n x d where n = # observations.

    X_obs, X_joint, X_indiv, X_noise, Y_obs, Y_joint, Y_indiv, Y_noise =
    generate_data_ajive_fig2()
    """
    # TODO: return ndarray instead of matrix
    if seed:
        np.random.seed(seed)

    # Sample X data
    X_joint = np.bmat([[np.ones((50, 50))],
                      [-1*np.ones((50, 50))]])
    X_joint = 5000 * np.bmat([X_joint, np.zeros((100, 50))])

    X_indiv = 5000 * np.bmat([[-1 * np.ones((25, 100))],
                              [np.ones((25, 100))],
                              [-1 * np.ones((25, 100))],
                              [np.ones((25, 100))]])

    X_noise = 5000 * np.random.normal(loc=0, scale=1, size=(100, 100))

    X_obs = X_joint + X_indiv + X_noise

    # Sample Y data
    Y_joint = np.bmat([[-1 * np.ones((50, 2000))],
                       [np.ones((50, 2000))]])
    Y_joint = np.bmat([np.zeros((100, 8000)), Y_joint])

    Y_indiv_t = np.bmat([[np.ones((20, 5000))],
                        [-1 * np.ones((20, 5000))],
                        [np.zeros((20, 5000))],
                        [np.ones((20, 5000))],
                        [-1 * np.ones((20, 5000))]])

    Y_indiv_b = np.bmat([[np.ones((25, 5000))],
                        [-1 * np.ones((50, 5000))],
                        [np.ones((25, 5000))]])

    Y_indiv = np.bmat([Y_indiv_t, Y_indiv_b])

    Y_noise = np.random.normal(loc=0, scale=1, size=(100, 10000))

    Y_obs = Y_joint + Y_indiv + Y_noise

    # TODO: make this into a list of dicts i.e. hierarchical
    return X_obs, X_joint, X_indiv, X_noise, Y_obs, Y_joint, Y_indiv, Y_noise

def train(self):

        if (self.status != 'init'):
            print("Please load train data and init W first.")
            return self.W

        self.status = 'train'

        original_X = self.train_X[:, 1:]

        K = utility.Kernel.kernel_matrix(self, original_X)

        # P = Q, q = p, G = -A, h = -c

        P = cvxopt.matrix(np.bmat([[K, -K], [-K, K]]))
        q = cvxopt.matrix(np.bmat([self.epsilon - self.train_Y, self.epsilon + self.train_Y]).reshape((-1, 1)))
        G = cvxopt.matrix(np.bmat([[-np.eye(2 * self.data_num)], [np.eye(2 * self.data_num)]]))
        h = cvxopt.matrix(np.bmat([[np.zeros((2 * self.data_num, 1))], [self.C * np.ones((2 * self.data_num, 1))]]))
        # A = cvxopt.matrix(np.append(np.ones(self.data_num), -1 * np.ones(self.data_num)), (1, 2*self.data_num))
        # b = cvxopt.matrix(0.0)
        cvxopt.solvers.options['show_progress'] = False
        solution = cvxopt.solvers.qp(P, q, G, h)

        # Lagrange multipliers
        alpha = np.array(solution['x']).reshape((2, -1))
        self.alpha_upper = alpha[0]
        self.alpha_lower = alpha[1]
        self.beta = self.alpha_upper - self.alpha_lower

        sv = abs(self.beta) > 1e-5
        self.sv_index = np.arange(len(self.beta))[sv]
        self.sv_beta = self.beta[sv]
        self.sv_X = original_X[sv]
        self.sv_Y = self.train_Y[sv]

        free_sv_upper = np.logical_and(self.alpha_upper > 1e-5, self.alpha_upper < self.C)
        self.free_sv_index_upper = np.arange(len(self.alpha_upper))[free_sv_upper]
        self.free_sv_alpha_upper = self.alpha_upper[free_sv_upper]
        self.free_sv_X_upper = original_X[free_sv_upper]
        self.free_sv_Y_upper = self.train_Y[free_sv_upper]

        free_sv_lower = np.logical_and(self.alpha_lower > 1e-5, self.alpha_lower < self.C)
        self.free_sv_index_lower = np.arange(len(self.alpha_lower))[free_sv_lower]
        self.free_sv_alpha_lower = self.alpha_lower[free_sv_lower]
        self.free_sv_X_lower = original_X[free_sv_lower]
        self.free_sv_Y_lower = self.train_Y[free_sv_lower]

        short_b_upper = self.free_sv_Y_upper[0] - np.sum(self.sv_beta * utility.Kernel.kernel_matrix_xX(self, self.free_sv_X_upper[0], self.sv_X)) - self.epsilon
        short_b_lower = self.free_sv_Y_lower[0] - np.sum(self.sv_beta * utility.Kernel.kernel_matrix_xX(self, self.free_sv_X_lower[0], self.sv_X)) + self.epsilon

        self.sv_avg_b = (short_b_upper + short_b_lower) / 2

        return self.W