|
由于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)
|