Open3d点云处理之圆柱拟合

由于Open3d没有跟PCL一样的圆柱拟合，所以参考PCL代码

经测试，该算法在y轴上效果不是很好，请适当考虑利用聚类，计算出曲面的y轴总长度，取代他中心点坐标来代替算法预估的y坐标

点云处理工具：open3d==0.17

"""
拟合圆柱抓取中心
"""
import numpy as np
import open3d as o3d
import numpy
import random

center_location = np.array([])
#定义结构体
class Model:
    def __init__(self):
        self.point = np.array([0, 0, 0])
        self.direction = np.array([0, 0, 1])
        self.r = 0
        self.lIndices = []
        self.gIndices = []


def RANSAC_FIT_Cyliner(pcd,sigma,min_r,max_r,sample_num,iter):
    """

    :param pcd: 点云数据文件
    :param sigma: 距离拟合圆柱两侧的距离
    :param min_r: 设置圆柱最小半径
    :param max_r: 设置圆柱最大半径
    :param sample_num: 随机采样的点数
    :param iter: 迭代次数
    :return:
    """
    k = 50         #邻近点数，计算法向量
    if not pcd.has_normals():
        pcd.estimate_normals(o3d.geometry.KDTreeSearchParamKNN(k))

    # 法向量重定向
    pcd.orient_normals_consistent_tangent_plane(10)
    points = np.asarray(pcd.points)
    normals= np.asarray(pcd.normals)
    nums = points.shape[0]
    #print(nums)
    if sample_num>nums:
        print("采样点大于点云点数")
    range_index = [i for i in range(nums)]#获取每个点的索引数值
    model = Model()
    pretotal = 0  # 符合拟合模型的数据个数
    for i in range(iter):
        idx = random.sample(range_index, sample_num)
        sample_data = points[idx, :]
        normal_data = normals[idx, :]
        # 拟合圆柱
        p1 = sample_data[0, :]
        p2 = sample_data[1, :]
        n1 = normal_data[0, :]
        n2 = normal_data[1, :]

        # 计算圆柱系数
        w = n1 + p1 - p2
        a = np.dot(n1, n1)
        b = np.dot(n1, n2)
        c = np.dot(n2, n2)
        d = np.dot(n1, w)
        e = np.dot(n2, w)
        denominator = a * c - b * b
        if denominator < 1e-8:
            sc = 0
            if b > c:
                tc = d / b
            else:
                tc = e / c
        else:
            sc = (b * e - c * d) / denominator
            tc = (a * e - b * d) / denominator

        line_pt = p1 + n1 + sc * n1  # 轴线上一点
        line_dir = p2 + tc * n2 - line_pt  # 轴向
        line_dir = line_dir / np.linalg.norm(line_dir)  # 轴向归一化
        # 计算半径（点到线的距离）
        vec = p1 - line_pt
        r = np.linalg.norm(np.cross(vec, line_dir))
        if r < min_r or r > max_r:
            #print("{}:半径为{}不符合要求，跳过".format(i, r))
            continue

        # *****统计符合内点的数量*******
        dist = np.dot(line_pt, line_dir)  # 原点到平面距离
        t = points @ line_dir + dist
        tmpt = t.reshape(nums, 1)
        tmp_line_dir = line_dir.reshape(1, 3)
        proj_point = points - tmpt @ tmp_line_dir  # 投影
        # 计算中心点
        ct = np.dot(line_pt, line_dir) + dist
        center_point = line_pt - ct * line_dir
        proj_pcd = o3d.geometry.PointCloud(pcd)
        proj_pcd.points = o3d.utility.Vector3dVector(proj_point)
        pcd_tree = o3d.geometry.KDTreeFlann(proj_pcd)

        [k1, gIndices, _] = pcd_tree.search_radius_vector_3d(center_point, r + sigma)
        if r - sigma > 0:
            [k2, lIndices, _] = pcd_tree.search_radius_vector_3d(center_point, r - sigma)
        else:
            lIndices = []

        total = len(gIndices) - len(lIndices)
        #print("{}：半径为{}，内部点数为：{}".format(i, r, total))

        if total > pretotal:
            pretotal = total
            model.point = line_pt
            model.direction = line_dir
            model.r = r
            model.lIndices = lIndices
            model.gIndices = gIndices


    print(f"圆柱中心点:{model.point[0], model.point[1],model.point[2]}\n"
          f"圆柱朝向：{model.direction}\n"
          f"圆柱半径：{ model.r}\n"
          f"内部点数为：{len(model.gIndices) - len(model.lIndices)}"
        )

    center_location = np.array([model.point[0],model.point[1],model.point[2]])

    return center_location,model.direction,model.r

# if __name__ == "__main__":
#     pcd = o3d.io.read_point_cloud('../111.pcd')
#     o3d.visualization.draw_geometries([pcd])
#     sigma = 10    # 距离拟合圆柱两侧的距离
#     min_r = 43      # 设置圆柱最小半径
#     max_r = 46      # 设置圆柱最大半径
#     sample_num = 3  # 随机采样的点数
#     RANSAC_FIT_Cyliner(pcd,sigma,min_r,max_r,sample_num)