python - python实现3D刚体平移和旋转

Question

我一直在尝试解决如何使用 python 解决以下问题：

1. 我们有构成刚体的点 a、b、c、d
2. 一些未知的3D平移和旋转应用于刚体
3. 我们现在知道 a、b、c 的坐标
4. 我们要计算 d 的坐标

到目前为止我所知道的：

• 由于万向节锁定等原因，尝试使用“直接”的欧拉角计算来做到这一点似乎是个坏主意。

• 因此，第 4 步将涉及一个变换矩阵，一旦您知道了旋转和平移矩阵，看起来这一步很容易使用其中之一：

  • http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
  • https://pypi.python.org/pypi/euclid/0.01

我无法解决的是如何在给定 a、b、c 的“新”坐标的情况下计算旋转和平移矩阵。

我可以看到，在一般情况下（非刚体），旋转部分是Wahba 的问题，但我认为对于刚体，应该有一些更快的方法来直接计算它，方法是使用点。

Answer 1

对于您尝试匹配的一组对应点（可能存在扰动），我使用了 SVD（奇异值分解），它似乎存在于 numpy.

这种技术的一个例子（甚至在 Python 中）可以在这里找到，但我没有评估它的正确性。

你要做的是一个“基础变换”或“基础变化”，它将被表示为一个变换矩阵。假设您的 3 个已知点不共线，您可以通过以下方式创建初始基础：

1. 计算向量：x=(ba) 和 y=(ca)
2. 归一化 x (x = x / 幅度 (x))
3. 将 y 投影到 x (proj_y = x DOT y * x)
4. 从 y 中减去投影 (y = y - proj_y)
5. 标准化 y
6. 计算 z = x 交叉 y

这给了你一个初始的 x,y,z 坐标基础 A。对你的新点做同样的事情，你会得到第二个基础 B。现在你想找到变换 T，它将在 A 中取一个点并将其转换为 B (基数变化）。那部分很容易。您可以反转 A 将点转换回法线基础，然后使用 B 转换为第二个。由于 A 是正交的，您只需转置 A 即可得到逆。所以“新 d”等于 d * inverse(A) * B。（尽管根据您的表示，您可能需要使用 B * inverse(A) * d。）

您需要熟悉矩阵才能获得所有这些。您对向量和矩阵的表示将告知您将矩阵相乘以得到 T 的顺序（T 是 inverse(A)*B 或 B*inverse(A)）。

要从向量 x=(x1,x2,x3), y=(y1,y2,y3), z=(z1,z2,z3) 计算基矩阵，请将其填充为：

| x1 y1 z1 |
| x2 y2 z2 |
| x3 y3 z3 |