对于分割大场景点云,为了初步提取中心部分点云可以采用包围盒缩小快速剔除点云的方式
思路:
获取点云包围盒——缩小包围盒——提取包围盒内的点云
核心代码:
1.求得底部角点到中心的四个向量 vec = (bottom_center - pt )/ np.linalg.norm(bottom_center - pt) 3.新的八角点去构建新的box,而后裁剪 |
如果需要四角都偏移,则改为八点向质心移动,而后构建BOX
如果要其他类型的包围盒偏移,则修改obx = pcd.get_minimal_oriented_bounding_box ()即可。
完整代码:
import json
import copy
import numpy as np
import open3d as o3d
'''
作者 : cscec_yhj;
日期 : 2024年1月27日;
功能 : 获取点云有向最小包围盒底部向着底部质心缩小一定范围后的内部点云或者外部点云,高度方向不变;
输入 : pcd格式点云,缩小的范围;
输出 : out_pcd = 缩小的包围盒到原来的包围盒之间的点云
in_pcd = 缩小的包围盒内的点云
obx_pcd_obx,obx = 点云的缩小包围盒,点云的包围盒
'''
def get_pcd_by_reduce_box(pcd,distance=5000):
def get_min_max_horizontal_height(pcd):
# 获取点云的包围盒
obb = pcd.get_axis_aligned_bounding_box()
# 从包围盒坐标中获取最高和最低水平高度
obb_points = np.asarray(obb.get_box_points())
min_horizontal_height = np.min(obb_points[:, 2]) # 最低水平高度
max_horizontal_height = np.max(obb_points[:, 2]) # 最高水平高度
return min_horizontal_height, max_horizontal_height
# 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
# 获一堆点中 Z 值最小的四个点
def get_Z0bottom_from_boxPoints(bottom_points_np):
# 从点云中提取点的坐标
# 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]
# 计算中心点坐标
center = np.mean(points, axis=0)
# 计算每个点相对于中心点的极坐标角度
angles = np.arctan2(points[:, 1] - center[1], points[:, 0] - center[0])
# 对角度进行排序
sorted_indices = np.argsort(angles)
points = points[sorted_indices]
# 按排序后的顺序连接点
return points
# 2023年10月31日 根据4个底部点和高度生成包围盒八个点
def create_obox_from_bottom_4points(bottom_points_np, cubeHeight):
"""
:rtype: object
"""
# 计算底面的中心点坐标
center = np.mean(bottom_points_np, axis=0)
angles = np.arctan2(bottom_points_np[:, 1] - center[1], bottom_points_np[:, 0] - center[0])
# 对角度进行排序
sorted_indices = np.argsort(angles)
bottom_points_np = bottom_points_np[sorted_indices]
# 计算立方体的顶面点云
top_points_np = bottom_points_np + np.array([0, 0, cubeHeight])
# 合并底面和顶面点云
cube_points_np = np.vstack((bottom_points_np, top_points_np))
return cube_points_np
min_horizontal_height, max_horizontal_height = get_min_max_horizontal_height(pcd)
obx = pcd.get_minimal_oriented_bounding_box ()
points = np.array(obx.get_box_points())
points = get_SequentPoint_from_boxPoints(points)
# 找到底部
bottom_points = get_Z0bottom_from_boxPoints(points)
bottom_center = np.mean(bottom_points)
for idx , pt in enumerate(bottom_points) :
vec = (bottom_center - pt )/ np.linalg.norm(bottom_center - pt)
bottom_points[idx] = pt + vec * distance
new_points = create_obox_from_bottom_4points(bottom_points,max_horizontal_height-min_horizontal_height)
obx_pcd = o3d.geometry.PointCloud()
obx_pcd.points = o3d.utility.Vector3dVector(new_points)
obx_pcd_obx = obx_pcd.get_minimal_oriented_bounding_box()
indices = obx_pcd_obx.get_point_indices_within_bounding_box( pcd.points)
out_pcd = pcd.select_by_index(indices,invert=True)
in_pcd = pcd.select_by_index(indices)
return in_pcd,out_pcd,obx_pcd_obx,obx
if __name__ == "__main__":
# 【加载点云】
print("->正在加载点云... ")
pcd = o3d.io.read_point_cloud(r"..\Data\PCD\singleWall.pcd")
print(pcd)
#o3d.visualization.draw_geometries([pcd])
pcd.paint_uniform_color([0.5, 0.5, 0.5])
pcd_in_pcd,pcd_out_pcd,obx_pcd_obx,obx= get_pcd_by_reduce_box(pcd,200)
Linset = o3d.geometry.LineSet.create_from_oriented_bounding_box(obx_pcd_obx)
Linset_pcd = o3d.geometry.LineSet.create_from_oriented_bounding_box(obx)
o3d.visualization.draw_geometries([pcd_in_pcd.paint_uniform_color([1, 0, 0]),pcd_out_pcd.paint_uniform_color([0, 0, 1]),Linset.paint_uniform_color([1, 0, 0]),Linset_pcd.paint_uniform_color([0, 0, 1])],window_name='Open3D', width=1920, height=1080, left=50, top=50, point_show_normal=True, mesh_show_wireframe=True, mesh_show_back_face=True)