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 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 |