def factor(W):
"""
This implements rigid factorization as described in
Tomasi, C. & Kanade, T. "Shape and motion from image streams under
orthography: a factorization method International Journal of Computer
Vision, 1992
"""
F = W.shape[0]/2
N = W.shape[1]
# Center W
T = W.mean(axis=1)
W = W - T[:, np.newaxis]
# Factor W
M_hat, B_hat = factor_matrix(W, J=3)
# Where we will build the linear system.
A = np.zeros((3*F, 6))
b = np.zeros((3*F,))
for f in range(F):
# Extract the two rows.
x_f, y_f = M_hat[f*2:f*2+2]
# Both rows must have unit length.
A[f*3] = util.vc(x_f, x_f)
b[f*3] = 1.0
A[f*3+1] = util.vc(y_f,y_f)
b[f*3+1] = 1.0
# And be orthogonal.
A[f*3+2] = util.vc(x_f - y_f, x_f + y_f)
# Recovec vech(Q) and Q
vech_Q = np.linalg.lstsq(A, b)[0]
Q = util.from_vech(vech_Q, 3, 3, sym=True)
# Factor out G recovery matrix
G, Gt = factor_matrix(Q)
# Upgrade M and B.
M = np.dot(M_hat, G)
B = np.linalg.solve(G, B_hat)
# Find actual rotations matrices.
Rs = np.zeros((F, 3, 3))
Rs[:,:2] = M.reshape(F,2,3)
Rs[:,2] = util.normed(np.cross(Rs[:,0], Rs[:,1], axis=-1))
# And 3D translations.
Ts = np.zeros((F,3))
Ts[:,:2] = T.reshape(F,2)
model = BasisShapeModel(Rs, Bs = B[np.newaxis,:,:], Ts = Ts)
return model
def generate_rotation_from_optical_axis(D, safety_checks=False):
"""
D -- a Fx3 vector of directions.
returns R so that
Rd = FxN vectors of [0 0 1]
"""
assert D.ndim == 2 and D.shape[1] == 3
F = D.shape[0]
# Space for the rotations.
R = np.zeros((F, 3, 3))
# Put the D vectors along the Z axis.
R[:, 2, :] = normed(D)
# Make x direction orthogonal in the y=0 plane.
R[:, 0, 0] = R[:, 2, 2]
R[:, 0, 2] = -R[:, 2, 0]
R[:, 0, :] = normed(R[:, 0, :], axis=-1)
# Make the y direction the cross product.
# Negation makes this a rotation instead of a reflection.
R[:, 1, :] = -np.cross(R[:, 0, :], R[:, 2, :], axis=-1)
if safety_checks:
for f in range(F):
if np.linalg.matrix_rank(R[f]) != 3:
raise Exception("Low rank.")
if np.linalg.det(R[f]) < 0:
# Flip one of the rows signs to change this reflection to a rotation matrix.
R[f, 0, :] *= -1
return R
def generate_rotation_from_optical_axis(D, safety_checks=False):
"""
D -- a Fx3 vector of directions.
returns R so that
Rd = FxN vectors of [0 0 1]
"""
assert D.ndim == 2 and D.shape[1] == 3
F = D.shape[0]
# Space for the rotations.
R = np.zeros((F, 3, 3))
# Put the D vectors along the Z axis.
R[:, 2, :] = normed(D)
# Make x direction orthogonal in the y=0 plane.
R[:, 0, 0] = R[:, 2, 2]
R[:, 0, 2] = -R[:, 2, 0]
R[:, 0, :] = normed(R[:, 0, :], axis=-1)
# Make the y direction the cross product.
# Negation makes this a rotation instead of a reflection.
R[:, 1, :] = -np.cross(R[:, 0, :], R[:, 2, :], axis=-1)
if safety_checks:
for f in range(F):
if np.linalg.matrix_rank(R[f]) != 3:
raise Exception("Low rank.")
if np.linalg.det(R[f]) < 0:
# Flip one of the rows signs to change this reflection to a rotation matrix.
R[f, 0, :] *= -1
return R
def Rs_from_M_k(M_k):
"""Estimates rotations from a column triple of M."""
n_frames = M_k.shape[0] / 2
# Estimate scales of each rotation matrix by averaging norms of two rows.
scales = .5 * (norm(M_k[0::2], axis=1) + norm(M_k[1::2], axis=1))
# Rescale to be close to rotation matrices.
R = M_k.reshape(n_frames, 2, 3) / scales[:, np.newaxis, np.newaxis]
Rs = []
# Upgrade to real rotations.
for f in range(n_frames):
Rx = R[f, 0]
Ry = R[f, 1]
Rz = normed(np.cross(Rx, Ry))
U, s, Vh = np.linalg.svd(np.asarray([Rx, Ry, Rz]))
Rs.append(np.dot(U, Vh))
return np.asarray(Rs)
def Rs_from_M_k(M_k):
"""Estimates rotations from a column triple of M."""
n_frames = M_k.shape[0]/2
# Estimate scales of each rotation matrix by averaging norms of two rows.
scales = .5 * (norm(M_k[0::2], axis=1) + norm(M_k[1::2], axis=1))
# Rescale to be close to rotation matrices.
R = M_k.reshape(n_frames, 2, 3) / scales[:,np.newaxis,np.newaxis]
Rs = []
# Upgrade to real rotations.
for f in range(n_frames):
Rx = R[f,0]
Ry = R[f,1]
Rz = normed(np.cross(Rx,Ry))
U,s,Vh = np.linalg.svd(np.asarray([Rx,Ry,Rz]))
Rs.append(np.dot(U, Vh))
return np.asarray(Rs)
