Python DualQuaternion.from_transformation_matrix Examples

Example #1

def random_transform_as_dual_quaternion(
        enforce_positive_non_dual_scalar_sign=True):
    T_rand = rand_transform()
    dq_rand = DualQuaternion.from_transformation_matrix(T_rand)
    if (enforce_positive_non_dual_scalar_sign):
        if (dq_rand.q_rot.w < 0):
            dq_rand.dq = -dq_rand.dq.copy()
    return dq_rand.copy()

Example #2

File: test_dual_quaternion.py Project: Abdob/maplab_ws

    def test_dq_to_matrix(self):

        pose = [1, 2, 3, 4., 5., 6., 7.]
        dq = DualQuaternion.from_pose_vector(pose)
        dq.normalize()

        matrix_out = dq.to_matrix()

        dq_from_matrix = DualQuaternion.from_transformation_matrix(matrix_out)
        matrix_out_2 = dq_from_matrix.to_matrix()

        npt.assert_allclose(dq.dq, dq_from_matrix.dq)
        npt.assert_allclose(matrix_out, matrix_out_2)