更改相机显示点的坐标系

Question

我正在开发一种产品，用于相对于另一个对象定位一个对象。我有一个立体相机，它为我提供了来自刚体（与我要定位的对象相关联）的以下数据，上面带有反光标记：

让我们调用对象 1 A 和对象 2 B 相机 returns 这些值： TxA、TyA、TzA、Q0A、QxA、QyA、QzA TxB、TyB、TzB、Q0B、QxB、QyB、QzB

相机软件允许您输出 B 参考 A 的值，但我想在我的脚本中重新创建此功能。

为此，在我的脚本中，我创建了 2 个四元数，例如：

QuatA = Quaternion (Q0A, QxA, QyA, QzA)
QuatB = Quaternion (Q0B, QxB, QyB, QzB)

我试图通过以下方式将 QuatB 引用到 QuatA：

Result = QuatA * QuatB * QuatA.conj()

但是结果好像和相机软件的不一样。

例如：

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

谁能帮忙解决这个问题？

Answer 1

经过一些研究，我发现这段代码对我来说是正确的：

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)

现在我将重点介绍此变换的坐标方面。

更改相机显示点的坐标系

Change coordinate system of points shown by camera

python

quaternions