如何找到将 X、Y、Z 向量与另一个坐标系对齐的角度
Posted
技术标签:
【中文标题】如何找到将 X、Y、Z 向量与另一个坐标系对齐的角度【英文标题】：How to find the angles to align X, Y, Z vectors to another coordinate system 【发布时间】：2020-08-15 20:13:31 【问题描述】：我正在尝试旋转在 pyqtgraph 和 opengl 中使用 GLMeshItem 创建的一组 3D 点（形成一个矩形对象）。但是，我在执行此操作时遇到了麻烦。
我需要将对象的轴（T_X、T_Y、T_Z）与轴（Q_X、Q_Y、Q_Z）对齐。 Q 轴是从四元数计算的向量。
重现步骤： 1.从四元数做一个旋转矩阵 2.矩阵乘法来定义我想旋转到的Q轴 3. 将对象原点转换为 Q。 4.制作单位向量并计算X、Y、Z轴对之间的角度 5.旋转X轴和Z轴的差
你能帮帮我吗？
示例代码：
import numpy as np
import sys
from PyQt5.QtWidgets import QApplication, QHBoxLayout, QWidget
import pyqtgraph as pg
import pyqtgraph.opengl as gl
from pyqtgraph import Vector as VC
class MainWindow(QWidget):
def __init__(self):
super(MainWindow, self).__init__()
def quaternion_to_rotmat(point):
qw = point[0]
qx = point[1]
qy = point[2]
qz = point[3]
RotMat = np.array([[1 - 2 * (qy ** 2) - 2 * (qz ** 2),
2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw],
[2 * qx * qy + 2 * qz * qw,
1 - 2 * (qx ** 2) - 2 * (qz ** 2),
2 * qy * qz - 2 * qx * qw],
[2 * qx * qz - 2 * qy * qw,
2 * qy * qz + 2 * qx * qw,
1 - 2 * (qx ** 2) - (2 * qy ** 2)]])
return RotMat
self.setFixedSize(1000, 700)
self.graphLayout = QHBoxLayout()
# Set camera
w = gl.GLViewWidget()
# Group of points defining the rectangle object
verts = np.array([(-1.0, -1.0, 0.0),
(1.0, -1.0, 0.0),
(-1.0, 1.0, 0.0),
(1.0, 1.0, 0.0),
(-1.2987148761749268, -1.3632668256759644, -0.16066408157348633),
(-1.2987148761749268, -1.3632668256759644, 7.678848743438721),
(-1.2987148761749268, 1.3632668256759644, -0.16066408157348633),
(-1.2987148761749268, 1.3632668256759644, 7.678848743438721),
(1.2987148761749268, -1.3632668256759644, -0.16066408157348633),
(1.2987148761749268, -1.3632668256759644, 7.678848743438721),
(1.2987148761749268, 1.3632668256759644, -0.16066408157348633),
(1.2987148761749268, 1.3632668256759644, 7.678848743438721),
(-1.0, -1.0, 7.536437511444092),
(1.0, -1.0, 7.536437511444092),
(-1.0, 1.0, 7.536437511444092),
(1.0, 1.0, 7.536437511444092)])
faces = np.array([(1, 2, 0), (1, 3, 2), (5, 6, 4),
(7, 10, 6), (11, 8, 10), (9, 4, 8),
(10, 4, 6), (7, 9, 11), (5, 7, 6),
(7, 11, 10), (11, 9, 8), (9, 5, 4),
(10, 8, 4), (7, 5, 9), (13, 14, 12),
(13, 15, 14)])
colors = np.array([[1, 1, 1, 1] for i in range(len(faces))])
# Object origin (should rotate around this position)
T_Pos = (verts[0] + verts[3]) / 2
# Q position
Q_Pos = np.array([-13.5708862, 1.1735056, 107.5772339])
# Q quaternion (W, X, Y, Z)
Q_Quat = np.array([0.547013, 0.593053, -0.543852, -0.230846])
# Find the rotation matrix of Q quaternion
rotMat = quaternion_to_rotmat(Q_Quat)
# Matrix multiplication
r1 = np.matmul(rotMat, np.array([1, 0, 0]))
r2 = np.matmul(rotMat, np.array([0, 1, 0]))
r3 = np.matmul(rotMat, np.array([0, 0, 1]))
# Define new points - Multiply by 25 to visualize the axis in openGL
Q_X = np.array([Q_Pos[0] + r1[0] * 25, Q_Pos[1] + r1[1] * 25, Q_Pos[2] + r1[2] * 25])
Q_Y = np.array([Q_Pos[0] + r2[0] * 25, Q_Pos[1] + r2[1] * 25, Q_Pos[2] + r2[2] * 25])
Q_Z = np.array([Q_Pos[0] + r3[0] * 25, Q_Pos[1] + r3[1] * 25, Q_Pos[2] + r3[2] * 25])
Q_Line_X = np.array([Q_Pos, Q_X])
Q_Line_Y = np.array([Q_Pos, Q_Y])
Q_Line_Z = np.array([Q_Pos, Q_Z])
Q_Vec_X = Q_Pos-Q_X
Q_Vec_Y = Q_Pos-Q_Y
Q_Vec_Z = Q_Pos-Q_Z
# Camera settings
w.setCameraPosition(distance=90, azimuth=-2)
w.opts['center'] = VC(Q_Pos)
# Add object to window
self.object = gl.GLMeshItem(vertexes=verts, faces=faces, faceColors=colors, smooth=False, shader='shaded', glOptions='opaque')
w.addItem(self.object)
# Add visualization of Q positions
sphere = gl.MeshData.sphere(rows=10, cols=20, radius=[1])
self.P_Point = gl.GLMeshItem(meshdata=sphere, smooth=True, color=(1, 0, 1, 0.2), shader="balloon", glOptions="additive")
w.addItem(self.P_Point)
tr1 = pg.Transform3D()
tr1.translate(*Q_Pos)
self.P_Point.setTransform(tr1)
# Translated object origin (should rotate around this position)
# Translate T to Q
posDiff = Q_Pos - T_Pos
verts = posDiff-verts
T_Pos_base = (verts[0] + verts[3]) / 2
T_Pos_baseX = T_Pos_base - np.array([T_Pos_base[0] + 10, T_Pos_base[1], T_Pos_base[2]])
T_Pos_baseY = T_Pos_base - np.array([T_Pos_base[0], T_Pos_base[1] + 10, T_Pos_base[2]])
T_Pos_baseZ = T_Pos_base - np.array([T_Pos_base[0], T_Pos_base[1], T_Pos_base[2] + 10])
unit_TX = T_Pos_baseX / np.linalg.norm(T_Pos_baseX)
unit_TY = T_Pos_baseY / np.linalg.norm(T_Pos_baseY)
unit_TZ = T_Pos_baseZ / np.linalg.norm(T_Pos_baseZ)
unit_QX = Q_Vec_X / np.linalg.norm(Q_Vec_X)
unit_QY = Q_Vec_Y / np.linalg.norm(Q_Vec_Y)
unit_QZ = Q_Vec_Z / np.linalg.norm(Q_Vec_Z)
dotX = np.dot(unit_TX, unit_QX)
dotY = np.dot(unit_TY, unit_QY)
dotZ = np.dot(unit_TZ, unit_QZ)
angleX = np.rad2deg(np.arccos(dotX))
angleY = np.rad2deg(np.arccos(dotY))
angleZ = np.rad2deg(np.arccos(dotZ))
# Visualization of T axes
T_Pos_X = [T_Pos[0]+10, T_Pos[1], T_Pos[2]]
self.T_Plot_X = gl.GLLinePlotItem(pos=np.array([T_Pos, T_Pos_X]), color=(1,0,0,1), width=1, antialias=False)
w.addItem(self.T_Plot_X)
T_Pos_Y = [T_Pos[0], T_Pos[1]+10, T_Pos[2]]
self.T_Plot_Y = gl.GLLinePlotItem(pos=np.array([T_Pos, T_Pos_Y]), color=(0,1,0,1), width=1, antialias=False)
w.addItem(self.T_Plot_Y)
T_Pos_Z = [T_Pos[0], T_Pos[1], T_Pos[2]+10]
self.T_Plot_Z = gl.GLLinePlotItem(pos=np.array([T_Pos, T_Pos_Z]), color=(0,0,1,1), width=1, antialias=False)
w.addItem(self.T_Plot_Z)
# Visualization of Q axes
self.Q_Plot_X = gl.GLLinePlotItem(pos=np.array(Q_Line_X), color=(1,0,0,1), width=1, antialias=False)
w.addItem(self.Q_Plot_X)
self.Q_Plot_Y = gl.GLLinePlotItem(pos=np.array(Q_Line_Y), color=(0,1,0,1), width=1, antialias=False)
w.addItem(self.Q_Plot_Y)
self.Q_Plot_Z = gl.GLLinePlotItem(pos=np.array(Q_Line_Z), color=(0,0,1,1), width=1, antialias=False)
w.addItem(self.Q_Plot_Z)
tr1 = pg.Transform3D()
tr1.translate(*Q_Pos)
tr1.rotate(-angleX, 0, 0, 1)
tr1.rotate(angleZ, 1, 0, 0)
self.T_Plot_X.setTransform(tr1)
self.T_Plot_Y.setTransform(tr1)
self.T_Plot_Z.setTransform(tr1)
tr5 = pg.Transform3D()
tr5.translate(*Q_Pos)
tr5.rotate(-angleX, 0, 0, 1)
tr5.rotate(angleZ, 1, 0, 0)
self.object.setTransform(tr5)
self.graphLayout.addWidget(w)
self.setLayout(self.graphLayout)
if __name__ == '__main__':
app = QApplication(sys.argv)
ex = MainWindow()
ex.show()
sys.exit(app.exec_())
【问题讨论】：
【参考方案1】：试试这个： another = verts + 2.0 * np.cross(Q_Quat[1:4], np.cross(Q_Quat[1:4], verts) + Q_Quat[0] * verts)
【讨论】：
谢谢！让它对所有顶点使用以下循环 arr = np.empty((0, 3), int) for i in range(len(verts)): another = verts[i] + 2.0 * np.cross(quat [1:4], np.cross(quat[1:4], verts[i]) + quat[0] * verts[i]) arr = np.append(arr, np.array([another]), axis=0) tr = pg.Transform3D() tr.translate(*pos)【参考方案2】：几何——你有 3 个点： mpo 是 meshPointOrigin mpe 是 meshPointEnd hp 是 headPoint -- 你有 2 个向量： V1=(mpe-mpo) 从 mpo 到 mpe V2=(hp-mpo) 从 mpo 到 hp -- 您想将 V1 旋转到 V2。 单位法线向量为 VN = (V1 cross V2).normalize() 角度的余弦为 V1 dot V2 从这里你可以制作四元数：
kcos = sqrt((1+cosine)/2) (half-angle formula)
ksin = sqrt(1-ksin*ksin)
quaternionWXYZ = (kcos, ksin*VN.x, ksin*VN.y, ksin*VN.z)
如果 V1 的长度与 V2 不同，则必须缩放头部。 （编辑：这不会让头部左转/右转，是吗？这可能只是一个开始。）
【讨论】：
我尝试实施您的建议，但我不确定具体该怎么做。如何使用四元数WXYZ 旋转网格？ @Frederik Petri 对不起，我不做 python 或 opengl。我做 javascript/three.js。肯定有某种“applyQuaternion”函数。【参考方案3】：创建 4x4 homogenuous transform matrices A,B 代表您的 2 个坐标系
如果您有原点和 xis 基向量，只需将其输入矩阵 ...
计算将A 转换为B 的转换C
A * C = B // Inverse(A)*
Inverse(A) * A * C = Inverse(A)* B
C = Inverse(A)* B
就是这样……
【讨论】：
嗨@Spektre。感谢您的回答。我没有练习过数学或 Python，但是，我在您在第 6 节中发布的链接中读到“如果您想要 glRotate 那么您应该使用四元数，因为这是围绕轴旋转而不是 3 个角度！”。所以我不知道 4x4 同质变换矩阵是否会起作用，因为我正在使用 glRotate？ @FrederikPetri glRotate 将旋转 4x4 矩阵......它内部使用的数学无关......如果你已经有你的 2 个坐标系，你总是可以创建它们的矩阵。并且对齐更容易......如果你只有旋转矩阵（来自四元数）你仍然可以制作 4x4 变换矩阵你只需要知道原点（翻译）如果不是你可以使用(0,0,0) 并纠正后者的偏移量
嗨@Spektre - 所以我有 A 和 B 的 4x4 矩阵。我不太确定下一步该做什么 - 我正在使用旋转（角度，x，y，z）。我认为它与 glRotate 相同。你能解释一下如何计算转换 C 吗？然后如何应用旋转（角度，x，y，z）？对不起，我的理解不好:)
@FrederikPetri 计算C = Inverse(A)* B，然后当您想将点从A 转换为B 时，只需将“实际”A 矩阵乘以C 即可
@FrederikPetri 也许这个Problem superimposing and aligning 3D triangles 会为你解决问题......但它的一个更高级别的问题，因为那里的矩阵是未知的。以上是关于如何找到将 X、Y、Z 向量与另一个坐标系对齐的角度的主要内容，如果未能解决你的问题，请参考以下文章