pycharm+python3.7+open3d对点云进行坐标变换代码

import numpy as np
import open3d as o3d
from math import pi

def main():
    # 深度相机坐标系到机械臂末端坐标系的位姿转换矩阵
    hand_eye_matrix = np.array([
        [-0.082, 0.997, -0.009, 68.668],
        [-0.997, -0.082, -0.014, -53.108],
        [-0.015, 0.008, 0.999, -170.343],
        [0, 0, 0, 1]
    ])

    # 输入的相机拍照位姿参数
    x, y, z = 723.958, -746.691, 630.351  # 位置坐标
    roll, pitch, yaw = -162.390, -24.240, -53.250  # 旋转角度

    # 将角度从度转换为弧度
    yaw_rad = yaw * pi / 180.0
    pitch_rad = pitch * pi / 180.0
    roll_rad = roll * pi / 180.0

    # 构建旋转矩阵
    Rz = np.array([
        [np.cos(yaw_rad), -np.sin(yaw_rad), 0],
        [np.sin(yaw_rad), np.cos(yaw_rad), 0],
        [0, 0, 1]
    ])
    Ry = np.array([
        [np.cos(pitch_rad), 0, np.sin(pitch_rad)],
        [0, 1, 0],
        [-np.sin(pitch_rad), 0, np.cos(pitch_rad)]
    ])
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(roll_rad), -np.sin(roll_rad)],
        [0, np.sin(roll_rad), np.cos(roll_rad)]
    ])

    # 组合旋转矩阵，假设旋转顺序是ZYX
    R = Rz @ Ry @ Rx

    # 构建位姿转换矩阵
    T_end_effector_to_base = np.eye(4)
    T_end_effector_to_base[:3, :3] = R
    T_end_effector_to_base[:3, 3] = [x, y, z]

    # 读取点云文件
    cloud_path = "F:/24.10.181/24.10.181/1/PointCloud.ply"
    if not o3d.io.read_point_cloud(cloud_path):
        print(f"Couldn't read file {cloud_path}")
        return

    cloud = o3d.io.read_point_cloud(cloud_path)

    # 将点云从相机坐标系转换到机械臂末端坐标系
    transformed_cloud = cloud.transform(hand_eye_matrix)

    # 将点云从机械臂末端坐标系转换到机械臂基坐标系
    final_cloud = transformed_cloud.transform(T_end_effector_to_base)

    # 保存转换后的点云到文件
    output_path = "F:/24.10.181/24.10.181/1/1.ply"
    o3d.io.write_point_cloud(output_path, final_cloud)
    print(f"Point cloud transformation and saving completed successfully to {output_path}")

if __name__ == "__main__":
    main()

需要安装python3.7

pip install python

 需要安装open3d

pip install open3d

 我还装了scipy

pip install scipy -i https://pypi.tuna.tsinghua.edu.cn/simple