def decompose_camera_matrix(P: np.ndarray) -> (np.ndarray, np.ndarray): ''' Decomposes the camera matrix into the K intrinsic and R rotation matrix Args: - P: 3x4 numpy array projection matrix Returns: - K: 3x3 intrinsic matrix (numpy array) - R: 3x3 orthonormal rotation matrix (numpy array) hint: use scipy.linalg.rq() ''' ############################## # TODO: Student code goes here # Get first three columns of P M = P[:, :3] K, R = rq(M) ############################## return K, R
def decompose_camera_matrix(P: np.ndarray) -> (np.ndarray, np.ndarray): ''' Decomposes the camera matrix into the K intrinsic and R rotation matrix Args: - P: 3x4 numpy array projection matrix Returns: - K: 3x3 intrinsic matrix (numpy array) - R: 3x3 orthonormal rotation matrix (numpy array) hint: use scipy.linalg.rq() ''' K = None R = None ############################## # TODO: Student code goes here M = P[:, 0:3] K, R = rq(M) # raise NotImplementedError ############################## return K, R
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 calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter): max_inliers = 0 p = get_p(K_matrix, positions) P = get_P(end_effector_positions) for i in range(0, 50000): # Sample 6 points indices = np.random.randint(0, noiter, size=6) M = get_projection_matrix(p[indices, :], P[indices, :]) # Calculate reprojection error homogeneous_P = np.hstack((P, np.ones((P.shape[0], 1)))) estimated_p = np.matmul(M, homogeneous_P.T) estimated_p = estimated_p / estimated_p[2, :] error = p - estimated_p[0:2, :].T error = error[:, 0] * error[:, 0] + error[:, 1] * error[:, 1] # Find inliers inlier_indices = np.unique(np.where(np.abs(error) < 3)) noinliers = np.sum(np.abs(error) < 2) if noinliers > max_inliers: max_inliers = noinliers final_inliers = inlier_indices final_M = get_projection_matrix(p[final_inliers, :], P[final_inliers, :]) # RQ decomposition K, R = linalg.rq(final_M[:, 0:3]) rvec = forward_kinematics_extrinsics.forwardKinematics.getEulerAngles( np.linalg.inv(R)) tvec = np.matmul(np.linalg.inv(K), final_M[:, 3]) H = np.vstack((np.hstack((R, tvec.reshape( (3, 1)))), np.array([[0, 0, 0, 1]]))) print("Using PnP Algorithm without non-linear optimization ") print("Orientation : {0}".format(rvec)) print("Position : {0}".format(tvec)) return np.linalg.inv(H)
def decompose_camera_matrix(P: np.ndarray) -> (np.ndarray, np.ndarray): ''' Decomposes the camera matrix into the K intrinsic and R rotation matrix Args: - P: 3x4 numpy array projection matrix Returns: - K: 3x3 intrinsic matrix (numpy array) - R: 3x3 orthonormal rotation matrix (numpy array) hint: use scipy.linalg.rq() ''' #print("P is", P) M = np.delete(P, 3, 1) #delete 4th column of P #print("dimensions",len(M.shape)) K, R = rq(M) ############################## # TODO: Student code goes here # raise NotImplementedError ############################## return K, R
def decompose_projection(P): """Decompose the projection matrix into K, R, C""" M = P[:, 0:3] MC = -1 * P[:, 3] C = np.dot(np.linalg.inv(M), MC) K, R = linalg.rq(M) return K, R, C
def decomposeCameraMatrix(P): p1 = P[:, [0]] p2 = P[:, [1]] p3 = P[:, [2]] p4 = P[:, [3]] M = np.hstack((p1, p2, p3)) X = np.linalg.det(np.hstack((p2, p3, p4))) Y = -np.linalg.det(np.hstack((p1, p3, p4))) Z = np.linalg.det(np.hstack((p1, p2, p4))) T = -np.linalg.det(np.hstack((p1, p2, p3))) # camera centre C = ut.h2a(np.array((X, Y, Z, T)).reshape(4, 1)) K, R = la.rq(M) K = K / np.abs(K[2, 2]) if K[0, 0] < 0: D = np.diag([-1, -1, 1]) K = K.dot(D) R = D.dot(R) if K[1, 1] < 0: D = np.diag([1, -1, -1]) K = K.dot(D) R = D.dot(R) if np.dot(np.cross(R[:, 0], R[:, 1]), R[:, 2]) < 0: print('Warning: R is left handed') return K, R, C
def decompose_proj_mat(P): ''' Decompose projection matrix into calib mat A, rotation R and translation t''' ''' # BUGGY METHOD B = P[:, 0:-1] b = P[:, -1] K = B.dot(B.T) scale = K[2, 2] K = K / K[2, 2] # normalize A = np.zeros((3, 3)) A[0, 2] = K[0, 2] # u0 A[1, 2] = K[1, 2] # v0 A[1, 1] = np.sqrt(K[1, 1] - A[1, 2]**2) # fy A[0, 1] = (K[1, 0] - (A[0, 2]*A[1, 2]))/A[1, 1] # shear A[0, 0] = np.sqrt(K[0, 0] - A[0, 2]**2 - A[0, 1]**2) # fx A[2, 2] = 1.0 R = np.linalg.solve(A, B) t = np.linalg.solve(A, b) return A, R, t, scale ''' # use instead RQ transform K, R = linalg.rq(P[:, :-1]) t = linalg.solve(K, P[:, -1]) # scale = K[-1, 1] return K, R, t
def decompose_camera_matrix(P: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: ''' Decomposes the camera matrix into the K intrinsic and R rotation matrix Args: - P: 3x4 numpy array projection matrix Returns: - K: 3x3 intrinsic matrix (numpy array) - R: 3x3 orthonormal rotation matrix (numpy array) hint: use scipy.linalg.rq() ''' K = None R = None ############################################################################ # TODO: YOUR CODE HERE # First three cols correspond to M M = P[:, :3] # RQ decoposition yields upper triangular matrix R & orthonormal matrix Q R, Q = rq(M) # Assign each to K & R K = R R = Q ############################################################################ #raise NotImplementedError('`decompose_camera_matrix` function in ' # + 'projection_matrix.py needs to be implemented') ############################################################################ # END OF YOUR CODE ############################################################################ return K, R
def factor(self): """ Factorize the camera into: K: Camera intrinsics t: Camera position (or translation) R: Camera pose (or Rotation) P = K[R|t] see: http://www.janeriksolem.net/2011/03/rq-factorization-of-camera-matrices.html """ # factor for first 3*3 part K, R = linalg.rq(self.P[:, :3]) #print "camera matrix: ", self.P # make diagonal of K positive # because focal length is positive T = np.diag(np.sign(np.diag(K))) self.K = np.dot(K, T) self.fx = K[0, 0] self.fy = K[1, 1] self.R = np.dot(T, R) # T is its own inverse self.t = np.dot(linalg.inv(self.K), self.P[:, 3]) return self.K, self.R, self.t
def P2KRt(P): """Decomposition of projection matrix into K,R,t. Example: >>> K = np.matrix([[1,0,50],[0,2,50],[0,0,1]]) >>> R = np.matrix([[-1,0,0],[0,1,0],[0,0,-1]]) >>> t = np.matrix([10,-10,0]) >>> P = K*np.hstack((R,t.T)) >>> K_,R_,t_ = P2KRt(P) >>> np.all(K_ == K) True >>> np.all(R_ == R) True >>> np.all(t_ == t) True """ K,R = rq(P[:,0:3]) # Fix signs T = np.diag(np.sign(np.diag(K))) K = np.dot(K,T) R = np.dot(T,R) t = np.linalg.inv(K) * P[:,3] return K,R,t.T
def P_to_KRt(P): ''' This function computes the decomposition of the projection matrix into intrinsic parameters, K, and extrinsic parameters Q (the rotation matrix) and t (the translation vector) Usage: K, Q, t = P_to_KRt(P) Input: P: 3x4 projection matrix Outputs: K: 3x3 camera intrinsics Q: 3x3 rotation matrix (extrinsics) t: 3x1 translation vector(extrinsics) ''' M = P[0:3, 0:3] R, Q = rq(M) # R, Q = scipy.linalg.rq(M) K = R / float(R[2, 2]) if K[0, 0] < 0: K[:, 0] = -1 * K[:, 0] Q[0, :] = -1 * Q[0, :] if K[1, 1] < 0: K[:, 1] = -1 * K[:, 1] Q[1, :] = -1 * Q[1, :] if np.linalg.det(Q) < 0: print('Warning: Determinant of the supposed rotation matrix is -1') P_3_3 = np.dot(K, Q) P_proper_scale = (P_3_3[0, 0] * P) / float(P[0, 0]) t = np.dot(np.linalg.inv(K), P_proper_scale[:, 3]) return K, Q, t
def decompose_camera_matrix(P: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: ''' Decomposes the camera matrix into the K intrinsic and R rotation matrix Args: - P: 3x4 numpy array projection matrix Returns: - K: 3x3 intrinsic matrix (numpy array) - R: 3x3 orthonormal rotation matrix (numpy array) hint: use scipy.linalg.rq() ''' K = None R = None ############################## # TODO: Student code goes here K, R = rq(P[:, :3]) ############################## return K, R
def test_random(self): n = 20 for k in range(2): a = random([n, n]) r, q = rq(a) assert_array_almost_equal(dot(q, transpose(q)), identity(n)) assert_array_almost_equal(dot(r, q), a)
def gaugeL(self, R, cutD): """ Similar to gaugeR, but towards the opposite direction. """ self.A = np.einsum('ijk,kl->ijl', self.A, R) self.Dr = self.A.shape[2] L = 0 if (self.Dl == 1): L = LA.norm(self.A) self.A = self.A / L L = np.array([[L]]) else: aux = np.swapaxes(self.A, 0, 1) aux = aux.reshape((self.Dl, self.s * self.Dr)) if (cutD == 0): L, Q = LA.rq(aux) self.A = np.swapaxes(Q.reshape((-1, self.s, self.Dr)), 0, 1) else: U, S, Vdag = LA.svd(aux, full_matrices=False) if (S.shape[0] > cutD): U = U[:, 0:cutD] S = S[0:cutD] Vdag = Vdag[0:cutD, :] L = np.einsum('ij,j->ij', U, S) self.A = np.swapaxes(Vdag.reshape((-1, self.s, self.Dr)), 0, 1) self.Dl = self.A.shape[1] return L
def DLT(self, imagePoints, groundPoints): """ compute exterior and inner orientation using direct linear transformations""" # change to homogeneous representation groundPoints = np.hstack((groundPoints, np.ones((len(groundPoints), 1)))) imagePoints = np.hstack((imagePoints, np.ones((len(imagePoints), 1)))) # compute design matrix a = self.ComputeDLTDesignMatrix(imagePoints, groundPoints) # compute eigenvalues and eigenvectors w, v = np.linalg.eig(np.dot(a.T, a)) # the solution is the eigenvector of the minimal eigenvalue p = v[:, np.argmin(w)] p = np.reshape(p, (3, 4)) k, r = rq(p[:3, :3]) k = k/np.abs(k[2,2]) # normalize # handle signs signMat = findSignMat(k) k = np.dot(k, signMat) r = np.dot(np.linalg.inv(signMat), r) # update orientation self.RotationMatrix = r.T self.PerspectiveCenter = -np.dot(inv(p[:3,:3]),p[:,3]) # update calibration self.camera.principalPoint = k[:2, 2] self.camera.focalLength = -k[0,0]
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 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 cameraMatrix(P): M = P[:, :3] K, R = linalg.rq(M, mode='full') C = -1 * np.matmul(np.linalg.inv(M), P[:, 3]) extrinsic = np.zeros((3, 4)) extrinsic[0:3, 0:3] = R extrinsic[:, 3] = -1 * np.matmul(R, C) return K, extrinsic
def QR_decomposition(matrix_P): # Extract P3x3 and do its QR decomposition as P3x3 = KR matrix = matrix_P[0:3, 0:3] K, R = linalg.rq(matrix) K = K / float(K[2, 2]) inv = np.linalg.inv(K) # Calculate Translation vector T = np.matmul(-inv, matrix_P[:, 3]) return K, R, T
def factor(self): K, R = linalg.rq(self.P[:, :3]) T = np.diag(np.sign(np.diag(K))) #K,Rの行列式を正にする self.K = np.dot(K, T) self.R = np.dot(T, R) self.t = np.linalg.inv(self.K) @ self.P[:, 3] return self.K, self.R, self.t
def decomp_projection_matrix(P): K, R = linalg.rq(P[:,:-1]) s = np.diag(np.sign(np.diag(K))) K = K.dot(s) R = R.dot(s) # print(s) T = np.linalg.inv(K).dot(P[:,-1]) C = R.T.dot(-T) return K, R, T, C
def factor(self): K, R = linalg.rq(self.P[:, :3]) T = geek.diag(geek.sign(geek.diag(K))) T[1, 1] *= -1 self.K = geek.dot(K, T) self.R = geek.dot(T, R) self.t = geek.dot(linalg.inv(self.K), self.P[:, 3]) return self.K, self.R, self.t
def test_decomposeCameraMatrix(): for i in range(0, 10000): M = np.random.rand(3, 3) K, R = la.rq(M) C = np.random.rand(3, 1) P = K.dot(np.hstack((R, -R.dot(C)))) Kr, Rr, Cr = camera.decomposeCameraMatrix(P) Pr = Kr.dot(np.hstack((Rr, -Rr.dot(Cr)))) assert (np.all((P - Pr) < eps)) & (np.all((K - Kr) < eps)) & (np.all( (R - Rr) < eps)) & (np.all((C - Cr) < eps))
def factor(P): K, R = rq(P[:, :3]) T = np.diag(np.sign(np.diag(K))) if np.linalg.det(T) < 0: T[1, 1] *= -1 K = np.dot(K, T) R = np.dot(T, R) t = np.dot(np.linalg.inv(K), P[:, 3]) c = -np.dot(R.T, t) return K, R, t, c
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(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): ''' to be used when P is given ''' K, R = linalg.rq(self.P[:, :3]) 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 calibrateCamera_Tsai(p, P): # DLT using Tsai's method # INPUT # p : homogeneous coordinates of pixel in the image frame # P : homogeneous coordinates of points in the world frame # OUTPUT # K, R, T, M assert p.shape[ 0] == 3, "p : homogeneous coordinates of pixel in the image frame of 3 by n" assert P.shape[ 0] == 4, "P : homogeneous coordinates of points in the world frame" assert p.shape[1] == P.shape[ 1], "number of columns of p shold match with P" n = p.shape[1] p_uv = p[0:2, :] / p[2, :] Q = np.empty((0, 12)) for i in range(n): Qi_0 = np.array([[1, 0, -p_uv[0, i]], [0, 1, -p_uv[1, i]]]) Qi = np.kron(Qi_0, P[:, i].T) Q = np.append(Q, Qi, axis=0) # 1. Find M_tilde using SVD U, S, VT = linalg.svd(Q) M_tilde = VT.T[:, -1].reshape(3, 4) # print(M_tilde /M_cv) # M is determined up to scale # 2. RQ factorization to find K_tilde and R K_tilde, R = linalg.rq(M_tilde[:, 0:3]) # 3. Resolve the ambiguity of RQ factorization D = np.diag(np.sign(np.diag(K_tilde))) K_tilde = K_tilde @ D R = D @ R # 4. Find T T = linalg.solve(K_tilde, M_tilde[:, -1]).reshape(3, 1) # 5. Recover scale s = 1 / K_tilde[2, 2] K = s * K_tilde M = s * M_tilde # 6. Resolve sign ambiguity if linalg.det(R) < 0: R = -R T = -T M = -M return K, R, T, M
def estimate_params(P): # Compute camera center c by svd u, s, vt = np.linalg.svd(P) # c ~ col of V w/ least eigval (last row of vt) c = np.array([vt[-1, 0:3] / vt[-1, -1]]).T # (3,) # Compute K and R by QR decomposition M = P[:, 0:3] K, R = rq(M) # Compute t = -Rc t = -np.dot(R, c) return K, R, t
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): """ Factorize the camera matrix into K,R,t as P = K[R|t]. """ # factor first 3*3 part K, R = rq(self.P[:, :3]) # make diagonal of K positive T = np.diag(np.sign(np.diag(K))) if det(T) < 0: T[1, 1] *= -1 self.K = np.dot(K, T) self.R = np.dot(T, R) # T is its own inverse self.t = np.dot(inv(self.K), self.P[:, 3]) return self.K, self.R, self.t
def RQ_decomposition(projection_matrix4): p1 = projection_matrix4[:, :3] p2 = projection_matrix4[:, 3] k, r1 = linalg.rq(p1) #np.allclose(p1, k @ r1) k = k / k[-1, -1] r2 = np.matmul(inv(k), p2) return k, r1, r2
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 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 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_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)
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_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 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 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)