Change coordinate system of points shown by camera

user_16234047 0
Viewed 34

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?

quaternions python
1 Answers

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.

user_162340470
Related