【Astra S 检测障碍并绘制最小包围盒】

主要对图片中的障碍进行检测，并绘制最小包围盒示意障碍物范围
存在的问题：

1. 对先验知识依赖较多，许多变量需要设置合适范围
2. 包围盒包围区域不能十分精确表示真是的障碍物状态
3. 对正对方向且障碍物紧贴空间平面的情况适用型好
4. 运行出来的图片结果是进行了180度反转的情况，具体情况还未处理

本次是对下面初始图片（未提取ROI）的图片进行处理

使用二值化罩层筛选点云

# 定义ROI区域
rect_roi = [100, 100, 400, 400]
x1, y1, x2, y2 = rect_roi

# 绘制彩图中的ROI区域
canvas = np.copy(color_image)
canvas = cv2.rectangle(canvas, (x1, y1), (x2, y2), [0, 0, 255], 5)
plt.imshow(canvas[:, :, ::-1])
# 创建空的罩层
cv2.imwrite(f"./data/finalExample/initialRGBimg_roiArea.png", canvas)
mask = np.zeros((img_height, img_width), dtype=np.uint8)
# 赋值罩层

mask[y1:y2, x1:x2] = 255
# 可视化罩层
plt.imshow(mask, cmap="gray")
# 根据罩层获取点云
pcd_rect_roi = camera.get_pcd(color_image, depth_image,\
                          mask=mask, camera="rgb_camera")

其中ROI区域的大小设置为 rect_roi = [100, 100, 400, 400] 如图

# 点云可视化
draw_geometry([pcd_rect_roi], window_name="点云ROI区域")
# 保存PCD点云 二进制格式
pcd_rect_roi_name = "pcd_rect_roi"
o3d.io.write_point_cloud(f"./data/finalExample/{pcd_rect_roi_name}.pcd", pcd_rect_roi)

提取图片ROI区域

空间平面拟合

# 导入初始场景图
scene_pcd = o3d.io.read_point_cloud(f"./data/finalExample/pcd_rect_roi.pcd")
# 点云可视化
o3d.visualization.draw_geometries([scene_pcd])

初始场景图显示

设定距离阈值

# 距离阈值 单位m
# 距离平面多少的点被认为是在这个平面上的点
distance_threshold = 0.05
# 平面拟合
plane_model, inliers = scene_pcd.segment_plane(distance_threshold=distance_threshold,
                                         ransac_n=3,
                                         num_iterations=500)
# 打印拟合平面的表达式
[a, b, c, d] = plane_model
print(f"拟合桌面平面的表达式: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

# 工作平面上的点云
ws_panel_cloud = scene_pcd.select_by_index(inliers)
ws_panel_cloud.paint_uniform_color([1.0, 0, 0])

draw_geometry([ws_panel_cloud], window_name="背景层")

通过设定 distance_threshold 阈值来实现一种聚类的思想，将障碍物和空间平面进行分离
scene_pcd.segment_plane采用 open3d RANSAC方法，采用open3d中RANSAC分割平面的函数为segment_plane，主要参数有三个。distance_threshold定义了一个点到一个估计平面的最大距离，这些距离内的点被认为是内点（inlier），ransac_n定义了使用随机抽样估计一个平面的点的个数，num_iterations定义了随机平面采样和验证的频率（迭代次数）。这个函数返回（A,B,C,D）作为一个平面，对于平面上每个点(x,y,z)满足上面的平面方程。这个函数还会返回内点索引的列表。
原理（RANSAC，Random Sample Consensus）方式，即随机抽样一致性，它是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。RANSAC算法的基本理论基础是大数定律，也就是当采样数达到一定数量后采样的数据就会符合它自身原有的概率属性。这是一种通过概率的方式来进行拟合。
以RANSAC平面分割为例，由于三个点可以确定一个平面，因此RANSAC会随机选择三个点来构建一个平面，并用点云中实际上有多少个点落到这个平面上来作为评估这个平面的正确程度。当随机抽样的次数足够多时，我们有较大概率获得所需要的平面。

移除（分离）障碍物后的空间平面图

至此，输出的拟合的空间平面（背景）表达式为： -0.49x + 0.11y + 0.87z + -1.84 = 0

移除空间平面（背景）

# 工作台平面上的点云
scene_pcd_on_panel = scene_pcd.select_by_index(inliers)

# 将工作平面上的点云移除
scene_pcd_rm_panel = scene_pcd.select_by_index(inliers, invert=True)
# 可视化
draw_geometry([scene_pcd_rm_panel], window_name="移除背景后的检测物点云")

移除背景后的障碍物图

通过移除空间平面后（背景）障碍物点云图附近还有部分比较杂乱的点云，下一步处理是移除障碍物后方的点云 （可以根据这些杂乱的点云进行粗略的障碍物大小的估计，实验部分还没进行）

points_3d = np.asarray(scene_pcd_rm_panel.points)
# 分别获取X坐标, Y坐标, Z坐标的列表
x_list = points_3d[:, 0]
y_list = points_3d[:, 1]
z_list = points_3d[:, 2]
# 获取平面内侧的点云
value = a*x_list + b*y_list + c*z_list + d
pcd_close_panel_index = np.argwhere(value > 0)

scene_pcd_close_panel = scene_pcd_rm_panel.select_by_index(pcd_close_panel_index, invert=True)
draw_geometry([scene_pcd_close_panel], window_name="移除检测物后方点云")

移除障碍物后方点云后障碍图

对障碍物上表面进行拟合

# 距离阈值 单位m
# 距离平面多少的点被认为是在这个平面上的点
distance_threshold = 0.01
# 平面拟合
box_plane_model, inliers = scene_pcd_close_panel.segment_plane(distance_threshold=distance_threshold,
                                         ransac_n=3,
                                         num_iterations=500)
# 打印拟合平面的表达式
[a1, b1, c1, d1] = box_plane_model
print(f"拟合盒子平面的表达式: {a1:.2f}x + {b1:.2f}y + {c1:.2f}z + {d1:.2f} = 0")

# 工作平面上的点云
box_panel_pcd = scene_pcd_close_panel.select_by_index(inliers)
box_panel_pcd.paint_uniform_color([0, 1.0, 0])

draw_geometry([box_panel_pcd, scene_pcd_on_panel], window_name="背景+检测物点云")
# 保存平面拟合的结果
# 保存平面信息
panel_model0 = np.float32([a1, b1, c1, d1])
np.savetxt("./data/finalExample/box_panel_model.txt", box_plane_model, delimiter=",", fmt="%.4f")
# 保存盒子表面的点云
# 保存PCD点云 二进制格式
o3d.io.write_point_cloud("./data/finalExample/box_panel_pcd.pcd", box_panel_pcd)

拟合思想类似空间平面（背景）拟合
./data/finalExample/box_panel_model.txt这个文件是记录拟合的障碍物表面的信息
信息内容是：

-0.1452
0.0808
0.9861
-1.3795

障碍物上表面拟合+背景结果图

至此，输出的拟合障碍物平面的表达式为：-0.40x + 0.15y + 0.90z + -1.40 = 0

获取相机坐标系下的工作台平面（背景）表达式

load_from_file = True
desktop_surface = o3d.io.read_point_cloud(f"./data/finalExample/pcd_rect_roi.pcd")
# 距离阈值 单位m
# 距离平面多少的点被认为是在这个平面上的点
distance_threshold = 0.05
# 平面拟合
plane_model, inliers = desktop_surface.segment_plane(distance_threshold=distance_threshold,
                                         ransac_n=3,
                                         num_iterations=500)
# 打印拟合平面的表达式
[a2, b2, c2, d2] = plane_model
print(f"拟合桌面平面的表达式: {a2:.2f}x + {b2:.2f}y + {c2:.2f}z + {d2:.2f} = 0")

# 工作平面上的点云
ws_panel_cloud = desktop_surface.select_by_index(inliers)
# ws_panel_cloud.paint_uniform_color([1.0, 0, 0])

draw_geometry([ws_panel_cloud], window_name="背景点云")

# 保存平面拟合的结果
# 保存平面信息
panel_model1 = np.float32([a2, b2, c2, d2])

np.savetxt("data/finalExample/desktop_panel_model.txt", panel_model1, delimiter=",", fmt="%.4f")

背景点云图

此时，输出的拟合空间平面（背景）的表达式为： -0.49x + 0.11y + 0.86z + -1.84 = 0（和刚开始获取的平面空间表达式几乎是一样的）
data/finalExample/desktop_panel_model.txt文件中保存的是空间平面（背景）拟合的表达式，内容：

-0.2074
0.1368
0.9686
-1.8472

绘制障碍物表面中心点和高度测量（障碍物表面到空间平面表面）

全场景点云

# 载入场景点云
scene_pcd1 = o3d.io.read_point_cloud(f"./data/finalExample/pcd_rect_roi.pcd")

# 载入盒子表面点云
box_panel_pcd1 = o3d.io.read_point_cloud("./data/finalExample/box_panel_pcd.pcd")

# 赋值为蓝色
box_panel_pcd1.paint_uniform_color([0.0, 0.0, 1.0])

# 点云可视化
o3d.visualization.draw_geometries([scene_pcd1], window_name="全场景点云")

障碍物表面与相机坐标系关系

# 创建彩色相机坐标系的Mesh
cam_corrd_mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2)
# 显示盒子上表面点云与相机坐标系
o3d.visualization.draw_geometries([box_panel_pcd1,  cam_corrd_mesh], window_name="被检测物表面点云与相机坐标系")

绘制障碍物表面中心点

center_point = box_panel_pcd1.get_center()
print(f"盒子点云中心点: {center_point}")
# 可视化，在质心位置绘制小球
mesh_sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.005)
mesh_sphere.compute_vertex_normals()
# 小球平移
mesh_sphere.translate(center_point.reshape(-1), relative=False)
# 给小球上色为红色
mesh_sphere.paint_uniform_color([1.0, 0, 0])
# 创建可视化窗口
draw_geometry([scene_pcd1, mesh_sphere],\
                window_name="绘制障碍物上表面中心点")

输出距离信息

# 载入工作台的平面数据
desktop_panel_model = np.loadtxt("./data/finalExample/desktop_panel_model.txt", delimiter=",")

[A, B, C, D] = desktop_panel_model
print(f"拟合桌面平面的表达式: {A:.2f}x + {B:.2f}y + {C:.2f}z + {D:.2f} = 0")
# 计算中心点距离平面的距离
distance = get_distance_to_panel(A, B, C, D, center_point)
print(f"盒子高度(点到平面距离) {distance:.3f}, 单位m")

输出结果

拟合桌面平面的表达式: -0.21x + 0.14y + 0.97z + -1.85 = 0
盒子高度(点到平面距离) 0.477, 单位m

KDTree体素最近邻检索

KDTree处理前点云图

# 载入场景点云
KD_scene_pcd = o3d.io.read_point_cloud(f"./data/finalExample/pcd_rect_roi.pcd")
# 载入盒子表面点云
box_panel_pcd_KD = o3d.io.read_point_cloud("./data/finalExample/box_panel_pcd.pcd")
# 赋值为灰色
box_panel_pcd_KD.paint_uniform_color([0.5, 0.5, 0.5])
# 点云可视化
o3d.visualization.draw_geometries([KD_scene_pcd], window_name="KDTree处理前场景图")

使用KDTree绘制障碍物表面中心点

center_point_KD = box_panel_pcd_KD.get_center()
print(f"盒子上表面质心: {center_point_KD}")
# 可视化，在质心位置绘制小球
mesh_sphere_KD = o3d.geometry.TriangleMesh.create_sphere(radius=0.005)
mesh_sphere_KD.compute_vertex_normals()
# 小球平移
mesh_sphere_KD.translate(center_point_KD.reshape(-1), relative=False)
# 给小球上色为红色
mesh_sphere_KD.paint_uniform_color([1.0, 0, 0])
# 创建可视化窗口
draw_geometry([KD_scene_pcd, mesh_sphere_KD],\
                window_name="KD绘制障碍物上表面中心点")

创建障碍物上表面点云的KDTree

# 创建盒子上表面点云的KDTree
box_panel_kdtree = o3d.geometry.KDTreeFlann(box_panel_pcd_KD)
neighbor_num = 4000 # 邻居个数
print(f"寻找距离 中心点{center_point_KD} \n最近的{neighbor_num}个邻居，并将其绘制为蓝色")
k, idx, _ = box_panel_kdtree.search_knn_vector_3d(center_point_KD, neighbor_num)
print(f"实际找到的邻居个数: {k}")
# 可视化
box_panel_pcd2 = copy.deepcopy(box_panel_pcd_KD)
# 点云上色
np.asarray(box_panel_pcd2.colors)[idx, :] = [0, 0, 1]

draw_geometry([box_panel_pcd2], \
              bk_color=[0.4, 0.8, 0.4], \
              window_name="上表面点云+N个最近邻")

障碍物上边面点云+最近邻
输出结果：

盒子上表面质心: [0.02157614 0.29031447 1.37834154]
寻找距离 中心点[0.02157614 0.29031447 1.37834154] 
最近的4000个邻居，并将其绘制为蓝色
实际找到的邻居个数: 4000

提取最近邻

# 根据索引获取选择到的点云
center_neighbor_pcd = box_panel_pcd2.select_by_index(idx)

o3d.visualization.draw_geometries([center_neighbor_pcd],\
                                  window_name="N个最近邻")

根据半径检索点云

# 检索半径, 单位m
radius = 0.01
print(f"寻找距离 中心点{center_point_KD} \n半径为{radius}的邻居，并将其绘制为蓝色")
k, idx, _ = box_panel_kdtree.search_radius_vector_3d(center_point_KD, radius)
print(f"实际找到的邻居个数: {k}")
# 可视化
box_panel_pcd3 = copy.deepcopy(box_panel_pcd_KD)
# 点云上色
np.asarray(box_panel_pcd3.colors)[idx, :] = [0, 0, 1]

draw_geometry([box_panel_pcd3], \
              bk_color=[0.4, 0.8, 0.4], \
              window_name="上表面点云+根据半径检索的点云")

障碍物上表面点云+半径检索的点云（r = 0.01）（检索半径设置的太小）
输出结果：

寻找距离 中心点[0.02157614 0.29031447 1.37834154] 
半径为0.01的邻居，并将其绘制为蓝色
实际找到的邻居个数: 24

移除离群点

移除离群点前点云图

# 载入盒子表面点云
box_panel_pcd_badoint = o3d.io.read_point_cloud("./data/finalExample/box_panel_pcd.pcd")

# 赋值为灰色
box_panel_pcd_badoint.paint_uniform_color([0.5, 0.5, 0.5])

# 点云可视化
o3d.visualization.draw_geometries([box_panel_pcd_badoint],\
                                  window_name="移除离群点前障碍物上表面点云")

离群点

cl, ind = box_panel_pcd_badoint.remove_statistical_outlier(nb_neighbors=100,#nb_neighbors=10,
                                        std_ratio=3.0)

display_inlier_outlier(box_panel_pcd_badoint, ind)

移除离群点后点云

box_panel_filter = box_panel_pcd_badoint.select_by_index(ind)
draw_geometry([box_panel_filter], window_name="移除离群点后的上表面点云")
o3d.io.write_point_cloud("./data/finalExample/box_panel_filter.pcd", box_panel_filter)

障碍物点云投影到空间平面

准备投影到空间平面前的点云图

# 场景点云
scene_pcd_projection = o3d.io.read_point_cloud(f"./data/finalExample/pcd_rect_roi.pcd")

# 载入盒子表面点云
# 去除离群点后的
box_panel_pcd_projection = o3d.io.read_point_cloud("./data/finalExample/box_panel_filter.pcd")

# 赋值为灰色
box_panel_pcd_projection.paint_uniform_color([0.5, 0.5, 0.5])

# 点云可视化
draw_geometry([box_panel_pcd_projection], window_name="投影空间平面前移除离群点后的上表面点云")

障碍物投影到空间平面

# 载入工作台的平面数据
desktop_panel_model_projection = np.loadtxt("data/finalExample/desktop_panel_model.txt", delimiter=",")

[A, B, C, D] = desktop_panel_model_projection
print(f"拟合桌面平面的表达式: {A:.2f}x + {B:.2f}y + {C:.2f}z + {D:.2f} = 0")
# 投影到空间平面上的点
box_underside_pcd = pcd_project2panel(box_panel_pcd_projection, A, B, C, D, x0=0, y0=0, z0=0)
box_underside_pcd.paint_uniform_color([0.0, 1.0, 0])
# 点云可视化
draw_geometry([box_panel_pcd_projection, box_underside_pcd],\
                                  window_name="投影空间平面后障碍物下表面点云")

投影后融合场景

# 点云可视化
draw_geometry([scene_pcd_projection, box_underside_pcd],\
                                  window_name="场景+投影空间平面后障碍物下表面点云")

绘制点云最小包围盒

最小包围盒处理前障碍物点云

np.set_printoptions(precision=3, suppress=True)
# 载入盒子表面点云
# 去除离群点后的
surroundding_box_panel_pcd = o3d.io.read_point_cloud("./data/finalExample/box_panel_filter.pcd")
# 赋值为灰色
surroundding_box_panel_pcd.paint_uniform_color([0.5, 0.5, 0.5])
# 点云可视化
draw_geometry([surroundding_box_panel_pcd], window_name="最小包围盒将要处理的障碍物上表面点云")

绘制点云AABB包围盒

aabb = surroundding_box_panel_pcd.get_axis_aligned_bounding_box()
aabb.color = [1, 0, 0] # 红色

# 绘制AABB
o3d.visualization.draw_geometries([surroundding_box_panel_pcd, aabb], window_name="点云AABB包围盒",
                                  width=800,  # 窗口宽度
                                  height=600)  # 窗口高度
# 包围盒,最小值坐标 [x_min, y_min, z_min]
print(f"min_bound: {aabb.min_bound}")
x_min, y_min, z_min = aabb.min_bound
print(f"x_min={x_min:.4f} y_min={y_min:.4f} z_min={z_min:.4f}, 单位m")
print(f"max_bound: {aabb.max_bound}")
x_max, y_max, z_max = aabb.max_bound
print(f"x_max={x_max:.4f} y_max={y_max:.4f} z_max={z_max:.4f}, 单位m")
# AABB包围盒的尺寸
length = x_max - x_min
width = y_max - y_min 
height = z_max - z_min
print(f"AABB包围盒尺寸: 长: {length:.4f} 宽: {width:.4f} 高: {height:.4f}")

输出结果：

min_bound: [-0.147  0.208  1.342]
x_min=-0.1467 y_min=0.2076 z_min=1.3420, 单位m
max_bound: [0.196 0.414 1.414]
x_max=0.1957 y_max=0.4138 z_max=1.4140, 单位m
AABB包围盒尺寸: 长: 0.3423 宽: 0.2062 高: 0.0720

绘制点云OBB包围盒

robust = True # 鲁棒开关， 实际测试robust=True效果反而会不好
obb = surroundding_box_panel_pcd.get_oriented_bounding_box(robust=robust)
obb.color = [0, 1, 0] # 绿色

# 绘制OBB
o3d.visualization.draw_geometries([surroundding_box_panel_pcd, obb], window_name="点云OBB包围盒",
                                  width=800,  # 窗口宽度
                                  height=600)  # 窗口高度
print(f"OBB包围盒的维度: {obb.dimension()}")
print(f"OBB包围盒中心点: {obb.center}")
length, width, height = obb.extent
print(f"OBB包围盒的尺寸: 长: {length:.4f} 宽: {width:.4f} 高: {height:.4f}")
# 包围盒的8个角点
print(np.asarray(obb.get_box_points()))

输出结果：

OBB包围盒的维度: 3
OBB包围盒中心点: [0.023 0.31  1.378]
OBB包围盒的尺寸: 长: 0.3477 宽: 0.2033 高: 0.0228
[[-0.144  0.42   1.353]
 [ 0.199  0.404  1.406]
 [-0.156  0.218  1.373]
 [-0.141  0.418  1.331]
 [ 0.19   0.2    1.403]
 [-0.153  0.216  1.351]
 [ 0.203  0.402  1.383]
 [ 0.187  0.202  1.426]]

点云AABB包围盒和OBB包围盒对比

o3d.visualization.draw_geometries([surroundding_box_panel_pcd, aabb, obb], window_name="点云AABB与OBB包围盒",
                                  width=800,  # 窗口宽度
                                  height=600)  # 窗口高度