BUG: fix dual quaternion normalization

Author: AlexanderFabisch

doc/source/api.rst

@@ -421,6 +421,7 @@ Dual Quaternion
 
    ~dual_quaternion_requires_renormalization
    ~norm_dual_quaternion
+   ~dual_quaternion_squared_norm
 
    ~assert_unit_dual_quaternion
    ~assert_unit_dual_quaternion_equal

pytransform3d/rotations/_quaternion.py

@@ -395,9 +395,6 @@ def q_prod_vector(q, v):
 def q_conj(q):
     r"""Conjugate of quaternion.
 
-    The conjugate of a unit quaternion inverts the rotation represented by
-    this unit quaternion.
-
     The conjugate of a quaternion :math:`\boldsymbol{q}` is often denoted as
     :math:`\boldsymbol{q}^*`. For a quaternion :math:`\boldsymbol{q} = w
     + x \boldsymbol{i} + y \boldsymbol{j} + z \boldsymbol{k}` it is defined as
@@ -407,10 +404,13 @@ def q_conj(q):
         \boldsymbol{q}^* = w - x \boldsymbol{i} - y \boldsymbol{j}
         - z \boldsymbol{k}.
 
+    The conjugate of a unit quaternion inverts the rotation represented by
+    this unit quaternion.
+
     Parameters
     ----------
     q : array-like, shape (4,)
-        Unit quaternion to represent rotation: (w, x, y, z)
+        Quaternion: (w, x, y, z)
 
     Returns
     -------
@@ -421,7 +421,7 @@ def q_conj(q):
     --------
     rotor_reverse : Reverse of a rotor, which is the same operation.
     """
-    q = check_quaternion(q)
+    q = check_quaternion(q, unit=False)
     return np.array([q[0], -q[1], -q[2], -q[3]])
 
 

pytransform3d/transformations/__init__.py

@@ -55,6 +55,7 @@
 from ._dual_quaternion import (
     dual_quaternion_requires_renormalization,
     norm_dual_quaternion,
+    dual_quaternion_squared_norm,
     check_dual_quaternion,
     assert_unit_dual_quaternion_equal,
     assert_unit_dual_quaternion,
@@ -136,6 +137,7 @@
     "random_exponential_coordinates",
     "pq_slerp",
     "norm_dual_quaternion",
+    "dual_quaternion_squared_norm",
     "dual_quaternion_double",
     "dq_q_conj",
     "dq_conj",

pytransform3d/transformations/_dual_quaternion.py

@@ -75,30 +75,37 @@ def check_dual_quaternion(dq, unit=True):
     return dq
 
 
-def dual_quaternion_requires_renormalization(dq, tolerance=1e-6):
-    r"""Check if dual quaternion requires renormalization.
+def dual_quaternion_squared_norm(dq):
+    """Compute squared norm of dual quaternion.
 
-    Dual quaternions that represent transformations in 3D should have unit
-    norm. Since the real and the dual quaternion are orthogonal, their dot
-    product should be 0. In addition, :math:`\epsilon^2 = 0`. Hence,
+    Parameters
+    ----------
+    dq : array-like, shape (8)
+        Dual quaternion to represent transform:
+        (pw, px, py, pz, qw, qx, qy, qz)
 
-    .. math::
+    Returns
+    -------
+    squared_norm : array, shape (2,)
+        Squared norm of dual quaternion, which is a dual number with a real and
+        a dual part.
+    """
+    dq = np.asarray(dq)
+    prod = concatenate_dual_quaternions(
+        dq, dq_q_conj(dq, unit=False), unit=False
+    )
+    return prod[[0, 4]]
 
-        ||\boldsymbol{p} + \epsilon \boldsymbol{q}||
-        =
-        \sqrt{(\boldsymbol{p} + \epsilon \boldsymbol{q}) \cdot
-        (\boldsymbol{p} + \epsilon \boldsymbol{q})}
-        =
-        \sqrt{\boldsymbol{p}\cdot\boldsymbol{p}
-        + 2\epsilon \boldsymbol{p}\cdot\boldsymbol{q}
-        + \epsilon^2 \boldsymbol{q}\cdot\boldsymbol{q}}
-        =
-        \sqrt{\boldsymbol{p}\cdot\boldsymbol{p}},
 
-    i.e., the norm only depends on the real quaternion.
+def dual_quaternion_requires_renormalization(dq, tolerance=1e-6):
+    r"""Check if dual quaternion requires renormalization.
+
+    Dual quaternions that represent transformations in 3D should have unit
+    norm (:math:`1 + 0 \epsilon`), that is the real quaternion must have unit
+    norm and the real and the dual quaternion must be orthogonal (their dot
+    product should be 0).
 
-    This function checks unit norm and orthogonality of the real and dual
-    part.
+    This function checks unit norm and orthogonality of the real and dual part.
 
     Parameters
     ----------
@@ -127,11 +134,11 @@ def dual_quaternion_requires_renormalization(dq, tolerance=1e-6):
     assert_unit_dual_quaternion
         Checks unit norm and orthogonality of real and dual quaternion.
     """
-    real = dq[:4]
-    dual = dq[4:]
-    real_norm = np.linalg.norm(real)
-    real_dual_dot = np.dot(real, dual)
-    return abs(real_norm - 1.0) > tolerance or abs(real_dual_dot) > tolerance
+    squared_norm = dual_quaternion_squared_norm(dq)
+    return (
+        abs(squared_norm[0] - 1.0) > tolerance
+        or abs(squared_norm[1]) > tolerance
+    )
 
 
 def norm_dual_quaternion(dq):
@@ -169,30 +176,12 @@ def norm_dual_quaternion(dq):
     .. [1] enki (2023). Properly normalizing a dual quaternion.
        https://stackoverflow.com/a/76313524
     """
-    dq = check_dual_quaternion(dq, unit=False)
-    dq_prod = concatenate_dual_quaternions(dq, dq_q_conj(dq), unit=False)
-
-    prod_real = dq_prod[:4]
-    prod_dual = dq_prod[4:]
-
-    real = np.copy(dq[:4])
+    # 1. ensure unit norm of real quaternion
+    dq = check_dual_quaternion(dq, unit=True)
+    # 2. ensure orthogonality of real and dual quaternion
+    real = dq[:4]
     dual = dq[4:]
-
-    prod_real_norm = np.linalg.norm(prod_real)
-    if prod_real_norm == 0.0:
-        real = np.array([1.0, 0.0, 0.0, 0.0])
-        prod_real_norm = 1.0
-        valid_dq = np.hstack((real, dual))
-        prod_dual = concatenate_dual_quaternions(
-            valid_dq, dq_q_conj(valid_dq), unit=False
-        )[4:]
-
-    real_inv_sqrt = 1.0 / prod_real_norm
-    dual_inv_sqrt = -0.5 * prod_dual * real_inv_sqrt**3
-
-    real = real_inv_sqrt * real
-    dual = real_inv_sqrt * dual + concatenate_quaternions(dual_inv_sqrt, real)
-
+    dual = dual - np.dot(real, dual) * real
     return np.hstack((real, dual))
 
 
@@ -229,14 +218,9 @@ def assert_unit_dual_quaternion(dq, *args, **kwargs):
         Normalization that enforces unit norm and orthogonality of the real and
         dual quaternion.
     """
-    real = dq[:4]
-    dual = dq[4:]
-
-    real_norm = np.linalg.norm(real)
-    assert_array_almost_equal(real_norm, 1.0, *args, **kwargs)
-
-    real_dual_dot = np.dot(real, dual)
-    assert_array_almost_equal(real_dual_dot, 0.0, *args, **kwargs)
+    real_sq_norm, dual_sq_norm = dual_quaternion_squared_norm(dq)
+    assert_array_almost_equal(real_sq_norm, 1.0, *args, **kwargs)
+    assert_array_almost_equal(dual_sq_norm, 0.0, *args, **kwargs)
 
     # The two previous checks are consequences of the unit norm requirement.
     # The norm of a dual quaternion is defined as the product of a dual
@@ -307,7 +291,7 @@ def dual_quaternion_double(dq):
     return -check_dual_quaternion(dq, unit=True)
 
 
-def dq_conj(dq):
+def dq_conj(dq, unit=True):
     """Conjugate of dual quaternion.
 
     There are three different conjugates for dual quaternions. The one that we
@@ -321,6 +305,12 @@ def dq_conj(dq):
         Unit dual quaternion to represent transform:
         (pw, px, py, pz, qw, qx, qy, qz)
 
+    unit : bool, optional (default: True)
+        Normalize the dual quaternion so that it is a unit dual quaternion.
+        A unit dual quaternion has the properties
+        :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and
+        :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.
+
     Returns
     -------
     dq_conjugate : array, shape (8,)
@@ -331,11 +321,11 @@ def dq_conj(dq):
     dq_q_conj
         Quaternion conjugate of dual quaternion.
     """
-    dq = check_dual_quaternion(dq)
+    dq = check_dual_quaternion(dq, unit=unit)
     return np.r_[dq[0], -dq[1:5], dq[5:]]
 
 
-def dq_q_conj(dq):
+def dq_q_conj(dq, unit=True):
     """Quaternion conjugate of dual quaternion.
 
     For unit dual quaternions that represent transformations, this function
@@ -352,6 +342,12 @@ def dq_q_conj(dq):
         Unit dual quaternion to represent transform:
         (pw, px, py, pz, qw, qx, qy, qz)
 
+    unit : bool, optional (default: True)
+        Normalize the dual quaternion so that it is a unit dual quaternion.
+        A unit dual quaternion has the properties
+        :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and
+        :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.
+
     Returns
     -------
     dq_q_conjugate : array, shape (8,)
@@ -362,7 +358,7 @@ def dq_q_conj(dq):
     dq_conj
         Conjugate of a dual quaternion.
     """
-    dq = check_dual_quaternion(dq)
+    dq = check_dual_quaternion(dq, unit=unit)
     return np.r_[dq[0], -dq[1:4], dq[4], -dq[5:]]
 
 

pytransform3d/transformations/_dual_quaternion.pyi

@@ -4,6 +4,7 @@ import numpy.typing as npt
 def check_dual_quaternion(
     dq: npt.ArrayLike, unit: bool = ...
 ) -> np.ndarray: ...
+def dual_quaternion_squared_norm(dq: npt.ArrayLike) -> np.ndarray: ...
 def dual_quaternion_requires_renormalization(
     dq: npt.ArrayLike, tolerance: float = ...
 ) -> bool: ...
@@ -13,8 +14,8 @@ def assert_unit_dual_quaternion_equal(
     dq1: npt.ArrayLike, dq2: npt.ArrayLike, *args, **kwargs
 ): ...
 def dual_quaternion_double(dq: npt.ArrayLike) -> np.ndarray: ...
-def dq_conj(dq: npt.ArrayLike) -> np.ndarray: ...
-def dq_q_conj(dq: npt.ArrayLike) -> np.ndarray: ...
+def dq_conj(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...
+def dq_q_conj(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...
 def concatenate_dual_quaternions(
     dq1: npt.ArrayLike, dq2: npt.ArrayLike, unit: bool = ...
 ) -> np.ndarray: ...

pytransform3d/transformations/test/test_dual_quaternion.py

@@ -40,18 +40,27 @@ def test_check_dual_quaternion():
 
 def test_normalize_dual_quaternion():
     dq = [1, 0, 0, 0, 0, 0, 0, 0]
+    norm = pt.dual_quaternion_squared_norm(dq)
+    assert_array_almost_equal(norm, [1, 0])
+    assert not pt.dual_quaternion_requires_renormalization(dq)
     dq_norm = pt.check_dual_quaternion(dq)
     pt.assert_unit_dual_quaternion(dq_norm)
     assert_array_almost_equal(dq, dq_norm)
     assert_array_almost_equal(dq, pt.norm_dual_quaternion(dq))
 
     dq = [0, 0, 0, 0, 0, 0, 0, 0]
+    norm = pt.dual_quaternion_squared_norm(dq)
+    assert_array_almost_equal(norm, [0, 0])
+    assert pt.dual_quaternion_requires_renormalization(dq)
     dq_norm = pt.check_dual_quaternion(dq)
     pt.assert_unit_dual_quaternion(dq_norm)
     assert_array_almost_equal([1, 0, 0, 0, 0, 0, 0, 0], dq_norm)
     assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq))
 
     dq = [0, 0, 0, 0, 0.3, 0.5, 0, 0.2]
+    norm = pt.dual_quaternion_squared_norm(dq)
+    assert_array_almost_equal(norm, [0, 0])
+    assert pt.dual_quaternion_requires_renormalization(dq)
     dq_norm = pt.check_dual_quaternion(dq)
     assert pt.dual_quaternion_requires_renormalization(dq_norm)
     assert_array_almost_equal([1, 0, 0, 0, 0.3, 0.5, 0, 0.2], dq_norm)
@@ -60,7 +69,7 @@ def test_normalize_dual_quaternion():
     )
 
     rng = np.random.default_rng(999)
-    for _ in range(5):  # norm != 1
+    for _ in range(5):  # real norm != 1
         A2B = pt.random_transform(rng)
         dq = rng.standard_normal() * pt.dual_quaternion_from_transform(A2B)
         dq_norm = pt.check_dual_quaternion(dq)
@@ -75,9 +84,21 @@ def test_normalize_dual_quaternion():
         assert pt.dual_quaternion_requires_renormalization(dq_roundoff_error)
         dq_norm = pt.norm_dual_quaternion(dq_roundoff_error)
         pt.assert_unit_dual_quaternion(dq_norm)
+        assert_array_almost_equal(
+            pt.dual_quaternion_squared_norm(dq_norm), [1, 0]
+        )
         assert not pt.dual_quaternion_requires_renormalization(dq_norm)
         assert_array_almost_equal(dq, dq_norm, decimal=3)
 
+    for _ in range(50):
+        dq = rng.standard_normal(size=8)
+        dq_norm = pt.norm_dual_quaternion(dq)
+        assert_array_almost_equal(
+            pt.dual_quaternion_squared_norm(dq_norm), [1, 0]
+        )
+        assert not pt.dual_quaternion_requires_renormalization(dq_norm)
+        pt.assert_unit_dual_quaternion(dq_norm)
+
 
 def test_dual_quaternion_double():
     rng = np.random.default_rng(4183)