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's problem，但我认为对于刚体，应该有一些更快的方法来直接计算它使用这些点的正交单位向量集。

Answer 1

对于你想要匹配的一组对应点（可能存在扰动），我使用了SVD（奇异值分解），它似乎存在于numpy中。

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

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

1. 计算向量：x =（b-a）和y =（c-a）
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 CROSS y

这给你一个初始的x，y，z坐标基础A.为你的新点做同样的事情，你得到第二个基础B.现在你想要找到转换T，它将在A中取一个点并转换它到B（改变基础）。那部分很容易。您可以反转A将点转换回Normal，然后使用B转换为第二个。由于A是正交的，你只需转置A即可得到逆。所以“new 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 |