The sclerp algorithm does not converge

[wsp666]

We find that the interpolation algorithm fails to converge when two double quaternions are close, leading to a very outrageous result. As shown below, the terminal feedback position T is

array([[-1.50605828e-01,  9.78038623e-01,  1.44077536e-01,
        -1.28873894e+03],
       [ 1.31112452e-01,  1.64213295e-01, -9.77672501e-01,
        -3.56061344e+02],
       [-9.79860913e-01, -1.28352817e-01, -1.52964521e-01,
        -2.03949883e+02],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])

here is the simplified code.

from scipy.spatial.transform import Rotation
import numpy as np
from dual_quaternions import DualQuaternion

T1 = np.array([[-1.20911060e-02, -9.99300112e-01,  3.53990259e-02,
        -4.36968382e+00],
       [ 9.93674343e-01, -1.59607498e-02, -1.11160043e-01,
         1.54711835e+01],
       [ 1.11647238e-01,  3.38310559e-02,  9.93171865e-01,
         9.34843861e-01],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])

T2 = np.array([[-3.37726903e-02, -9.99344514e-01,  1.30364371e-02,
        -4.47583692e+00],
       [ 9.92807002e-01, -3.50451704e-02, -1.14481845e-01,
         1.54705164e+01],
       [ 1.14863668e-01,  9.07630609e-03,  9.93339800e-01,
         9.37977190e-01],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])
# Rotation matrices
R1 = T1[:3, :3]
R2 = T2[:3, :3]

# euler angles
euler1 = Rotation.from_matrix(R1).as_euler('zyx')
euler2 = Rotation.from_matrix(R2).as_euler('zyx')

print("Euler angles for T1: ", euler1)
print("Euler angles for T2: ", euler2)

dq1 = DualQuaternion.from_homogeneous_matrix(T1)
dq2 = DualQuaternion.from_homogeneous_matrix(T2)
delta = 0.5
# sclerp
dq = DualQuaternion.sclerp(dq1, dq2, delta)
T = dq.homogeneous_matrix()

# Linear interpolation is not satisfied
np.dot(np.linalg.inv(T1), T) - np.dot(T, np.linalg.inv(T2))

[RubensBenevides]

I'm facing exactly the same problem. I think there is something to do with the function 'transformation_matrix' called from pyquaternion, to convert from dual quaternion to matrice. In "Accessing matrix form" described here: "https://kieranwynn.github.io/pyquaternion/" they say:

"Both matrices and quaternions avoid the singularities and discontinuities involved with rotation in 3 dimensions by adding extra dimensions. This has the effect that different values could represent the same rotation, for example, quaternion q and -q represent the same rotation. It is therefore possible that, when converting a rotation sequence, the output may jump between different but equivalent forms. This could cause problems where subsequent operations such as differentiation are done on this data. Programmers should be aware of this issue."

So probably the map between matrices and quaternions is not working properly. I will check this elementwise on the conversion steps, if this is really the problem, I'll update my answer here.

[RubensBenevides]

After a lot of work examining every step inside the sclerp() function, I found the error. Just comment out these two lines and the error disappears:

if (start.q_r * stop.q_r).w < 0:
    start.q_r *= -1

I don't know exactly why the error stops occurring when we do this. I found out by pure testing. I will give more details so that someone who understands more about the mathematics of dual quaternions can explain it.

The sclerp() function will call the inverse() and pow() functions. The inverse() is working correctly, I have checked every step in the pyquaternion library, it is in pow() that the translation explodes. This happens exactly on line 13:

se = (self.q_d.vector - s0 * d/2 * np.cos(theta/2)) / np.sin(theta/2)

When dividing by np.sin(theta/2) the value becomes very large, and since it will be used to calculate the dual part of the dual quaternion, the translation value comes out wrong. I have no idea why this does not happen when we do not check the scalar part of the quaternion encoding the rotation q_r.w

All I know is that I tested the code in about 14 poses and it worked. The poses were very close in rotation and translation. Before removing those two lines, interpolation only worked on 4 of the 12 poses, now it works on all of them.

Translated with DeepL.com (free version)

[Achllle]

Thanks for digging in. I will take a look at this the coming weekend. I remember trying to fix this while working on #10 but getting stuck on something. Hopefully this moves it along! Otherwise PR welcome!

[RubensBenevides]

I am sorry, but I have some bad news. I tested the changed function on a larger dataset of 901 poses, in 41 poses the translation exploded again, but before the change, this value was much higher, at 300 of the poses were bad, and with a much larger error, thousands of meters in translation, instead two hundred now.

I carried out further tests and saw that the error also influences the interpolation of the rotation. I used another quaternion library and compared the result with the SLERP method.

Here is an image of the error in a set of poses using groundtruth. I use several methods to optimize poses in a circuit, clearly there is something wrong with the dual quaternion interpolation method SrLERP:

[RubensBenevides]

Where translation does not explode, SrLERP works perfectly like SLERP

[RubensBenevides]

Finally! In the pow function, change:

theta = 2*np.arccos(self.q_r.w) to theta = dq_ratio.q_r.angle

The pyquaternion function 'angle' will call atan2, which takes the quadrant into account when calculating the tangent arc, so somehow this makes a difference in the strange space of dual quaternions. Believe me, I've tested 64 combinations of signs between the elements of the dual quaternions being interpolated (start and stop in the sclerp). None worked, I even implemented a linear interpolation, because I'm only interested in the shortest path property, not the constant interval. To do this, I had to implement a normalization function for the dual quaternions, because the one in the library doesn't work. Both functions, my LERP and ScLERP, gave the same results, even when I interchanged the signs of the dual quaternions.

Here is my pow() function:

# Function used to calculate the power of a dual quaternion
def pow(self, exponent):
    """self^exponent
    :param exponent: single float
    """
    exponent = float(exponent)
    theta = self.q_r.angle
    t = self.translation() # translation of the dual quaternion
    if np.isclose(theta, 0):
        return DualQuaternion.from_translation_vector(exponent * np.array(t))
    else:
        # Plucker coordinates
        theta = theta                         # screw axis angle
        l = self.q_r.vector / np.sin(theta/2) # screw axis vector
        d = np.dot(l,t)                       # screw axis translation
        m = 0.5 * (np.cross(t, l) + np.cross(l, np.cross(t, l)/np.tan(theta/2))) # screw axis moment
        #l = self.q_r.vector / np.sin(theta/2) 
        #d = -2. * self.q_d.w / np.sin(theta/2)
        #m = (self.q_d.vector - l * d/2 * np.cos(theta/2)) / np.sin(theta/2) # BUG
        # Create the dual quaternion
        q_r = Quaternion(scalar=np.cos(exponent*theta/2),
                         vector=np.sin(exponent*theta/2) * l)
        q_d = Quaternion(scalar=-exponent*d/2 * np.sin(exponent*theta/2),
                         vector=exponent*d/2 * np.cos(exponent*theta/2) * l + np.sin(exponent*theta/2) * m)
        return DualQuaternion(q_r, q_d)

here is the sclerp (I only removed the check)

    def sclerp(cls, start, stop, t):
        """Screw Linear Interpolation

        Generalization of Quaternion slerp (Shoemake et al.) for rigid body motions
        ScLERP guarantees both shortest path (on the manifold) and constant speed
        interpolation and is independent of the choice of coordinate system.
        ScLERP(dq1, dq2, t) = dq1 * dq12^t where dq12 = dq1^-1 * dq2

        :param start: DualQuaternion instance
        :param stop: DualQuaternion instance
        :param t: fraction betweem [0, 1] representing how far along and around the
        """
        return start * (start.quaternion_conjugate() * stop).pow(t))

here is my normalization

import pyquaternion as quat
import dual_quaternion as dq
def normalize_dual_quaternion(dual_quat):
    # Multiply by its conjugate to get a dual number QQ* = [s + t]
    squared_norm = dual_quat*dual_quat.quaternion_conjugate()   
    s = squared_norm.q_r.w
    t = squared_norm.q_d.w
    # Compute the dual number [a,b] = [a = 1/np.sqrt(s), b = (-tu^3)/2]
    # https://stackoverflow.com/questions/23174899/properly-normalizing-a-dual-quaternion
    a = 1 / (s)**0.5
    b = -t*(a**3) / 2
    # Multiply this dual number by the blended dual quaternion using dual number 
    # multiplication: (a+b)*(c+d) = (a*c) + (a*d + b*c)
    quat_r_normalized = a*dual_quat.q_r
    quat_d_normalized = a*dual_quat.q_d + b*dual_quat.q_r
    # Convert to dual quaternion
    dual_quat_normalized = dq.DualQuaternion(quat_r_normalized, quat_d_normalized)
    return dual_quat_normalized

and here is the lerp, I used it to compare with sclerp result

def dq_lerp(start, stop, t):
    # Sum both dual quaternions
    blended_dq = (1-t)*start + t*stop
    # Normalize the blended_dq
    blended_dq_normalized = normalize_dual_quaternion(blended_dq)
    return blended_dq_normalized

In sclerp, instead of using start.inverse(), changing to start.quaternion_conjugate() gives the same result. Using plucker coordinates is also more stable and I think it's faster. I simply used the formulas in the screw() function with the nomenclature that the texts in the articles use.

[RubensBenevides]

The image below shows how the interpolation should behave, it is now very close to the black line (SLERP + LINEAR in translation). I mean DQ_ScLERP instead of DQ_SrLERP

[Achllle]

That's awesome investigation, @RubensBenevides! Would you like to make a PR for this so it's fixed for everyone? Capturing your tests into unit tests would also be very welcome. If you don't have time for this, just let me know!

[RubensBenevides]

Unfortunately I don't have the time, and I don't even know how to do the pull request. Sorry.