Open3d获取到的包围盒的包围盒八点的顺序是三角面,一遍走完8点的排序,我想要的是底部四点+顶部四点最后合成的八个numpy点
思路:将八点按Z轴值分为上四点和下四点,然后再分别排序,最后再组合输出
核心代码:
# 根据 Z 值对点进行排序
sorted_points = bottom_points_np[np.argsort(bottom_points_np[:, 2])]
# 获取 Z 值最小的四个点
points = sorted_points[:4]
# 计算中心点坐标
center = np.mean(points, axis=0)
# 计算每个点相对于中心点的极坐标角度np.arctan2(y,x)
angles = np.arctan2(points[:, 1] - center[1], points[:, 0] - center[0])
# 对角度进行排序
sorted_indices = np.argsort(angles)
points = points[sorted_indices]
import json
import copy
import numpy as np
import open3d as o3d
'''
作者 : cscec_yhj;
日期 : 2024年1月27日;
功能 : 获取点云包围盒的八个numpy点,从下四角开始到上四角;
输入 : numpy格式点云;
输出 : bottom = 排序好的np点云
'''
# 2023年9月18日-获取box八个点的排序,从下四角开始到上四角
def get_SequentPoint_from_boxPoints(bottom_points_np):
'''
:param bottom_points_np: 8个numpy点
:return: 排好序的八个点
'''
# 从点云中提取点的坐标
# obox = point_cloud.get_oriented_bounding_box()
# bottom_points_np = np.array(obox.get_box_points())
# 根据 Z 值对点进行排序
sorted_points = bottom_points_np[np.argsort(bottom_points_np[:, 2])]
# 获取 Z 值最小的四个点
points = sorted_points[:4]
# 获取 Z 值最大的四个点
points_max = sorted_points[4:]
# 计算中心点坐标
center = np.mean(points, axis=0)
center_max = np.mean(points_max, axis=0)
# 计算每个点相对于中心点的极坐标角度
angles = np.arctan2(points[:, 1] - center[1], points[:, 0] - center[0])
angles_max = np.arctan2(points_max[:, 1] - center_max[1], points_max[:, 0] - center_max[0])
# 对角度进行排序
sorted_indices = np.argsort(angles)
sorted_indices_max = np.argsort(angles_max)
points = points[sorted_indices]
points_max = points_max[sorted_indices_max]
# 按排序后的顺序连接点
bottom = np.empty([8, 3], dtype=float)
bottom[0] = points[0]
bottom[1] = points[1]
bottom[2] = points[2]
bottom[3] = points[3]
bottom[4] = points_max[0]
bottom[5] = points_max[1]
bottom[6] = points_max[2]
bottom[7] = points_max[3]
return bottom
if __name__ == "__main__":
# 【加载点云】
print("->正在加载点云... ")
pcd = o3d.io.read_point_cloud(r"..\Data\PCD\球杆点云局部.pcd")
print(pcd)
#o3d.visualization.draw_geometries([pcd])
pcd.paint_uniform_color([0.5, 0.5, 0.5])
# 点云转numpy
obx = pcd.get_minimal_oriented_bounding_box()
points = np.array(obx.get_box_points())
seq_points = get_SequentPoint_from_boxPoints(points)
# numpy转点云
pcd_1=o3d.geometry.PointCloud()
pcd_1.points = o3d.utility.Vector3dVector(seq_points)
o3d.visualization.draw_geometries([pcd_1.paint_uniform_color([1, 0, 0])],window_name='Open3D', width=1920, height=1080, left=50, top=50, point_show_normal=True, mesh_show_wireframe=True, mesh_show_back_face=True)