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)
