Equivalent quaternions not considered when comparing PosePath3D

[YifuTao]

Hi, I found that the __eq__ method in PosePath3D does not consider equivalent quaternions.

Example

Say there are two quaternions: [-0.08249053, -0.07845361, 0.00826215, 0.99346469] and [ 0.08249053, 0.07845361, -0.00826215, -0.99346469].

They are equivalent (double-cover) and corresponds to the same rotation matrix, but just have opposite signs.

However, in the implementation of __eq__:

equal &= np.allclose(self.orientations_quat_wxyz,
                     other.orientations_quat_wxyz)

This would give False to the above case, even though they are equivalent.

I went into this issue in a unittest where I load a trajectory, save, reload and compare.

Do you think it's a good idea to handle these equivalent quaternions?

Potential reimplementation

equal &= all(
    [
        np.allclose(q1, q2) or np.allclose(q1, -q2)
        for q1, q2 in zip(self.orientations_quat_wxyz, other.orientations_quat_wxyz)
    ]
)

[MichaelGrupp]

Yes sounds like a good idea. I used the equal operator so far only for very simple checks, but if there's a conversion in between then this could happen. I will add this.