空间直角坐标系旋转点的坐标变化（一）

空间直角坐标系旋转，空间中点的坐标变化如下，坐标系xyz旋转后到坐标系x'y'z'变换矩阵如下表

	x	y	z
x'	$\alpha$ 1	$\beta$ 1	$\gamma$ 1
y'	$\alpha$ 2	$\beta$ 2	$\gamma$ 2
z'	$\alpha$ 3	$\beta$ 3	$\gamma$ 3

那么 $\begin{bmatrix} x'\\ y'\\ z' \end{bmatrix}=\begin{bmatrix} cos(\alpha 1) &cos(\beta 1) &cos(\gamma 1) \\ cos(\alpha 2) &cos(\beta 2) &cos(\gamma 2) \\ cos(\alpha 3) &cos(\beta 3) &cos(\gamma 3) \end{bmatrix}\begin{bmatrix} x\\ y\\ z \end{bmatrix}$

空间直角坐标系绕x轴顺时针旋转θ空间坐标的变化如下

	x	y	z
x'	0°	90°	90°
y'	90°	θ°	θ+90°
z'	90°	90°-θ	θ°

那么 $\begin{bmatrix} x'\\ y'\\ z' \end{bmatrix}=\begin{bmatrix} 1 &0 &0 \\ 0&cos(\Theta ) &cos(\Theta +90°) \\ 0&cos(90-\Theta ) &cos(\Theta ) \end{bmatrix}\begin{bmatrix} x\\ y\\ z \end{bmatrix}$ 变换后 $\begin{bmatrix} x'\\ y'\\ z' \end{bmatrix}=\begin{bmatrix} 1 &0 &0 \\ 0&cos(\Theta ) &-sin(\Theta) \\ 0&sin(\Theta ) &cos(\Theta ) \end{bmatrix}\begin{bmatrix} x\\ y\\ z \end{bmatrix}$

python实现对3d点云数据进行坐标变换

import open3d as o3d
import numpy as np
import math as m

def exchange_coor(pcd,theta_yz):
    '''
    绕x轴顺时针旋转theta_yz度点云坐标转换
    :param pcd: 原始点云
    :param theta_yz: 旋转的角度
    :return: 变换坐标后的点云
    '''
    point_arr =np.asarray(pcd.points)
    color_arr =np.asarray(pcd.colors)
    ex_arr = np.array([[1,0,0],[0,m.cos(theta_yz),-m.sin(theta_yz)],[0,m.sin(theta_yz),m.cos(theta_yz)]])
    print(ex_arr)
    point_new =ex_arr.dot(np.transpose(point_arr))
    pcd_new = o3d.geometry.PointCloud()
    pcd_new.points = o3d.utility.Vector3dVector(np.transpose(point_new))
    pcd_new.colors = o3d.utility.Vector3dVector(color_arr)
    return pcd_new

if __name__ == '__main__':
    ply_path =r'D:\data\fabricated_building\mech_mind_plysave\1600-13.ply'
    pcd = o3d.io.read_point_cloud(ply_path)
    theta_yz = -15*np.pi/180
    pcd_new = exchange_coor(pcd,theta_yz)
    o3d.io.write_point_cloud('ex_coor.pcd', pcd_new)

猜你喜欢

目录

热门文章