Question

我正在尝试计算4x4旋转和平移矩阵以将一组3D点与另一组3D点对齐（示例显示2D情况）。我编写的代码可以在(0, 0)周围旋转或在翻译时正常工作，但在围绕任意枢轴点旋转时不能正常工作。

我已经在各个地方（例如here和here）阅读了你需要减去原点的偏移但是这不是我所追求的，因为它将旋转的形状返回到它的原始起源。

这些示例的旋转和平移是否可以存储在一个4x4矩阵中？

import numpy as np
import matplotlib.pyplot as plt
from matplotlib import collections

def rotation(r):
    return np.asarray([np.cos(r), -np.sin(r), np.sin(r), np.cos(r)]).reshape(2, 2)

def calculate_rotation(A, B, R_=np.arange(0, 360)):
    yaw_rotation = np.deg2rad(R_) # Brute force!
    diff = np.zeros(len(yaw_rotation))
    for i, a in enumerate(yaw_rotation):
        C = B.copy()
        C[:, :2] = np.dot(C[:, :2], rotation(a))
        diff[i] = sum([((C[:, 0].copy() - A[:, 0])**2).mean(), 
                       ((C[:, 1].copy() - A[:, 1])**2).mean()])
    return -yaw_rotation[np.where(diff == diff.min())][0]

def rotation_matrix(rot=[0, 0, 0], tra=[0, 0, 0]):

    xA, yA, zA = rot

    rX = np.matrix([[1, 0, 0, 0], [0, np.cos(xA), -np.sin(xA), 0], [0, np.sin(xA), np.cos(xA), 0], [0, 0, 0, 1]])
    rY = np.matrix([[np.cos(yA), 0, np.sin(yA), 0], [0, 1, 0, 0], [-np.sin(yA), 0, np.cos(yA), 0], [0, 0, 0, 1]])
    rZ = np.matrix([[np.cos(zA), -np.sin(zA), 0, 0], [np.sin(zA), np.cos(zA), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
    r = np.dot(rX, np.dot(rY, rZ))

    t = np.identity(4)
    t[:3, 3] = tra

    return np.dot(t, r)

def plot_triangle(Tri, i, color='none'):

    ax = plt.subplot(1, 3, i)
    pc = collections.PolyCollection((Tri[:, :2], ), facecolor=color, alpha=.5)
    ax.add_collection(pc)

    for i, row in enumerate(Tri):
        ax.scatter(row[0], row[1])
        ax.text(row[0], row[1], i)

plt.figure(figsize=(12, 4))

X = [4, 0, 0]
Y = [0, 3, 0]
Z = [0, 0, 0]

A = np.vstack([X, Y, Z, np.ones(3)]).T

for i, (r_, t_) in enumerate([[np.deg2rad(33), [0, 0]],    # rotate
                              [0, [2, -2]],                # tranlate
                              [np.deg2rad(33), [-2, 1]]]): # combined

    Xoff, Yoff, Zoff, roll, pitch, yaw = np.zeros(6)

    # offset and rotate B
    B = A.copy()
    B[:, :2] = np.dot(B[:, :2], rotation(r_))
    B[:, 0] += t_[0]
    B[:, 1] += t_[1]
    C = B.copy() # so we can then apply solution to original B

    ################################################

    # calculate distance between objects
    Xoff = A[0, 0] - C[0, 0] 
    Yoff = A[0, 1] - C[0, 1] 
    Zoff = A[0, 2] - C[0, 2]

    # calculate and apply offset of A to 0...
    A_ = A.copy()
    A_[:, 0] -= A_[0, 0]
    A_[:, 1] -= A_[0, 1]
    A_[:, 2] -= A_[0, 2]

    # calculate offset of C to 0...
    Xrot = C[0, 0]
    Yrot = C[0, 1]
    Zrot = C[0, 2]
    C[:, 0] -= Xrot
    C[:, 1] -= Yrot
    C[:, 2] -= Zrot

    # calculate rotation
    yaw = calculate_rotation(A_, C)

    # generate 4x4 matrix
    M = rotation_matrix(rot=(roll, pitch, yaw), 
                        tra=(Xoff, Yoff, Zoff))

    D = np.asarray(np.dot(M, B.T).T[:, :3])

    plot_triangle(A[:, :3], i+1)
    plot_triangle(D, i+1, 'y')

此代码的输出是绿色三角形是起始位置，红色三角形与之对齐，黄色三角形是我所在的位置。

生成并应用原点不为（0,0）的4x4旋转和平移矩阵

0 个答案: