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 是逆 (A)*B 或 B*逆(A)）。

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

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