如何找到将X，Y，Z向量与另一个坐标系对齐的角度

Question

我正在尝试旋转在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_())

Answer 1

尝试一下：另一个=顶点+ 2.0 * np.cross（Q_Quat [1：4]，np.cross（Q_Quat [1：4]，verts）+ Q_Quat [0] * verts）

Answer 2

几何-您有3个点：MPO是meshPointOriginmpe是meshPointEndhp是headPoint-您有2个向量：V1 =（MPE-MPO）从MPO到MPEV2 =（hp-mpo）从mpo到hp-您要将V1旋转到V2。单位法向向量为VN =（V1跨V2）.normalize（）角度的余弦为V1点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的长度不同，则必须缩放磁头。（编辑：这不会左右转动头，对吗？这可能只是一个开始。）

Answer 3

创建4x4 homogenuous transform matrices A,B表示您的2个坐标系
如果您有原点和xis基向量，只需将其输入矩阵...

计算将C转换为A]的转换B

             A * C =             B // Inverse(A)*
Inverse(A) * A * C = Inverse(A)* B
                 C = Inverse(A)* B

就是这样...

如何找到将X，Y，Z向量与另一个坐标系对齐的角度

问题描述投票：2回答：3

3个回答

最新问题

如何找到将X，Y，Z向量与另一个坐标系对齐的角度

问题描述 投票：2回答：3

3个回答

最新问题

问题描述投票：2回答：3