如何从旋转和平移矩阵比较两个物体的姿势和位置？答案

【问题标题】：How to compare pose and position of two objects from rotation and translation matrix?如何从旋转和平移矩阵比较两个物体的姿势和位置？
【发布时间】：2014-12-05 03:59:45
【问题描述】：

我有两个 4*4 矩阵，表示 OpenGL 中两个对象的姿势和位置。如何计算这些对象的方向和原点的差异。这是从OpenGL-Style矩阵转换而来的矩阵。

r1 r2 r3 t1
r4 r5 r6 t2
r7 r8 r9 t3
 0  0  0  1

【问题讨论】：

标签： opengl graphics computer-vision

【解决方案1】：

假设这两个矩阵是 M1 和 M2，从右边开始应用于向量：

x1 = M1 * x
x2 = M2 * x

两个矩阵的“差”可以定义为矩阵Md需要应用到x1得到x2：

Md * x1 = x2
Md * (M1 * x) = M2 * x

要为所有向量 x 创建此恒等式，Md 需要满足以下等式：

Md * M1 = M2

为了隔离这个方程中的 Md，我们乘以 M1 的倒数：

Md * M1 * inv(M1) = M2 * inv(M1)

矩阵乘法是关联的，所以我们可以将左侧分组为：

Md * (M1 * inv(M1)) = M2 * inv(M1)

Md 的结果是：

Md = M2 * inv(M1)

因此，您可以通过 4x4 矩阵求逆和矩阵乘法来解决这个问题。

另一种方法是将原始矩阵分解为旋转和平移，分别应用于向量：

x1 = R1 * x + t1
x2 = R2 * x + t2

其中 R1 和 R2 是 3x3 矩阵，t1 和 t2 平移向量。按照与之前相同的步骤，但还将差异分解为旋转 Rd 和平移 td：

Rd * x1 + td = x2
Rd * (R1 * x + t1) + td = R2 * x + t2
Rd * R1 * x + Rd * t1 + td = R2 * x + t2

要匹配两边的旋转部分，Rd需要满足：

Rd * R1 = R2

毫不奇怪，Rd 的计算看起来与上面的 Md 非常相似。作为简化，我们可以利用旋转的逆是它的转置这一事实：

Rd = R2 * inv(R1)
Rd = R2 * transp(R1)

现在在上面的等式中，翻译部分也需要匹配，这给了我们 td:

Rd * t1 + td = t2
td = t2 - Rd * t1

总而言之，您计算差异的旋转矩阵和平移向量为：

Rd = R2 * transp(R1)
td = t2 - Rd * t1

【讨论】：

感谢您的回复。我会试试你说的，然后发表评论。
考虑到我的情况，我更喜欢第二种方法。当我计算 Rd 时，它似乎运行良好。但是当两个对象之间的方向存在明显差异时，即使对象共享相同的原点，td 也不会是 (0,0,0)。所以我只是让td = t1-t2，因为这两个向量是两个对象的中心，对吗？
是的，我想如果你只是想知道它们之间的距离，t2 - t1 会给你。在我的计算中它出现不同的原因是它适用于您想要应用“差异”转换的情况。在这种情况下，您将首先旋转，这需要在翻译时考虑。