test_decompose.py

import numpy as np
import scipy
from scipy.spatial.transform import Rotation as R
np.set_printoptions(precision=4,suppress=True)

rot = R.random().as_matrix()
trans = np.random.uniform(size=(3,))
c2w = np.eye(4)
c2w[:3,:3] = rot
c2w[:3,3] = trans
w2c = np.linalg.inv(c2w)

intr = np.array(
    [[500.,0.,250.,0.],
    [0.,500.,250.,0.],
    [0.,0.,1.,0.]]
)

# intr[:3,:3] @ np.concatenate([np.eye(3), np.zeros([3,1])], axis=1)

P = intr @ w2c



#---------------------------------
#       对 3x4 的 P 直接 SVD
#---------------------------------
U, E_, VT = np.linalg.svd(P)
E = np.zeros([3,4])
E[:3,:3] = np.diagflat(E_)
V = VT.T
# 观察可以发现, V 的最后一列，归一化后，确实就是 trans
print(VT[-1,:3]/VT[-1,3], c2w[:3,3])



#---------------------------------
#       对 4x3 的转置以后的 P.T 做 SVD
#---------------------------------
U2, E_2, VT2 = np.linalg.svd(P.T)
print(U2[:3,-1]/U2[3,-1], c2w[:3,3])



#---------------------------------
#     对补全齐次的 P_ 做 RQ 或 QR
#---------------------------------
P_ = np.concatenate([P, np.array([0,0,0,1.]).reshape(1,4)],0)
r1, q1 = scipy.linalg.rq(P_)
q2, r2 = scipy.linalg.qr(P_)
# 从 q1, q2 中，并不能找出来 c2w[:3,3] !

# 而且可以发现，补全齐次以后的 P 甚至变成了满秩=4 的方阵
np.linalg.svd(P_)[1]

P_2 = np.concatenate([P, np.array([0,0,0,0.]).reshape(1,4)],0)
r3, q3 = scipy.linalg.rq(P_2)
q4, r4 = scipy.linalg.qr(P_2)
# 从 q3, q4 中，并不能找出来 c2w[:3,3] !