本文章应用于钢卷库自动装卸场景中的一处应用,根据货车上的鞍座位置信息,预估出放置钢卷中心的位置坐标。具体方法请参考文献内容,这里不做详细解释。其中代码85行H+5这个5为测试误差,加上5正好符合实际坐标。单位mm
语言:Python
点云处理工具:Open3d==0.17
参考文献:[1]谭云月.基于点云数据的钢卷装卸自动定位系统的设计与实现[D].重庆大学,2017.
import open3d as o3d
import numpy as np
#鞍座放置位置
estimate_cyliner = np.array([])
#定义一个预估函数
def estimate_Cyliner_axle(pcd,R,esp=10,min_points=3,print_progress=False):
"""
:param pcd: 点云数据
:param R: 钢卷半径
:param esp: 范围阈值
:param min_points: 最小点数
:param print_progress: bool 打印处理过程
:return:
"""
#pcd.points To points
points = np.array(pcd.points)
#DBScan聚类
labels = np.array(pcd.cluster_dbscan(esp,
min_points,
print_progress))
max_label = labels.max()
print(f"points cloud has {max_label+1} cluters")
#存放每一簇的点数
clusters = {}
#存放平面参数
Param = {}
for i in range(max_label+1):
clusters_indices = np.where(labels==i)[0]#找出在labels中标签与i相等的索引
cluster_points = np.asarray(pcd.points)[clusters_indices]#找出对应索引点的坐标
pcd_plane = o3d.geometry.PointCloud()
pcd_plane.points = o3d.utility.Vector3dVector(cluster_points)
# 使用 RANSAC 算法拟合平面
plane_model, inliers = pcd_plane.segment_plane(distance_threshold=0.01,
ransac_n=3,
num_iterations=1000)
[a, b, c, d] = plane_model
Param_name_1 = "a_" + str(i+1)
Param_name_2 = "b_" + str(i+1)
Param_name_3 = "c_" + str(i+1)
Param_name_4 = "d_" + str(i+1)
Param[Param_name_1] = a
Param[Param_name_2] = b
Param[Param_name_3] = c
Param[Param_name_4] = d
#两平面相交的直线方向向量l_intersect
A = Param['b_1'] * Param['c_2'] - Param['c_1'] * Param['b_2']
B = Param['a_2'] * Param['c_1'] - Param['a_1'] * Param['c_2']
C = Param['a_1'] * Param['b_2'] - Param['b_1'] * Param['a_2']
l_intersect = np.array([A,B,C])#相交直线法向量参数ABC
k_x = ((Param['b_1'] * Param['d_2'] )- (Param['d_1'] * Param['b_2'])) / ((Param['a_1'] * Param['b_2']) - (Param['b_1'] * Param['a_2']))
print(k_x)
k_y = (Param['a_1'] * Param['d_2'] - Param['d_1'] * Param['a_2']) / (Param['b_1'] * Param['a_2'] - Param['a_1'] * Param['b_2'])
k_z = 0
k = np.array([k_x,k_y,k_z])
new_array = np.empty((len(points),3))
#投影每个点到直线上
for i in range(len(points)):
x_p = points[i, 0]
y_p = points[i, 1]
z_p = points[i, 2]
K_fenzi_1 = np.array([x_p - k_x, y_p - k_y, z_p])
K_fenzi = np.dot(K_fenzi_1, l_intersect)
K = K_fenzi / (A ** 2 + B ** 2 + C ** 2)
X = k_x + K * A
Y = k_y + K * B
Z = k_z + K * C
# 将X、Y、Z的值分别赋值给new_array的每一行
new_array[i] = [X, Y, Z]
x_max = np.max(new_array[:, 0])
x_min = np.min(new_array[:, 0])
y_max = np.max(new_array[:, 1])
y_min = np.min(new_array[:, 1])
z_max = np.max(new_array[:, 2])
z_min = np.min(new_array[:, 2])
W_x = (x_max + x_min) / 2
W_y = (y_max + y_min) / 2
W_z = (z_max + z_min) / 2
# 求α
a11 = np.array([Param['a_1'], Param['b_1'], Param['c_1']])
a22 = np.array([Param['a_2'], Param['b_2'], Param['c_2']])
alpha = np.arccos((np.dot(a11, a22)) / (np.linalg.norm(a11) / np.linalg.norm(a22)))
beter = (180 - alpha) / 2
H = R / np.sin(beter)
print("预估的圆心坐标为:",W_x,W_y,W_z+H+5)
print(Param)
print(k_x,k_y,k_z)
print(A,B,C)
1万+



被折叠的 条评论
为什么被折叠?



