【open3d】点云旋转

索引

一、原理
二、源码
三、结果

---

一、原理

这里是引用

1. xxx

---

二、源码

"""
    根据旋转轴两个端点、旋转角度，得到旋转矩阵
    v1   旋转轴端点1 (-130.009995,-158.779999,31.349001)
    v2   旋转轴端点2 (-129.216995,-157.925003,31.320999)
    theta 旋转弧度 60度
"""
def rotateMatrix(v1, v2, theta):
    # 向量相减
    p = np.asarray(v2) - np.asarray(v1)
    # 求单位向量
    rand_axis = p / np.linalg.norm(p)
    # 旋转角度
    r = math.radians(theta)
    # 返回旋转矩阵
    rot_matrix = linalg.expm(np.cross(np.eye(3), rand_axis * r))

    return rot_matrix


"""
    点云旋转
    file_path  pcd 格式点云文件
    point1   旋转轴端点1 (-130.009995,-158.779999,31.349001)
    point2   旋转轴端点2 (-129.216995,-157.925003,31.320999)
    rotate_angle 旋转弧度 60度
    save_path   旋转后pcd 格式保存的点云地址
"""
def pointCloud_rotate(file_path, point1, point2, rotate_angle, save_path):
    pcd = o3d.io.read_point_cloud(file_path)
    points=np.asarray(pcd.points)
    np_max = np.where(points[:, 2] >= points[:, 2].max())[0]
    index=pcd.select_by_index(np_max)
    rotate_center=tuple((index.points)[0])
    print("rotate_center",rotate_center)
    ro_pcd = deepcopy(pcd)
    # R = pcd.get_rotation_matrix_from_xyz(rotate_angle)
    R = rotateMatrix(point1,point2,rotate_angle)
    print(R)
    ro_pcd.rotate(R, center=rotate_center)

    crop__rotation_pcd_address=save_path+now+"crop_ro_.pcd"
    o3d.io.write_point_cloud(crop__rotation_pcd_address, ro_pcd)

    return crop__rotation_pcd_address
    

---

三、结果