点云-圆柱包围框-原理-代码实现

点云-圆柱包围框-原理-代码实现

引用

CSDN

1.

https://blog.csdn.net/u012901740/article/details/142410387

定义：使用一个圆柱体包围点云的所有点，通常用于长柱状物体。

优点：适合于柱状或长条形的点云。

缺点：计算较为复杂，尤其是确定圆柱体的轴线方向和半径。

找到圆柱尽量满足下面条件：

• 找到能够完全包围3D物体的最小圆柱体。
• 这个圆柱体的轴线通常与物体的主轴对齐。
• 圆柱体的半径要足够大以包含物体的所有点。
• 圆柱体的高度要足够长以覆盖物体在轴向上的全部范围。

算法步骤

1. 计算物体的主轴：

• 使用主成分分析(PCA)来确定物体的主要方向。
• PCA会给出物体点云的协方差矩阵的特征向量,其中最大特征值对应的特征向量即为主轴方向。

2. 对齐坐标系：

• 将物体旋转,使其主轴与坐标系的z轴对齐。

3. 计算圆柱半径：

• 在xy平面上投影所有点。
• 找出距离z轴最远的点,其到z轴的距离即为圆柱半径。

4. 计算圆柱高度：

• 找出物体在z轴方向上的最大和最小值。
• 圆柱高度为这两个极值之差。

5. 确定圆柱位置：

• 圆柱底面中心的z坐标为物体在z轴上的最小值。
• xy平面上的中心可以取物体质心的xy坐标。

6. 优化(可选)：

• 可以微调圆柱的位置和方向,以最小化圆柱体积。

7. 转换回原始坐标系：

• 将计算得到的圆柱体从对齐后的坐标系转换回原始坐标系。

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 go

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
    }

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] = axis
    rotation_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 += center
    
    return 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)