2

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")
Jérôme Richard
  • 41,678
  • 6
  • 29
  • 59

0 Answers0