圆柱包围框-Bounding Cylinder-原理-代码实现

定义：使用一个圆柱体包围点云的所有点，通常用于长柱状物体。
优点：适合于柱状或长条形的点云。
缺点：计算较为复杂，尤其是确定圆柱体的轴线方向和半径。
找到圆柱尽量满足下面条件
- 找到能够完全包围3D物体的最小圆柱体。
- 这个圆柱体的轴线通常与物体的主轴对齐。
- 圆柱体的半径要足够大以包含物体的所有点。
- 圆柱体的高度要足够长以覆盖物体在轴向上的全部范围。

算法步骤

计算物体的主轴:
- 使用主成分分析(PCA)来确定物体的主要方向。
- PCA会给出物体点云的协方差矩阵的特征向量,其中最大特征值对应的特征向量即为主轴方向。
对齐坐标系:
- 将物体旋转,使其主轴与坐标系的z轴对齐。
计算圆柱半径:
- 在xy平面上投影所有点。
- 找出距离z轴最远的点,其到z轴的距离即为圆柱半径。
计算圆柱高度:
- 找出物体在z轴方向上的最大和最小值。
- 圆柱高度为这两个极值之差。
确定圆柱位置:
- 圆柱底面中心的z坐标为物体在z轴上的最小值。
- xy平面上的中心可以取物体质心的xy坐标。
优化(可选):
- 可以微调圆柱的位置和方向,以最小化圆柱体积。
转换回原始坐标系:
- 将计算得到的圆柱体从对齐后的坐标系转换回原始坐标系。

def bounding_cylinder(points):# 计算点云的协方差矩阵cov_mat = np.cov(points, rowvar=False)# 计算协方差矩阵的特征值和特征向量eigenvalues, eigenvectors = np.linalg.eig(cov_mat)# 找到最大特征值对应的特征向量，这就是圆柱的主轴major_axis = eigenvectors[:, np.argmax(eigenvalues)]# 将点投影到主轴上projected_points = np.dot(points, major_axis)# 计算圆柱的高度height = np.max(projected_points) - np.min(projected_points)# 计算圆柱的中心点center = np.mean(points, axis=0)# 计算点到主轴的距离distances = np.linalg.norm(np.cross(points - center, major_axis), axis=1)# 圆柱的半径是最大距离radius = np.max(distances)return {'center': center,'axis': major_axis,'radius': radius,'height': height}

完整代码

下面增加可视化的代码部分

import numpy as np
import plotly.graph_objects as godef bounding_cylinder(points):# 计算点云的协方差矩阵cov_mat = np.cov(points, rowvar=False)# 计算协方差矩阵的特征值和特征向量eigenvalues, eigenvectors = np.linalg.eig(cov_mat)# 找到最大特征值对应的特征向量，这就是圆柱的主轴major_axis = eigenvectors[:, np.argmax(eigenvalues)]# 将点投影到主轴上projected_points = np.dot(points, major_axis)# 计算圆柱的高度height = np.max(projected_points) - np.min(projected_points)# 计算圆柱的中心点center = np.mean(points, axis=0)# 计算点到主轴的距离distances = np.linalg.norm(np.cross(points - center, major_axis), axis=1)# 圆柱的半径是最大距离radius = np.max(distances)return {'center': center,'axis': major_axis,'radius': radius,'height': height}def create_cylinder_mesh(center, axis, radius, height, resolution=50):# 生成圆柱表面的点theta = np.linspace(0, 2*np.pi, resolution)z = np.linspace(-height/2, height/2, resolution)theta, z = np.meshgrid(theta, z)x = radius * np.cos(theta)y = radius * np.sin(theta)# 创建旋转矩阵rotation_matrix = np.eye(3)rotation_matrix[:, 2] = axisrotation_matrix[:, 0] = np.cross(axis, [0, 1, 0])rotation_matrix[:, 1] = np.cross(rotation_matrix[:, 2], rotation_matrix[:, 0])# 应用旋转coords = np.dot(np.array([x.flatten(), y.flatten(), z.flatten()]).T, rotation_matrix)# 平移到中心coords += centerreturn coords.T.reshape((3, resolution, resolution))def visualize_bounding_cylinder(points, cylinder):# 创建点云散点图scatter = go.Scatter3d(x=points[:, 0], y=points[:, 1], z=points[:, 2],mode='markers',marker=dict(size=2, color='blue', opacity=0.5),name='Point Cloud')# 创建圆柱体表面cylinder_mesh = create_cylinder_mesh(cylinder['center'], cylinder['axis'], cylinder['radius'], cylinder['height'])surface = go.Surface(x=cylinder_mesh[0], y=cylinder_mesh[1], z=cylinder_mesh[2],colorscale=[[0, 'red'], [1, 'red']],opacity=0.5,name='Bounding Cylinder')# 创建图形布局layout = go.Layout(scene=dict(xaxis_title='X',yaxis_title='Y',zaxis_title='Z',aspectmode='data'),title='点云和包围圆柱')# 创建图形fig = go.Figure(data=[scatter, surface], layout=layout)# 显示图形fig.show()# 主程序
if __name__ == "__main__":# 生成一些随机点np.random.seed(0)points = np.random.randn(1000, 3) * [1, 1, 3]  # 创建一个椭圆形的点云cylinder = bounding_cylinder(points)print("圆柱中心:", cylinder['center'])print("圆柱轴:", cylinder['axis'])print("圆柱半径:", cylinder['radius'])print("圆柱高度:", cylinder['height'])# 可视化结果visualize_bounding_cylinder(points, cylinder)