The homogeneous coordinate system describes the coordinate system and position of the rigid body, and also provides a set of methods for relative rotation, relative movement, absolute rotation, and absolute movement. It is the best choice for drawing rotated 3D stereo

homogeneous coordinate system

Denote the three axes of the Cartesian coordinate system as $xyz$ , and denote any homogeneous coordinate system as $normal$

We use such a matrix to $normal$ describe $xyz$ the relationship between the coordinate system and the coordinate system:

$S_{noa} = \begin{bmatrix} R_{noa} & \vec{p}\\vec{0} & 1 \end{bmatrix} = \begin{bmatrix}n_x&o_x&a_x&x\\n_y & o_y & a_y & y\\ n_z & o_z & a_z & z\\ 0 & 0 & 0 & 1 \end{bmatrix}$

It represents $\vec{p}=\begin{bmatrix} x & y & z \end{bmatrix}^T$ the absolute position of the origin of the coordinate system in the coordinate system, and represents the direction vector of the n-axis in the coordinate system (and is a unit vector), and the o-axis and the a-axis are the same $normal$ $xyz$ $\vec{n} = \begin{bmatrix} n_x & n_y & n_z \end{bmatrix}$ $xyz$

In addition, we can complete the transformation of the aligned sub-coordinate system through a homogeneous transformation matrix, which includes a rotation matrix ( $R_x, R_y, R_z$ ) and a translation matrix ( $T$ ):

$R_x= \begin{bmatrix} 1&&&\\&\cos{\theta}&- \sin{\theta}&\\&\sin{\theta}&\cos{\theta}&\\&& & 1 \end{bmatrix}, R_y= \begin{bmatrix} \cos{\theta} & & \sin{\theta} & \\ & 1 & & \\ - \sin{\theta} & & \cos{ \theta}&\\&&&1\end{bmatrix}$

$R_z= \begin{bmatrix}\cos{\theta}& - \sin{\theta}&&\\\sin{\theta}&\cos{\theta}&&\\&&1&\\&& & 1 \end{bmatrix}, T=\begin{bmatrix} 1 & & & dx\\ & 1 & & dy\\ & & 1 & dz\\ & & & 1 \end{bmatrix}$

Taking the rotation matrix $R_x$ as an example, $R_x \times S_{noa}$ it means that $normal$ the coordinate system rotates around the x-axis (that is, absolute transformation), $S_{noa} \times R_x$ and that $normal$ the coordinate system rotates around the n-axis (that is, relative transformation)

Usually when drawing, what we need to pay attention to is the relative positional relationship between the various components of each graph (such as the mechanical arm: https://hebitzj.blog.csdn.net/article/details/123810092 ), the homogeneous transformation matrix provides The homogeneous coordinate system transformation method can obviously meet our needs

Graphic Transformation Principle

In the case of a known homogeneous coordinate system $normal$ , how to draw graphics on this coordinate system?

$S_{normal}$ The subarray in $R_{normal}$ describes the components of $normal$ the three axes on $xyz$ the coordinate system, for example: $n_x$ describes the components of the n-axis on the x-axis, $o_x$ describes the components of the o-axis on the x-axis, and $a_x$ describes the components of the a-axis on the x-axis

$normal$ If a point in the coordinate system is given $\beta = \begin{bmatrix} n' & o' & a' \end{bmatrix}$ , its $xyz$ x-coordinate in the coordinate system is:

$x' = n'n_x+o'o_x + a'a_x$

The same is true for the y and z coordinates of the point, then the following transformation converts $\beta = \begin{bmatrix} n' & o' & a' \end{bmatrix}$ the point $normal$ from coordinates to $xyz$ coordinates:

$\begin{bmatrix} x' & y' & z' \end{bmatrix} =\beta \times R_{noa}^T + \vec{p}^T = \begin{bmatrix} n' & o' & a' \end{bmatrix} \times \begin{bmatrix} n_x & n_y & n_z\\ o_x & o_y & o_z\\ a_x & a_y & a_z \end{bmatrix} + \begin{bmatrix} x & y & z \end{bmatrix}$

The parameters of plot_surface and voxels in matplotlib are point sets in three-dimensional space

We can first draw the graph at the origin of the coordinates (as drawn in the coordinate system), and then after the matrix corresponding to the $normal$ homogeneous coordinate system is given , use the above equation to solve the coordinates of each point of the graph in the coordinate system $normal$ $xyz$

Of course, this conclusion can also be extended to a two-dimensional homogeneous coordinate system

core code

In order to realize the seamless connection between matplotlib drawing and homogeneous coordinate system, I wrote the CoordSys_3d class, and the functions of each class method are as follows:

trans: Given an offset on the xyz axis, generate a translation transformation matrix
rot: Given the rotation angle and rotation axis, generate a rotation transformation matrix
abs_tf: Input the transformation matrix generated by trans and rot, and perform absolute transformation
rela_tf: Input the transformation matrix generated by trans and rot, and perform relative transformation
apply: given the xyz matrix describing the surface/solid, translate and rotate the surface/solid according to the homogeneous coordinate system matrix

from typing import Union

import matplotlib.pyplot as plt
import numpy as np


class _CoordSys_nd:
    dtype = np.float16
    dim = None
    # 位置, 各个轴的方向向量
    position = property(fget=lambda self: self.s[:self.dim, -1])
    direction = property(fget=lambda self: self.s[:self.dim, :self.dim])

    def __init__(self, state: np.ndarray = None):
        size = self.dim + 1
        self.s = np.eye(size, dtype=self.dtype)
        # 使用非空形参
        if isinstance(state, np.ndarray):
            assert state.shape == self.s.shape
            self.s = state

    def abs_tf(self, tf):
        ''' 绝对变换'''
        return type(self)(tf @ self.s)

    def rela_tf(self, tf):
        ''' 相对变换'''
        return type(self)(self.s @ tf)

    def apply(self, *coords) -> tuple:
        ''' 局部坐标值 -> 全局坐标值'''
        xyz = np.stack(coords, axis=-1) @ self.direction.T + self.position
        return tuple(i[..., 0] for i in np.split(xyz, self.dim, axis=-1))

    def plot_coord_sys(self, length=.5, linewidth=None,
                       colors=['orangered', 'deepskyblue', 'greenyellow'], labels='xyz'):
        ''' 绘制局部坐标系'''
        pos = self.position
        axis = self.direction.T * length
        for i in range(self.dim):
            plt.plot(*zip(pos, pos + axis[i]), c=colors[i], label=labels[i], linewidth=linewidth)

    def __str__(self):
        return str(self.s) + '\n'

    __repr__ = __str__


class CoordSys_2d(_CoordSys_nd):
    dim = 2

    def apply(self, x: np.ndarray, y: np.ndarray) -> tuple:
        ''' 局部坐标值 -> 全局坐标值'''
        return super().apply(x, y)

    def transform(self, dx: float = 0., dy: float = 0.,
                  theta: float = 0, relative: bool = True):
        ''' dx,dy: 平移变换的参数
            theta: 旋转变换的参数
            relative: 是否使用相对变换'''
        # 绕 z 轴旋转, 并平移
        mat = np.concatenate((np.eye(3, 2, dtype=self.dtype),
                              np.array((dx, dy, 1))[:, None]), axis=-1)
        if theta:
            theta = np.deg2rad(theta)
            cos, sin = np.cos(theta), np.sin(theta)
            mat[:2, :2] = np.array([[cos, -sin], [sin, cos]])
        return (self.rela_tf if relative else self.abs_tf)(mat)


class CoordSys_3d(_CoordSys_nd):
    dim = 3

    def apply(self, x: np.ndarray, y: np.ndarray, z: np.ndarray) -> tuple:
        ''' 局部坐标值 -> 全局坐标值'''
        return super().apply(x, y, z)

    @classmethod
    def trans(cls, dx: float = 0., dy: float = 0., dz: float = 0.) -> np.ndarray:
        ''' 齐次变换矩阵: 平移'''
        return np.concatenate((np.eye(4, 3, dtype=cls.dtype),
                               np.array((dx, dy, dz, 1))[:, None]), axis=-1)

    @classmethod
    def rot(cls, theta: float, axis: Union[int, str]) -> np.ndarray:
        ''' 齐次变换矩阵: 旋转'''
        mat, theta = np.eye(4, dtype=cls.dtype), np.deg2rad(theta)
        cos, sin = np.cos(theta), np.sin(theta)
        axis = 'xyz'.index(axis) if isinstance(axis, str) else axis
        if axis == 0:
            mat[1: 3, 1: 3] = np.array([[cos, -sin], [sin, cos]])
        elif axis == 1:
            mat[:3, :3] = np.array([[cos, 0, sin], [0, 1, 0], [-sin, 0, cos]])
        elif axis == 2:
            mat[:2, :2] = np.array([[cos, -sin], [sin, cos]])
        else:
            raise AssertionError(f'axis {axis} is out of bounds for 3 dimensions')
        return mat


if __name__ == '__main__':
    rot = CoordSys_3d.rot
    trans = CoordSys_3d.trans

    state = CoordSys_3d()
    # 相对变换
    state = state.rela_tf(rot(30, 'y'))
    print(state)
    # 绝对变换
    state = state.abs_tf(trans(dx=2, dy=3, dz=4))
    print(state)

Next, define two functions to draw surfaces and solids respectively, and verify our method:

cylinder: Draw a hollow cylinder, which is composed of inner and outer surfaces, upper and lower bottom surfaces, and call the plot_surface function 4 times to draw; in the initial state, the main axis of the hollow cylinder and the normal vectors of the two bottom surfaces are both z-axis
rubik_cube: Draw a hollow Rubik's cube, which is composed of several cubes, and will call the voxels function once to draw

The commonality of these two functions is that first centering on the origin (this is very important, the effect can be fine-tuned after the effect meets expectations) to generate an xyz matrix describing the surface and three-dimensional , and then use the apply function of the CoordTF object to transform the xyz matrix to complete the transformation. Translation and rotation operations of 3D images

import matplotlib.pyplot as plt
import numpy as np

from coord import CoordSys_3d

red = 'orangered'
orange = 'orange'
yellow = 'yellow'
green = 'greenyellow'
cyan = 'aqua'
blue = 'deepskyblue'
purple = 'mediumpurple'
pink = 'violet'

ROUND_EDGE = 30  # 圆等效多边形边数
DTYPE = np.float16  # 矩阵使用的数据类型


def figure3d():
    ''' 创建3d工作站'''
    figure = plt.subplot(projection='3d')
    tuple(getattr(figure, f'set_{i}label')(i) for i in 'xyz')
    return figure


def cylinder(figure, state: CoordSys_3d,
             R: float, h: float, r: float = 0,
             smooth: int = 2, **plot_kwd):
    ''' 以 state 的 z 轴为主轴绘制圆柱
        figure: 3D 工作站对象
        state: CoordSys_3d 齐次变换矩阵
        R: 圆柱底面外径
        r: 圆柱底面内径
        h: 圆柱高度
        smooth: 图像细致程度 (至少 2)'''
    theta = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    z = np.linspace(-h / 2, h / 2, smooth, dtype=DTYPE)
    theta, z = np.meshgrid(theta, z)
    # 绘制圆柱内外曲面: 以 z 轴为主轴, 原点为中心
    x, y = np.cos(theta), np.sin(theta)
    figure.plot_surface(*state.apply(x * R, y * R, z), **plot_kwd)
    figure.plot_surface(*state.apply(x * r, y * r, z), **plot_kwd)

    phi = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    radius = np.linspace(r, R, 2, dtype=DTYPE)
    phi, radius = np.meshgrid(phi, radius)
    # 绘制上下两底面: 法向量为 z 轴, 原点为中心, 在 z 轴上偏移得到两底面
    x, y = np.cos(phi) * radius, np.sin(phi) * radius
    z = np.zeros_like(x)
    for dz in (-h / 2, h / 2):
        s = state.rela_tf(CoordSys_3d.trans(dz=dz))
        figure.plot_surface(*s.apply(x, y, z), **plot_kwd)


def rubik_cube(figure, state: CoordSys_3d,
               length: float, hollow: float = 0.7, smooth: int = 10,
               colors: list = [red, orange, yellow, green, cyan, blue, purple, pink], **plot_kwd):
    ''' 绘制魔方
        length: 边长
        smooth: 魔方的细粒度'''
    x = np.linspace(-length / 2, length / 2, smooth + 1)
    filled = np.random.random([smooth] * 3) > hollow
    color = np.random.choice(colors, size=filled.shape)
    # 绘制各个通道
    figure.voxels(*state.apply(*np.meshgrid(x, x, x)), filled=filled,
                  facecolors=color, edgecolors='white', **plot_kwd)
    return figure


if __name__ == '__main__':
    plt.rcParams['figure.figsize'] = [6.4, 6.4]

    fig = figure3d()
    fig.set_xlim((-6, 4))
    fig.set_ylim((-3, 7))
    fig.set_zlim((-5, 5))

    rot = CoordSys_3d.rot
    trans = CoordSys_3d.trans

    # 绕 y 轴相对旋转 20°, 再绝对平移
    state = CoordSys_3d().rela_tf(rot(20, 'y')).abs_tf(trans(dx=-1, dy=2, dz=-2))
    print(state)
    # 以 z 轴为主轴, 绘制空心圆柱
    cylinder(fig, state=state, R=5, r=4, h=3, cmap='Set3', alpha=0.5)
    # 绘制局部坐标系
    state.plot_coord_sys(length=10, linewidth=5), plt.legend()

    # 在空心圆柱的 z 轴上平移
    state = state.rela_tf(trans(dz=5))
    print(state)
    # 绘制空心魔方
    rubik_cube(fig, state=state, length=6, hollow=0.8, smooth=10, alpha=0.6)
    plt.show()

The final drawing effect is shown in the figure above, end

Matplotlib homogeneous coordinate system draws rotated 3D stereo

homogeneous coordinate system

Graphic Transformation Principle

core code

Guess you like