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 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)
