I am working on a product to localise an object relative to another. I have a Stereoscopic camera that gives me the following data from Rigid bodies (which are linked to the objects I want to localise) with reflective markers on them :
Let's call object1 A and object2 B
The camera returns those values :
TxA, TyA, TzA, Q0A, QxA, QyA, QzA
TxB, TyB, TzB, Q0B, QxB, QyB, QzB
The camera software allow you to output B values referenced to A, but I wanted to recreate this function in my script.
For that, in my script, I then created 2 quaternions such as :
QuatA = Quaternion (Q0A, QxA, QyA, QzA)
QuatB = Quaternion (Q0B, QxB, QyB, QzB)
I tried to reference QuatB to QuatA by doing :
Result = QuatA * QuatB * QuatA.conj()
But the results seem to be different than what the camera software.
For example :
QuatA = Quaternion(0.8381, 0.2948, -0.0762, -0.4526)
QuatB = Quaternion(0.6062, -0.2564, -0.6147, -0.4347)
Results : Quaternion(0.753, 0.241, -0.626, 0.421)
Camera output : 0.676, -0.149, -0.714, 0.111
Can anyone help on this problem?
After doing some research, I found this code that works correctly for me :
QuatA = QuatA.conj()
a = ( QuatA.q0 + QuatA.qx ) * ( QuatB.q0 + QuatB.qx)
b = ( QuatA.qz - QuatA.qy ) * ( QuatB.qy - QuatB.qz)
c = ( QuatA.qx - QuatA.q0 ) * ( QuatB.qy + QuatB.qz)
d = ( QuatA.qy + QuatA.qz ) * ( QuatB.qx - QuatB.q0)
e = ( QuatA.qx + QuatA.qz ) * ( QuatB.qx + QuatB.qy)
f = ( QuatA.qx - QuatA.qz ) * ( QuatB.qx - QuatB.qy)
g = ( QuatA.q0 + QuatA.qy ) * ( QuatB.q0 - QuatB.qz)
h = ( QuatA.q0 - QuatA.qy ) * ( QuatB.q0 + QuatB.qz)
rotatedq0 = b + ( -e - f + g + h ) / 2
rotatedqx = a - ( e + f + g + h ) / 2
rotatedqy = -c + ( e - f + g - h ) / 2
rotatedqz = -d + ( e - f - g + h ) / 2
Result = Quaternion(rotatedq0, rotatedqx, rotatedqy, rotatedqz)
Now I'll focus on the coordinate aspect of this transformation.