def better_rec(self, w, model, s=1, weights=1, damp_z=1):
     """Quick switch to allow reconstruction at unknown scale
     returns a,r and scale"""
     from numpy.core.umath_tests import matrix_multiply
     proj = matrix_multiply(self.cam[np.newaxis], model)
     proj[:, :2] = (proj[:, :2] * s + w * weights) / (s + weights)
     proj[:, 2] *= damp_z
     out = matrix_multiply(self.cam.T[np.newaxis], proj)
     return out

    def test_gufunc_new_axis(self):

        @guvectorize([void(float64[:, :], float64[:, :], float64[:, :])],
                     '(m,n),(n,p)->(m,p)',
                     target='cuda')
        def matmulcore(A, B, C):
            m, n = A.shape
            n, p = B.shape
            for i in range(m):
                for j in range(p):
                    C[i, j] = 0
                    for k in range(n):
                        C[i, j] += A[i, k] * B[k, j]

        gufunc = matmulcore

        X = np.random.randn(10, 3, 3)
        Y = np.random.randn(3, 3)

        gold = ut.matrix_multiply(X, Y)

        res1 = gufunc(X, Y)
        np.testing.assert_allclose(gold, res1)

        res2 = gufunc(X, np.tile(Y, (10, 1, 1)))
        np.testing.assert_allclose(gold, res2)

    def test_gufunc_auto_transfer(self):

        @guvectorize([void(float32[:, :], float32[:, :], float32[:, :])],
                     '(m,n),(n,p)->(m,p)',
                     target='cuda')
        def matmulcore(A, B, C):
            m, n = A.shape
            n, p = B.shape
            for i in range(m):
                for j in range(p):
                    C[i, j] = 0
                    for k in range(n):
                        C[i, j] += A[i, k] * B[k, j]

        gufunc = matmulcore
        gufunc.max_blocksize = 512

        matrix_ct = 2
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2,
                                                                   4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4,
                                                                   5)

        dB = cuda.to_device(B)

        C = gufunc(A, dB).copy_to_host()
        Gold = ut.matrix_multiply(A, B)
        self.assertTrue(np.allclose(C, Gold))

    def test_gufunc(self):

        @guvectorize([void(float32[:, :], float32[:, :], float32[:, :])],
                     '(m,n),(n,p)->(m,p)',
                     target='cuda')
        def matmulcore(A, B, C):
            m, n = A.shape
            n, p = B.shape
            for i in range(m):
                for j in range(p):
                    C[i, j] = 0
                    for k in range(n):
                        C[i, j] += A[i, k] * B[k, j]

        gufunc = matmulcore
        gufunc.max_blocksize = 512

        matrix_ct = 1001 # an odd number to test thread/block division in CUDA
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2,
                                                                   4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4,
                                                                   5)

        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)
        self.assertTrue(np.allclose(C, Gold))

    def test_gufunc_stream(self):
        #cuda.driver.flush_pending_free()
        matrix_ct = 1001 # an odd number to test thread/block division in CUDA
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2,
                                                                   4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4,
                                                                   5)

        ts = time()
        stream = cuda.stream()
        dA = cuda.to_device(A, stream)
        dB = cuda.to_device(B, stream)

        dC = cuda.device_array(shape=(1001, 2, 5), dtype=A.dtype, stream=stream)
        dC = gufunc(dA, dB, out=dC, stream=stream)
        C = dC.copy_to_host(stream=stream)
        stream.synchronize()

        tcuda = time() - ts

        ts = time()
        Gold = ut.matrix_multiply(A, B)
        tcpu = time() - ts

        stream_speedups.append(tcpu / tcuda)

        self.assertTrue(np.allclose(C, Gold))

def _map_params_to_P_zero(params, params_type, initial, params_slice, filler,
                          boo, cholesky_of_P_zero,
                          square_root_filters):
    """Map parameters from params to P_zero."""
    # write params in filler
    filler[:] = 0
    filler[boo] = params[params_slice]

    # transform the filler
    if params_type == 'short' or cholesky_of_P_zero is True:
        if square_root_filters is False:
            # make chol_t to not chol
            filler = matrix_multiply(
                np.transpose(filler, axes=(0, 2, 1)), filler)
    else:
        # make not_chol to not_chol (as covariance matrices are symmetric,
        # only half of its off-diagonal elements have to be estimated. here the
        # lower triangle is filled with he transpose of the upper triangle.)
        for i in range(len(filler)):
            filler[i] += (filler[i] - np.diag(np.diagonal(filler[i]))).T

        if square_root_filters is True:
            # make not_chol to chol_t
            filler = np.transpose(cholesky(filler), axes=(0, 2, 1))

    if square_root_filters is False:
        initial[:] = filler
    else:
        initial[:, :, 1:, 1:] = filler

    def check_matmul_gufunc(self, gufunc):
        matrix_ct = 1001
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2, 4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4, 5)

        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)

        self.assertTrue(np.allclose(C, Gold))

    def check_matmul_gufunc(self, gufunc):
        matrix_ct = 1001
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2, 4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4, 5)

        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)

        np.testing.assert_allclose(C, Gold, rtol=1e-5, atol=1e-8)

    def build_and_rot_model(a, e, s0, r):
        """
        Build model and rotate according to the identified rotation matrix
        """
        from numpy.core.umath_tests import matrix_multiply

        r2 = Prob3dPose.upgrade_r(r.T).transpose((0, 2, 1))
        mod = Prob3dPose.build_model(a, e, s0)
        mod = matrix_multiply(r2, mod)
        return mod

def transform_points_with_homography(H, _xys):
    """
    Args:
        H (ndarray[float64_t, ndim=2]):  homography/perspective matrix
        _xys (ndarray[ndim=2]): (N x 2) array
    """
    xyz  = add_homogenous_coordinate(_xys)
    xyz_t = matrix_multiply(H, xyz)
    xy_t  = remove_homogenous_coordinate(xyz_t)
    return xy_t

def x_to_y(matFn, date, vecs, reverse=False):
    vecs = np.asarray(vecs)
    assert vecs.ndim == 2 
    assert vecs.shape[1] == 3
        
    et = date2es(date)
    mat = matFn(et)
    if reverse:
        mat = mat.T
    vecsOut = matrix_multiply(mat, vecs[...,np.newaxis]).reshape(vecs.shape)
    return vecsOut

    def test_gufunc_adjust_blocksize(self):
        matrix_ct = 1001 # an odd number to test thread/block division in CUDA
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2,
                                                                   4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4,
                                                                   5)

        gufunc.max_blocksize = 32
        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)
        self.assertTrue(np.allclose(C, Gold))

    def compare_matrix_multiply_results(self, tp):
        d1 = np.array(np.random.rand(2, 3, 4), dtype=tp)
        d2 = np.array(np.random.rand(2, 3, 4), dtype=tp)
        msg = "matrix multiply on type %s" % d1.dtype.name

        def permute_n(n):
            if n == 1:
                return ([0],)
            ret = ()
            base = permute_n(n-1)
            for perm in base:
                for i in range(n):
                    new = perm + [n-1]
                    new[n-1] = new[i]
                    new[i] = n-1
                    ret += (new,)
            return ret

        def slice_n(n):
            if n == 0:
                return ((),)
            ret = ()
            base = slice_n(n-1)
            for sl in base:
                ret += (sl+(slice(None),),)
                ret += (sl+(slice(0, 1),),)
            return ret

        def broadcastable(s1, s2):
            return s1 == s2 or s1 == 1 or s2 == 1

        permute_3 = permute_n(3)
        slice_3 = slice_n(3) + ((slice(None, None, -1),)*3,)

        ref = True
        for p1 in permute_3:
            for p2 in permute_3:
                for s1 in slice_3:
                    for s2 in slice_3:
                        a1 = d1.transpose(p1)[s1]
                        a2 = d2.transpose(p2)[s2]
                        ref = ref and a1.base is not None
                        ref = ref and a2.base is not None
                        if (a1.shape[-1] == a2.shape[-2] and
                                broadcastable(a1.shape[0], a2.shape[0])):
                            assert_array_almost_equal(
                                umt.matrix_multiply(a1, a2),
                                np.sum(a2[..., np.newaxis].swapaxes(-3, -1) *
                                       a1[..., np.newaxis,:], axis=-1),
                                err_msg=msg + ' %s %s' % (str(a1.shape),
                                                          str(a2.shape)))

        assert_equal(ref, True, err_msg="reference check")

 def average(self):
     
     if len(self.shape) == 1:
         
         import numpy.core.umath_tests as ut
         system = ut.matrix_multiply(self.qs[:,:,np.newaxis], self.qs[:,np.newaxis,:]).sum(axis=0)
         w, v = np.linalg.eigh(system)
         qiT_dot_qref = (self.qs[:,:,np.newaxis] * v[np.newaxis,:,:]).sum(axis=1)
         return Quaternions(v[:,np.argmin((1.-qiT_dot_qref**2).sum(axis=0))])            
     
     else:
         
         raise NotImplementedError('Cannot average multi-dimensionsal Quaternions')

    def test_gufunc(self):
        gufunc = GUVectorize(matmulcore, '(m,n),(n,p)->(m,p)', target='cpu')
        gufunc.add(argtypes=[float32[:, :], float32[:, :], float32[:, :]])
        gufunc = gufunc.build_ufunc()

        matrix_ct = 1001 # an odd number to test thread/block division in CUDA
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2, 4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4, 5)

        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)

        self.assertTrue(np.allclose(C, Gold))

    def test_gufunc(self):
        gufunc = GUVectorize(matmulcore, '(m,n),(n,p)->(m,p)',
                             target=self.target)
        gufunc.add((float32[:, :], float32[:, :], float32[:, :]))
        gufunc = gufunc.build_ufunc()

        matrix_ct = 1001
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2, 4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4, 5)

        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)

        self.assertTrue(np.allclose(C, Gold))

def gmm(k, xs, tol=1e-6, max_iter=200):
    """Vectorized version of GMM. Faster than above but still rough."""
    
    n, p = xs.shape
    
    mus, z = initialization.kmeanspp(k, xs, ret='both')
    pis = np.array([len(np.where(z==i)[0])/n for i in np.unique(z)])
    sigmas = np.array([np.eye(p)]*k)

    ll_old = 0
    for i in range(max_iter):
        exp_A = []
        exp_B = []
        ll_new = 0

        # E-step, ws are responsabilities
        ws = np.zeros((k, n))
        for j in range(k):
            ws[j, :] = pis[j]*multivariate_normal(mus[j], sigmas[j]).pdf(xs)
        ws /= ws.sum(0)
            
        # M-step
        pis = ws.sum(axis=1)
        pis /= n

        mus = np.dot(ws, xs)
        mus /= ws.sum(1)[:, None]

        sigmas = np.zeros((k, p, p))
        for j in range(k):
            ys = xs - mus[j, :]
            sigmas[j] = (ws[j,:,None,None]*\
                       matrix_multiply(ys[:,:,None], ys[:,None,:])).sum(axis=0)
        sigmas /= ws.sum(axis=1)[:,None,None]

        # update complete log likelihoood
        ll_new = 0
        for pi, mu, sigma in zip(pis, mus, sigmas):
            ll_new += pi*multivariate_normal(mu, sigma).pdf(xs)
        ll_new = np.log(ll_new).sum()

        # convergence test
        if np.abs(ll_new - ll_old) < tol:
            break
        ll_old = ll_new

    z = ws.T
    labels = np.argmax(z, axis=1)

    return labels

    def test_cpu_guvectorize(self):
        target = 'cpu'

        gufunc = guvectorize([void(float32[:,:], float32[:,:], float32[:,:])],
                             '(m,n),(n,p)->(m,p)',
                             target=target)(matmulcore)

        matrix_ct = 1001 # an odd number to test thread/block division in CUDA
        A = np.arange(matrix_ct * 2 * 4, dtype=np.float32).reshape(matrix_ct, 2, 4)
        B = np.arange(matrix_ct * 4 * 5, dtype=np.float32).reshape(matrix_ct, 4, 5)

        C = gufunc(A, B)
        Gold = ut.matrix_multiply(A, B)

        self.assertTrue(np.allclose(C, Gold))

 def flux_down(self, fluxDownTop, emission=None):
     '''Compute downwelling radiative flux at interfaces between layers.
     
     Inputs:
         fluxDownTop: flux down at top
         emission: emission from atmospheric levels (N)
             defaults to zero if not given
     Returns:
         vector of downwelling radiative flux between levels (N+1)
         element 0 is the flux down to the surface.'''
     if emission is None:
         emission = np.zeros_like(self.absorptivity)
     E = np.concatenate((emission, np.atleast_1d(fluxDownTop)), axis=-1)
     #  dot product (matrix multiplication) along last axes
     return np.squeeze(matrix_multiply(self.Tdown, E[..., np.newaxis]))

 def flux_up(self, fluxUpBottom, emission=None):
     '''Compute upwelling radiative flux at interfaces between layers.
     
     Inputs:
         fluxUpBottom: flux up from bottom
         emission: emission from atmospheric levels (N)
             defaults to zero if not given
     Returns:
         vector of downwelling radiative flux between levels (N+1)
         element N is the flux up to space.'''        
     if emission is None:
         emission = np.zeros_like(self.absorptivity)
     E = np.concatenate((np.atleast_1d(fluxUpBottom),emission), axis=-1)
     #  dot product (matrix multiplication) along last axes
     return np.squeeze(matrix_multiply(self.Tup, E[..., np.newaxis]))