Rotation Family in Transformation 几何变换中的各种旋转

2022-07-25 00:35:30 阅读：216 来源： 互联网

标签：rad Family vTo 旋转几何变换 vFrom np Rotation axis

Rotation

1 Rotate with aligned axis

在3D坐标系下的旋转，可以是以一种分别沿着x-axis, y-axis, z-axis旋转$\alpha$, $\beta$, $\gamma$角度得到各自旋转，然后进行矩阵组合：

python code

def rotate_with_aligned_axis(alpha, beta, gamma):
    alpha_rad = np.deg2rad(alpha)
    beta_rad = np.deg2rad(beta)
    gamma_rad = np.deg2rad(gamma)

    R_x = np.eye(4, dtype=np.float32)
    R_y = np.eye(4, dtype=np.float32)
    R_z = np.eye(4, dtype=np.float32)

    R_x[1, 1] = np.cos(alpha_rad); R_x[1, 2] = - np.sin(alpha_rad)
    R_x[2, 1] = np.sin(alpha_rad); R_x[2, 2] = np.cos(alpha_rad)

    R_y[0, 0] = np.cos(beta_rad); R_y[0, 2] = np.sin(beta_rad)
    R_y[2, 0] = - np.sin(beta_rad); R_y[2, 2] = np.cos(beta_rad)

    R_z[0, 0] = np.cos(gamma_rad); R_z[0, 2] = - np.sin(gamma_rad)
    R_z[1, 0] = np.sin(gamma_rad); R_z[1, 1] = np.cos(gamma_rad)

    return np.matmul(R_x, np.matmul(R_y, R_z))

2 Rotate with arbitrary axis

只是绕着aligned-axis旋转，会存在万向节锁的问题，如果绕着任意axis (vector in 3d) 旋转的话，更加高效。

绕任意轴旋转，根据罗德里戈公式（推导过程见旋转之二 - 三维空间中的旋转:罗德里格旋转公式）

python code

def rotate_with_arbitrary_axis(axis, theta):
    theta_rad = np.deg2rad(theta)
    axis = axis / np.linalg.norm(axis)
    axis = np.reshape(axis, (3, 1))

    N = np.zeros(shape=(3, 3), dtype=np.float32)
    N[0, 1] = - axis[2]; N[0, 2] = axis[1]
    N[1, 0] = axis[2]; N[1, 2] = - axis[0]
    N[2, 0] = - axis[1]; N[2, 1] = axis[0]

    R = np.cos(theta_rad) * np.eye(3, dtype=np.float32) + (1 - np.cos(theta_rad)) * np.matmul(axis, axis.T) + np.sin(theta_rad) * N

    return R

3 Rotate from vFrom to vTo

在平常开发中，经常会有需要计算从一个向量vFrom到另一个向量vTo的旋转矩阵的情况，本着拿来主义，这里的公式是：

python code

def rotate_vFrom_to_vTo(vFrom, vTo):
    vFrom = vFrom / np.linalg.norm(vFrom)
    vTo = vTo / np.linalg.norm(vTo)

    c = np.dot(vFrom, vTo)
    R = np.eye(3, dtype=np.float32)

    if np.abs(c - 1.0) < 1e-4 or np.abs(c + 1.0) < 1e-4:
        return R
    
    v = np.cross(vFrom, vTo)
    v_x = np.array([
        [0., -v[2], v[1]],
        [v[2], 0., -v[0]],
        [-v[1], v[0], 0.]
    ], dtype=np.float32)

    R += v_x + v_x @ v_x / (1 + c)

    return R

4 Rotate from one points set to another

经典面试问题：给你一堆点集$\mathbf{X}$, 再给你经过旋转、平移变换过后的点集$\mathbf{Y}$, 求从$\mathbf{X}$到$\mathbf{Y}$的旋转和平移。

参考论文Least-Squares Fitting of Two 3-D Point Sets, 求解步骤：

python code

def compute_transformation(X, Y):
    """
    X -- [N, 3]
    Y -- [N, 3]
    return R|t
    """
    X_mean = np.mean(X, axis=0)
    Y_mean = np.mean(Y, axis=0)

    X_hat = X - X_mean
    Y_hat = Y - Y_mean
    s = np.sum(np.linalg.norm(Y_hat, axis=1), axis=0) / np.sum(np.linalg.norm(X_hat, axis=1), axis=0)

    A = s * X_hat.T @ Y_hat
    U, _, Vh = np.linalg.svd(A, full_matrices=True)

    R = Vh.T @ U.T

    t = Y_mean - s * R @ X_mean

    return s, R, t

5 Code url

rotation.py

6. Reference

标签：rad,Family,vTo,旋转,几何变换,vFrom,np,Rotation,axis
来源： https://www.cnblogs.com/XiWJ/p/16516020.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9