def test_random(self):
     n = 20
     for k in range(2):
         a = random([n,n])
         r,q = rq(a)
         assert_array_almost_equal(dot(transpose(q),q),identity(n))
         assert_array_almost_equal(dot(r,q),a)

 def test_random_complex(self):
     n = 20
     for k in range(2):
         a = random([n,n])+1j*random([n,n])
         r,q = rq(a)
         assert_array_almost_equal(dot(q, conj(transpose(q))),identity(n))
         assert_array_almost_equal(dot(r,q),a)

 def test_random_tall(self):
     m = 200
     n = 100
     for k in range(2):
         a = random([m,n])
         r,q = rq(a)
         assert_array_almost_equal(dot(q, transpose(q)),identity(n))
         assert_array_almost_equal(dot(r,q),a)

def factor(P):
    K, R = linalg.rq(P[:, :3])
    T = np.diag(np.sign(np.diag(K)))
    if linalg.det(T) < 0:
        T[1, 1] *= -1

    K = np.dot(K, T)
    R = np.dot(T, R)
    t = np.dot(linalg.inv(K), P[:, 3])
    return K, R, t

 def test_random_complex_economic(self):
     m = 100
     n = 200
     for k in range(2):
         a = random([m,n])+1j*random([m,n])
         r,q = rq(a, mode='economic')
         assert_array_almost_equal(dot(q,conj(transpose(q))),identity(m))
         assert_array_almost_equal(dot(r,q),a)
         assert_equal(q.shape, (m, n))
         assert_equal(r.shape, (m, m))

	def factor(self):
		""" Factorize the camera matrix into K,R,t as P = K[R|t]. """
		# factor first 3*3 part
		K,R = linalg.rq(self.P[:,:3])
		# make diagonal of K positive
		T = diag(sign(diag(K)))
		if linalg.det(T) < 0:
			T[1,1] *= -1
		self.K = dot(K,T)
		self.R = dot(T,R) # T is its own inverse
		self.t = dot(linalg.inv(self.K),self.P[:,3])
		return self.K, self.R, self.t

def factor(self):

    K,R = linalg.rq(self.P[:,:3])
    T = diag(sign(diag(K)))
    if linalg.det(T) < 0:
        T[1,1] *= -1

    self.K = dot(K,T)
    self.R = dot(T,R)
    self.t = dot(linalg.inv(self.K), self.P[:,:3])

    return self.K, self.R, self.t

 def factor(self):
     """Factorize the camera matrix into K, R, t as P=K[P|t]"""
     K,R = linalg.rq(self.P[:,:3])
     #make diagonal of K positive
     T = np.diag(np.sign(np.diag(K)))
     if linalg.det(T)<0:
         T[1,1]*=-1
         
     self.K = np.dot(K,T)
     self.R = np.dot(T,R)
     self.t = np.dot(linalg.inv(self.K),self.P[:,3])
     
     return self.K, self.R, self.t

 def factor(self):
     """ P = K[R|t]に従い、カメラ行列を　K,R,tに分解する　"""
     # 最初の3*3の部分を分解する
     K,R = linalg.rq(self.P[:,:3])
     
     # Kの対角成分が正になるようにする。
     T = np.diag(np.sign(np.diag(K)))
     if linalg.det(T) < 0:
         T[1,1] *= -1
         
     self.K = np.dot(K,T)
     self.R = np.dot(T,R) # Tはそれ自身が逆行列
     self.t = np.dot(linalg.inv(self.K),self.P[:,3])
         
     return self.K, self.R, self.t

def extract_KRt(P):
	"""Based on http://www.janeriksolem.net/2011/03/rq-factorization-of-camera-matrices.html"""
	H = P[0:3, 0:3]
	K, R = rq(H)
	T = np.diag(np.sign(np.diag(K)))
	K = np.dot(K,T)
	R = np.dot(T,R)
	t = np.dot(np.linalg.inv(K), P[:,-1])

	if np.linalg.det(R) < 0 :
		R = np.dot(R, -1 * np.eye(3))
		t = np.dot(-1 * np.eye(3), t)

	K = K/K[-1,-1]

	return K,R,t

    def retr(self, X, Z):
        XU, XS, XV = X
        ZUp, ZM, ZVp = Z
        Qu, Ru = la.qr(ZUp)
        Rv, Qv = rq(ZVp, mode='economic')

        zero_block = np.zeros((Ru.shape[0], Rv.shape[1]))
        block_mat = np.array(np.bmat([[XS + ZM, Rv],
                                     [Ru, zero_block]]))

        Ut, St, Vt = la.svd(block_mat, full_matrices=False)

        U = np.hstack((XU, Qu)).dot(Ut[:, :self._k])
        V = Vt[:self._k, :].dot(np.vstack((XV, Qv)))
        # add some machinery eps to get a slightly perturbed element of a manifold
        # even if we have some zeros in S
        S = np.diag(St[:self._k]) + np.diag(np.spacing(1) * np.ones(self._k))
        return (U, S, V)

    def factor(self, K=None):
        """    Factorize the camera matrix into K,R,t as P = K[R|t]. """
        if K is not None:
            # factor first 3*3 part
            K,R = linalg.rq(self.P[:,:3])

            # make diagonal of K positive
            T = diag(sign(diag(K)))
            if linalg.det(T) < 0:
                T[1,1] *= -1

            self.K = dot(K,T)
            self.R = dot(T,R) # T is its own inverse
            self.t = dot(linalg.inv(self.K),self.P[:,3])

            return self.K, self.R, self.t
        else:
            print("K")
            print(self.K)
            print("Kinv")
            Kinv = linalg.inv(self.K)
            print(Kinv)
            Pose = dot(Kinv, self.P)

def RDU(A): # Calculate the RDU decomposition of a matrix {{{
    r,U = rq(A)
    d = numpy.diagonal(r)
    R = r / d[newaxis,:]
    D = diagflat(d)
    return R,D,U #}}}

    def decomposeQR(self, gl=None, order='qr'):
        """
        Decomposes gl using QR decomposition into:

          gl = q p s m (order 'qr' or 'qpsm') 
          gl = p s m q (order 'rq' or 'psmq')

        where:
          - q: rotation (orthogonal, with det +1) matrix
          - p: parity (diagonal, all elements +1, except that the element 
          corresponding to self.parity_axismatrix can be -1)
          possibly -1
          - s: scale martix, diagonal and positive
          - m: shear matrix, upper triangular, all diagonal elements 1

        Arguments:
          - gl: (ndarray) general linear transformation
          - order: decomposition order 'qr' or 'rq'

        Returns: (q, p, s, m)
        """

        # set decomposition type
        self.order = order

        # parse arg
        if gl is None:
            gl = self.gl
        ndim = gl.shape[0]

        # QR decompose 
        if (order == 'rq') or (order == 'psmq'):
            r, q = linalg.rq(gl)
        elif (order == 'qr') or (order == 'qpsm'):
            q, r = linalg.qr(gl)
        else:
            ValueError("Argumnet order: ", order, " not understood. It should ",
                       "be 'psmq' (same as 'rq') or 'qpsm' (same as 'qr').")

        # extract s, p and m
        r_diag = r.diagonal()
        s_diag = numpy.abs(r_diag)
        s = numpy.diag(s_diag)
        p_diag = numpy.sign(r_diag)
        p = numpy.diag(p_diag)
        s_inv_diag = 1. * p_diag / s_diag
        m = numpy.dot(numpy.diag(s_inv_diag), r)

        # make q = q p and p = 1
        if (order == 'rq') or (order == 'psmq'):
            m = numpy.dot(numpy.dot(p, m), p)
            q = numpy.dot(p, q)
        elif (order == 'qr') or (order == 'qpsm'):
            q = numpy.dot(q, p)
        p = numpy.abs(p)

        # make sure det(q) > 0 and adjust p accordingly
        if linalg.det(q) < 0:
            p = numpy.identity(ndim, dtype=int)
            p[self.parity_axis, self.parity_axis] = -1
            if (order == 'rq') or (order == 'psmq'):
                q = numpy.dot(p, q)
                m = numpy.dot(numpy.dot(p, m), p)
            elif (order == 'qr') or (order == 'qpsm'):
                q = numpy.dot(q, p)

        return q, p, s, m

 def test_simple(self):
     a = [[8,2,3],[2,9,3],[5,3,6]]
     r,q = rq(a)
     assert_array_almost_equal(dot(transpose(q),q),identity(3))
     assert_array_almost_equal(dot(r,q),a)

def state2MPS(state,sitedim,l,method='qr',tol=1e-8):
    '''
    Parse a normal state into a Matrix produdct state.

    state:
        The target state, 1D array.
    sitedim:
        The dimension of a single site, integer.
    l:
        The division point of left and right canonical scanning, integer between 0 and number of site.
    method:
        The method to extract A,B matrices.
        * 'qr'  -> get A,B matrices by the method of QR decomposition, faster, rank revealing in a non-straight-forward way.
        * 'svd'  -> get A,B matrices by the method of SVD decomposition, slow, rank revealing.
    tol:
        The tolerence of singular value, float.

    *return*:
        A <MPS> instance.
    '''
    nsite=int(round(log(len(state))/log(sitedim)))
    AL,BL=[],[]
    ri=1
    assert(method=='svd' or method=='qr')
    assert(l>=0 and l<=nsite)

    for i in xrange(l):
        state=state.reshape([sitedim*ri,-1])
        if method=='svd':
            U,S,V=svd(state,full_matrices=False)
            #remove zeros from v
            kpmask=abs(S)>tol
            ri=kpmask.sum()
            state=S[kpmask,newaxis]*V[kpmask]
            U=U[:,kpmask]
        else:
            U,state=qr(state,mode='economic')
            kpmask=sum(abs(state),axis=1)>tol
            ri=kpmask.sum()
            state=state[kpmask]
            U=U[:,kpmask]
        ai=swapaxes(U.reshape([-1,sitedim,ri]),0,1)
        AL.append(ai)

    ri=1
    for i in xrange(nsite-l):
        state=state.reshape([-1,sitedim*ri])
        if method=='svd':
            U,S,V=svd(state,full_matrices=False)
            #remove zeros from v
            kpmask=abs(S)>tol
            ri=kpmask.sum()
            state=S[kpmask]*U[:,kpmask]
            V=V[kpmask,:]
        else:
            state,V=rq(state,mode='economic')
            kpmask=sum(abs(state),axis=0)>tol
            ri=kpmask.sum()
            state=state[:,kpmask]
            V=V[kpmask]
        bi=swapaxes(V.reshape([ri,sitedim,-1]),0,1)
        BL.append(bi)
    BL=BL[::-1]
    S=state.diagonal()
    return KMPS(AL,BL,S=S)

 def test_simple_complex(self):
     a = [[3,3+4j,5],[5,2,2+7j],[3,2,7]]
     r,q = rq(a)
     assert_array_almost_equal(dot(q, conj(transpose(q))),identity(3))
     assert_array_almost_equal(dot(r,q),a)

 def test_simple_tall(self):
     a = [[8,2],[2,9],[5,3]]
     r,q = rq(a)
     assert_array_almost_equal(dot(transpose(q),q),identity(2))
     assert_array_almost_equal(dot(r,q),a)

 def test_r(self):
     a = [[8,2,3],[2,9,3],[5,3,6]]
     r,q = rq(a)
     r2 = rq(a, mode='r')
     assert_array_almost_equal(r, r2)