def csv_row_data_to_transformation(A_p_AB, q_AB_np): """ Constructs a proper transformation matrix out of the given translation vector and quaternion. Returns: 4x4 transformation matrix. """ assert (math.fabs(1.0 - np.linalg.norm(q_AB_np)) < 1e-8) T_AB = np.identity(4) q_AB = Quaternion(q=q_AB_np) R_AB = q_AB.to_rotation_matrix() T_AB[0:3, 0:3] = R_AB T_AB[0:3, 3] = A_p_AB return T_AB
def test_quaternion_to_rotation_matrix(self): q = Quaternion(0.5, 0.5, 0.5, 0.5) rotation_matrix = q.to_rotation_matrix() expected_rotation_matrix = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) npt.assert_allclose(rotation_matrix, expected_rotation_matrix)