绕任意点的旋转矩阵推导与计算

nnzzll

已于 2022-07-07 17:52:01 修改

阅读量1.7k

点赞数 1

分类专栏： python 医学图像处理文章标签：矩阵线性代数算法

于 2022-07-07 16:57:31 首次发布

python 同时被 2 个专栏收录

12 篇文章

订阅专栏

医学图像处理

9 篇文章

订阅专栏

2D

在右手坐标系 $X O Y$ 下，点 $P (x, y)$ 绕任意点 $O′(xo,yo)O^\prime(x_{o},y_{o})$ 旋转 $θ\theta$ 角度(逆时针)得到点 $P′(x′,y′)P^\prime(x^\prime,y^\prime)$

令 $∣O′P∣=r|O^\prime P|=r$ , $O′P→\overrightarrow{O^\prime P}$ 与 $x$ 轴的夹角为 $α\alpha$
则有
$\left\{ \begin{matrix} x=rcos\alpha + x_{o} \\ y=rsin\alpha + y_{o} \\ \end{matrix} \right. \tag{1}$

$\left\{ \begin{matrix} x^\prime=rcos(\alpha+\theta) + x_{o} \\ y^\prime=rsin(\alpha+\theta) + y_{o} \\ \end{matrix} \right. \tag{2}$

将(1)代入(2)消去 $r$ 并展开，得
$\left\{ \begin{matrix} x^\prime=cos\theta x - sin\theta y + x_{o}(1-cos\theta)+y_{o}sin\theta \\ y^\prime=sin\theta x + cos\theta y + y_{o}(1-cos\theta)-x_{o}sin\theta \\ \end{matrix} \right.$

齐次旋转矩阵为
$[cosθ−sinθxo(1−cosθ)+yosinθsinθcosθyo(1−cosθ)−xosinθ001]\begin{bmatrix} cos\theta&-sin\theta&x_{o}(1-cos\theta)+y_{o}sin\theta \\ sin\theta&cos\theta&y_{o}(1-cos\theta)-x_{o}sin\theta\\ 0&0&1\\ \end{bmatrix}$
即

$\begin{bmatrix} x^\prime\\ y^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} cos\theta&-sin\theta&x_{o}(1-cos\theta)+y_{o}sin\theta \\ sin\theta&cos\theta&y_{o}(1-cos\theta)-x_{o}sin\theta\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ 1\\ \end{bmatrix}$

3D

点 $P (x, y, z)$ 绕任意点 $O′(xo,yo,zo)O^\prime(x_{o},y_{o},z_{o})$ 旋转矩阵 $R$ 可分解为 $Rx(θx)Ry(θy)Rz(θz)R_{x}(\theta_{x})R_{y}(\theta_{y})R_{z}(\theta_{z})$

由二维可以推广 $Rx(θx)R_{x}(\theta_{x})$ 在右手坐标系 $Y O Z$ 下进行,则线性变化为
$\begin{bmatrix} x^\prime\\ y^\prime\\ z^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} 1&0&0&0\\ 0&cos\theta_{x}&-sin\theta_{x}&y_{o}(1-cos\theta_{x})+z_{o}sin\theta_{x} \\ 0&sin\theta_{x}&cos\theta_{x}&z_{o}(1-cos\theta_{x})-y_{o}sin\theta_{x}\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}$

同理可以推广 $Ry(θy)R_{y}(\theta_{y})$ 在右手坐标系 $Z O X$ 下进行,则线性变化为
$\begin{bmatrix} x^\prime\\ y^\prime\\ z^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} cos\theta_{y}&0&sin\theta_{y}&x_{o}(1-cos\theta_{y})-z_{o}sin\theta_{y}\\ 0&1&0&0 \\ -sin\theta_{y}&0&cos\theta_{y}&z_{o}(1-cos\theta_{y})+x_{o}sin\theta_{y}\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}$

同理可以推广 $Rz(θz)R_{z}(\theta_{z})$ 在右手坐标系 $X O Y$ 下进行,则线性变化为
$\begin{bmatrix} x^\prime\\ y^\prime\\ z^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} cos\theta_{z}&-sin\theta_{z}&0&x_{o}(1-cos\theta_{z})+y_{o}sin\theta_{z}\\ sin\theta_{z}&cos\theta_{z}&0&y_{o}(1-cos\theta_{z})-x_{o}sin\theta_{z}\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}$

import numpy as np
from math import cos, sin


def Rx(x, y, z, theta):
    return np.array([
        [1, 0, 0, 0],
        [0, cos(theta), -sin(theta), y*(1-cos(theta))+z*sin(theta)],
        [0, sin(theta), cos(theta), z*(1-cos(theta))-y*sin(theta)],
        [0, 0, 0, 1]
    ])


def Ry(x, y, z, theta):
    return np.array([
        [cos(theta), 0, sin(theta), x*(1-cos(theta))-z*sin(theta)],
        [0, 1, 0, 0],
        [-sin(theta), 0, cos(theta),  z*(1-cos(theta))+x*sin(theta)],
        [0, 0, 0, 1]
    ])


def Rz(x, y, z, theta):
    return np.array([
        [cos(theta), -sin(theta), 0, x*(1-cos(theta))+y*sin(theta)],
        [sin(theta), cos(theta), 0, y*(1-cos(theta))-x*sin(theta)],
        [0,  0, 1, 0, ],
        [0, 0, 0, 1]
    ])

def rotate_matrix(points, theta):
    rx = Rx(points[0],points[1],points[2], theta[0])
    ry = Ry(points[0],points[1],points[2], theta[1])
    rz = Rz(points[0],points[1],points[2], theta[2])
    R = rx@ry@rz
    return R