I'm trying to extract rotation matrix from affine transform matrix (via QR decomposition), but it gives me wrong rotations. After applying rotation vectors should have zero angle between them. I have no idea what is going wrong.
Here is the code that is currently giving the unexpected output:
import numpy as np
import math
np.set_printoptions(suppress=True)
def compute_affine(ins, out): # matrices 3x4
# https://stackoverflow.com/questions/8873462/how-to-perform-coordinates-affine-transformation-using-python
# finding transformation
l = len(ins)
entry = lambda r, d: np.linalg.det(np.delete(np.vstack([r, ins.T, np.ones(l)]), d, axis=0))
M = np.array([[(-1) ** i * entry(R, i) for R in out.T] for i in range(l + 1)])
print(M)
A, t = np.hsplit(M[1:].T / (-M[0])[:, None], [l - 1])
t = np.transpose(t)[0]
# output transformation
print("Affine transformation matrix:\n", A)
print("Affine transformation translation vector:\n", t)
# unittests
print("TESTING:")
for p, P in zip(np.array(ins), np.array(out)):
image_p = np.dot(A, p) + t
result = "[OK]" if np.allclose(image_p, P) else "[ERROR]"
print(p, " mapped to: ", image_p, " ; expected: ", P, result)
return A, t
def dot_product_angle(v1,v2):
if np.linalg.norm(v1) == 0 or np.linalg.norm(v2) == 0:
print("Zero magnitude vector!")
else:
vector_dot_product = np.dot(v1,v2)
arccos = np.arccos(vector_dot_product / (np.linalg.norm(v1) * np.linalg.norm(v2)))
angle = np.degrees(arccos)
return angle
return 0
if __name__=="__main__":
import numpy as np
src = np.array([
[1, 0, 0],
[0, 0, 1],
[1, 0, 1],
[1, 1, 1],
])
uv = np.array([
[0.1, 0],
[0, -0.1],
[0.1, -0.1],
[1, 1]
])
angle = 45
rot_matrix = np.array([[math.cos(angle), -math.sin(angle), 0 ],
[math.sin(angle), math.cos(angle), 0],
[0, 0, 1]])
dst = np.dot(src, rot_matrix)
# compute affine matrices
src_A, src_t = compute_affine(src, uv)
dst_A, dst_t = compute_affine(dst, uv)
src_Q, src_R = np.linalg.qr(np.vstack([src_A, np.cross(src_A[0], src_A[1])]))
dst_Q, dst_R = np.linalg.qr(np.vstack([dst_A, np.cross(dst_A[0], dst_A[1])]))
vec1 = np.dot(src_Q[:-1], src[0])
vec2 = np.dot(dst_Q[:-1], dst[0])
if not dot_product_angle(vec1, vec2) == 0:
raise Exception("Angle is invalid should be zero")