基于激光点云生长算法

最新推荐文章于 2024-07-12 14:51:54 发布

云杂项

最新推荐文章于 2024-07-12 14:51:54 发布

阅读量133

点赞数 3

文章标签：算法

本文链接：https://blog.csdn.net/m0_73126623/article/details/131981875

版权

该算法利用给定点云和初始点point1，通过设置圆柱截取参数，预测点point2和point3来不断生长曲线。根据point2的极值确定生长方向，正向或反向。在满足特定距离条件时，更新种子点并扩展曲线，直至达到终止条件。

摘要由CSDN通过智能技术生成

一、已知条件

当我们已知一个三维点point1和该环境下的点云文件，通过该点来获取的生长所需要的点云，推荐使用波动范围小、提取处噪点少且点云连续的情况下。

二、算法原理

通过已知点point1，通过设置三维点云参数，在point1截取圆柱，预测点point2是point1所截取的最大一个点，，及point2在曲线上，预测point3 = 2*point2- point1, 获取离point3最近的以point2所画圆柱的点，为下一个种子点，不断重复该过程，获得其相关曲线。

根据point2取值的不同决定轨道向前或者向后生长，当point2取最大值则相对正向生长，当point2取最小值则相对反向生长，所以当point1为曲线上任意一点即可生长出该点云曲线。

三、概要代码展示

3.1 起始点获得

3.1.1 根据种子点获取周围圆柱点云

idx = np.where(((points[:, 0] - seed[0]) ** 2 + (points[:, 1] - seed[1]) ** 2 <= 0.25 ** 2) &
                   (points[:, 2] >= seed[2] - rope_d) & (points[:, 2] <= seed[2] + rope_d))[0]

    cloud_grow = pcd.select_by_index(idx)

3.1.2 point2生长方向

正向生长

seed_inc_id = np.argmax(cloud_grow_points, axis=0)[grow_axis]
    seed_inc = cloud_grow_points[seed_inc_id]

反向生长

seed_dec_id = np.argmin(cloud_grow_points, axis=0)[grow_axis]
    seed_dec = cloud_grow_points[seed_dec_id]

3.2 种子点生长

通过seed(point2)和seed_pred(point3)之间的相互关系，不断进行种子点的改变，当出现需求制约条件的不同，则不进行相关的延伸工作。

    while True:

        seed_pred = 2 * seed - last_seed

        #  当点云聚集 改变其x轴值
        # if (seed[0] - seed_pred[0]) ** 2 + (seed[1] - seed_pred[1]) ** 2 < 0.005**2:
        #     seed_pred[0] += 0.5
        # print(seed)
        grow_x, grow_y, grow_z = seed
        idx = np.where(
            ((points[:, 0] - grow_x) ** 2 + (points[:, 1] - grow_y) ** 2 <= 0.25 ** 2) &
            (grow_z - 0.01 <= points[:, 2]) & (points[:, 2] <= grow_z + 0.01))[0]

        cloud_grow = pcd.select_by_index(idx)

        # 添加种子
        if if_inc:
            all_seeds.extend([seed])
        else:
            all_seeds.insert(0, seed)

        rope_cloud_idx.extend(idx)

        cloud_grow_points = np.asarray(cloud_grow.points)

        dist_squared = (cloud_grow_points[:, 0] - seed_pred[0]) ** 2 + (cloud_grow_points[:, 1] - seed_pred[1]) ** 2
        next_seed_id = np.argmin(dist_squared)
        # print(dist_squared[next_seed_id])
        if dist_squared[next_seed_id] > 0.3 \
                or ((primeSeed[0] - seed_pred[0]) ** 2 + (primeSeed[1] - seed_pred[1]) ** 2 > 4 ** 2) \
                or ((seed[0] - seed_pred[0]) ** 2 + (seed[1] - seed_pred[1]) ** 2 < 0.05 ** 2) \
                or not len(dist_squared):
            break
        last_seed = seed
        seed = cloud_grow_points[next_seed_id]