0.任务描述
-
背景：从端面拍摄大型圆筒工件，该工件周向尺寸大于相机视野，只能拍摄到1/3左右的圆周，且无法保证相机与端面垂直拍摄
-
任务：需要拟合圆周与轴线位置
-
难点：三维圆拟合与检测都很复杂，没有方便可用的成熟方案，最小二乘法既无法处理高维情况，也会受异常点干扰，RANSAC的检测迭代次数过多，选点的随机性过大
-
基本思路：通过深度信息过滤干扰平面，使用RANSAC算法检测最大平面作为圆筒端面，在该端面上随机选取一个点作为初始化圆心，以最小化所有点到圆心的距离差作为优化目标，求解最优化问题，得到圆心和半径，结合端面法向量可以求出轴线方程。
-
相关数据：深度信息过滤后的点云数据下载
1.加载显示原始点云
- 目录结构：数据都存在duanmian 文件夹下，本文用力为第三次拍摄，存在3文件夹下：
file = '3'
file_before = 'duanmian/'
pcd = o3d.io.read_point_cloud(file_before + file + '/point_cloud_00000.ply')
- 原始点云全白，与显示的背景色重合无法有效可视化，所以需要改变颜色，这里改为了全黑
points = np.array(pcd.points)
colors = np.zeros(np.array(pcd.points).shape[0])
pcd.colors = o3d.utility.Vector3dVector(np.zeros(np.array(pcd.colors).shape))
o3d.visualization.draw_geometries([pcd])
2.点云预处理
- 均匀降采样：保证点的位置准确度的同时方便存储与后续计算
pcd = pcd.uniform_down_sample(every_k_points = 20)
o3d.visualization.draw_geometries([pcd])
- 存储降采样后的原始点云，方便后期对比与效果展示：
o3d.io.write_point_cloud(file_before + file + '/old.ply', pcd)
- RANSAC平面检测分割（详细解释参考这里）：
plane_model, inliers = pcd.segment_plane(distance_threshold=3 * 1e-3,
ransac_n=3,
num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
inlier_cloud = pcd.select_by_index(inliers)
outlier_cloud = pcd.select_by_index(inliers, invert=True)
- 进一步滤波，滤去离群干扰点（在端面平面内但明显不在圆周上的点）
- 有两种常用方法：统计滤波和半径滤波（参考博文）
- 统计滤波：
#统计滤波 # nb_neighbors:最近k个点 std_ratio:基于标准差的阈值，越小滤除点越多 cl,ind = inlier_cloud.remove_statistical_outlier(nb_neighbors=3, std_ratio=1) inlier_cloud = inlier_cloud.select_by_index(ind) inlier_cloud.paint_uniform_color([1.0, 0, 0]) outlier_cloud2 = inlier_cloud.select_by_index(ind, invert=True)- 半径滤波：
#半径滤波 # nb_points:基于球体内包含点数量的阈值 radius:半径 cl,ind = inlier_cloud.remove_radius_outlier(nb_points=3, radius = 1.0) inlier_cloud = inlier_cloud.select_by_index(ind)- 实际效果来看统计滤波效果更好，半径滤波需要设定半径的大小，常常无法在保留内点的同时过滤离群点
- 之后需要被拟合的点都存在了inlier_cloud中
3.构建学习模型
model(points: torch.tensor, cir: torch.tensor, line: torch.tensor)- 传入待拟合点，待优化圆心（二维），端面平面方程，使用端面方程去计算圆心的三维坐标，这样可以保证优化过程中圆心始终在端面平面内
def model(points: torch.tensor, cir: torch.tensor, line: torch.tensor):
'''
功能：
根据圆心和待拟合点计算损失
输入：
待拟合点，待优化圆心（二维），端面平面方程
输出：
圆心，半径，损失
'''
line = line.float()
points = points.float()
cir = cir.float()
#计算圆心三维坐标
cir_z = torch.matmul(cir, line[0:2].T) + line[-1]
cir_z = cir_z / (1e-10 - line[2])
cir_z = cir_z.unsqueeze(0)
cir = torch.cat([cir, cir_z], 0)
#计算半径矩阵和损失
#损失一定程度上表示每个点到圆心的距离的差距
points = points - cir
points = torch.matmul(points, points.T)
points = torch.diag(points)
n_all = points.shape[0]
r_all = torch.sum(points) / (n_all ** 1)
e = 0
for i in range(1,4):
n = int(n_all / i)
r = torch.sum(points[:n]) / (n ** 1)
e += ((r - r_all) ** 2 )
return cir, r_all ** 0.5, e
4.训练模型
- 数据准备
points_2 = np.array(inlier_cloud.points) #* 100
cir =torch.from_numpy(points_2[0][0:2])
cir.requires_grad = True
points_2 = torch.from_numpy(points_2)
line = torch.Tensor(np.array([a, b, c, d]))
- 模型训练：采用分段学习率，每5000次更新打印一次信息
learning_rate_o = 1e-3
learning_rate_2 = 1e-2
learning_rate_3 = 1
learning_rate_4 = 8
repect_n = 0
repect = 0
epoch = 0
jingdu = 1e-28
epoch_max = 5 * 1e5
print('-------开始学习---------')
while(True):
epoch += 1
if cir.grad is not None:
#梯度归零
cir.grad.zero_()
#前向传播
_, r, l = model(points_2, cir, line)
#反向传播
l.backward()
if cir.grad is None:
#梯度爆炸就及时退出
print('++++++++++++')
print('epoch:', epoch)
print('a:', cir)
print('grad:', cir.grad)
print('r:', r)
break
#分段学习率
if l < 100:
learning_rate = learning_rate_2
if l < 45:
learning_rate = learning_rate_3
if l < 0.2:
learning_rate = learning_rate_4
else:learning_rate = learning_rate_3
else:learning_rate = learning_rate_2
else:
learning_rate = learning_rate_o
with torch.no_grad():
cir -= learning_rate * cir.grad
if epoch % 5e3 == 0:
print('------------------')
print('epoch:',epoch)
print('a:', cir)
print('grad:', cir.grad)
print('rate:',learning_rate)
print('loss:', l)
print('r:', r.item())
if l < jingdu:
print('精度足够，停止学习')
break
if epoch > epoch_max:
break
if l == repect:
repect_n += 1
else:
repect = l
repect_n = 0
if repect_n > 15:
print('达到收敛停止学习')
break
- 打印最终训练结果：
print('*****************************')
print('epoch:',epoch)
print('a:', cir)
print('grad:', cir.grad)
print('rate:',learning_rate)
print('loss:', l)
print('r:', r.item())
cir, r, l = model(points_2, cir, line)
print('圆心坐标:(', cir, '),半径:', r.item())
-
效果展示：
-
将圆心坐标存入inlier_cloud中：
see = np.row_stack([np.array(inlier_cloud.points), cir.detach().numpy()])
inlier_cloud.points = o3d.utility.Vector3dVector(see)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
5.三维圆与轴线的散点计算
- 已知圆心、半径、圆所在平面方程，计算该圆的散点和轴线散点：
#空间圆可视化https://www.doc88.com/p-813917521845.html
def get_points_of_circle_3d(line, cir, r):
'''
已知圆心、半径、圆所在平面方程，计算该圆的散点和轴线散点
'''
A, B, C, D = line
#取平面上不贡献三个点，组成不共线两个向量
p1 = np.array([0, 0, -1 * D / C])
p2 = np.array([1, 0, (-1 * D - A) / C])
p3 = np.array([0, 1, (-1 * D - B) / C])
u1 = p1 - p2
u2 = p1 - p3
#法向量
n = np.cross(u1, u2)
n_cir = [cir + n * x for x in np.arange(-0.1, 0.1, 0.001)]
#print(n)
#圆平面建立坐标系
v = np.cross(u1, n)
#转为单位向量
u = u1 / (np.dot(u1, u1.T) ** 0.5)
v = v / (np.dot(v, v.T) ** 0.5)
#根据参数方程生成圆的散点
import math
p_cir = [cir + u * r * math.cos(x) + v * r * math.sin(x) for x in np.arange(0, 2*3.15 ,0.05)]
return np.array(p_cir), np.array(n_cir)
6.可视化绘制与新点云保存
- 计算结果与原始点云叠加显示：
p_cir, n_cir = get_points_of_circle_3d(line.detach().numpy(), cir.detach().numpy(), r.detach().numpy())
see = np.row_stack([cir.detach().numpy(), p_cir, n_cir])
cir_cloud = o3d.geometry.PointCloud()
cir_cloud.points = o3d.utility.Vector3dVector(see)
cir_cloud.paint_uniform_color([0, 1.0, 0])
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, outlier_cloud, cir_cloud])
- 新建点云保存结果：
new_pcd = o3d.geometry.PointCloud()
new_pcd.points = o3d.utility.Vector3dVector(np.row_stack([np.array(inlier_cloud.points),
np.array(outlier_cloud.points),
np.array(outlier_cloud2.points),
np.array(cir_cloud.points)]))
new_pcd.colors = o3d.utility.Vector3dVector(np.row_stack([np.array(inlier_cloud.colors),
np.array(outlier_cloud.colors),
np.array(outlier_cloud2.colors),
np.array(cir_cloud.colors)]))
o3d.io.write_point_cloud(file_before + file + '/new.ply', new_pcd)
7.最终效果
- 绿色为计算拟合出来的散点圆弧和轴线
- 红色内用来拟合的内点
- 黑色为过滤出去的外点（离群点）
8.完整代码
import open3d as o3d
import numpy as np
from numpy.linalg import det
import random
import torch
def model(points: torch.tensor, cir: torch.tensor, line: torch.tensor):
'''
功能：
根据圆心和待拟合点计算损失
输入：
待拟合点，待优化圆心（二维），端面平面方程
输出：
圆心，半径，损失
'''
line = line.float()
points = points.float()
cir = cir.float()
#计算圆心三维坐标
cir_z = torch.matmul(cir, line[0:2].T) + line[-1]
cir_z = cir_z / (1e-10 - line[2])
cir_z = cir_z.unsqueeze(0)
cir = torch.cat([cir, cir_z], 0)
#计算半径矩阵和损失
#损失一定程度上表示每个点到圆心的距离的差距
points = points - cir
points = torch.matmul(points, points.T)
points = torch.diag(points)
n_all = points.shape[0]
r_all = torch.sum(points) / (n_all ** 1)
e = 0
for i in range(1,4):
n = int(n_all / i)
r = torch.sum(points[:n]) / (n ** 1)
e += ((r - r_all) ** 2 )
return cir, r_all ** 0.5, e
file = '3'
file_before = 'duanmian/'
pcd = o3d.io.read_point_cloud(file_before + file + '/point_cloud_00000.ply')
#pcd = pcd.voxel_down_sample(voxel_size=5e-3)
points = np.array(pcd.points)
colors = np.zeros(np.array(pcd.points).shape[0])
pcd.colors = o3d.utility.Vector3dVector(np.zeros(np.array(pcd.colors).shape))
#o3d.visualization.draw_geometries([pcd])
pcd = pcd.uniform_down_sample(every_k_points = 20)
#o3d.visualization.draw_geometries([pcd])
o3d.io.write_point_cloud(file_before + file + '/old.ply', pcd)
plane_model, inliers = pcd.segment_plane(distance_threshold=3 * 1e-3,
ransac_n=3,
num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
inlier_cloud = pcd.select_by_index(inliers)
outlier_cloud = pcd.select_by_index(inliers, invert=True)
print('------开始滤波------')
#参考https://blog.csdn.net/skycol/article/details/127429843
#统计滤波
# nb_neighbors:最近k个点 std_ratio:基于标准差的阈值，越小滤除点越多
cl,ind = inlier_cloud.remove_statistical_outlier(nb_neighbors=3, std_ratio=1)
inlier_cloud = inlier_cloud.select_by_index(ind)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud2 = inlier_cloud.select_by_index(ind, invert=True)
#半径滤波
# nb_points:基于球体内包含点数量的阈值 radius:半径
#cl,ind = inlier_cloud.remove_radius_outlier(nb_points=3, radius = 1.0)
#inlier_cloud = inlier_cloud.select_by_index(ind)
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, outlier_cloud2])
points_2 = np.array(inlier_cloud.points) #* 100
cir =torch.from_numpy(points_2[0][0:2])#选取第一个点作为初始化圆心
cir.requires_grad = True
points_2 = torch.from_numpy(points_2)
line = torch.Tensor(np.array([a, b, c, d]))
learning_rate_o = 1e-3
learning_rate_2 = 1e-2
learning_rate_3 = 1
learning_rate_4 = 8
repect_n = 0
repect = 0
epoch = 0
jingdu = 1e-28
epoch_max = 5 * 1e5
print('-------开始学习---------')
while(True):
epoch += 1
if cir.grad is not None:
#梯度归零
cir.grad.zero_()
#前向传播
_, r, l = model(points_2, cir, line)
#反向传播
l.backward()
if cir.grad is None:
#梯度爆炸就及时退出
print('++++++++++++')
print('epoch:', epoch)
print('a:', cir)
print('grad:', cir.grad)
print('r:', r)
break
#分段学习率
if l < 100:
learning_rate = learning_rate_2
if l < 45:
learning_rate = learning_rate_3
if l < 0.2:
learning_rate = learning_rate_4
else:learning_rate = learning_rate_3
else:learning_rate = learning_rate_2
else:
learning_rate = learning_rate_o
with torch.no_grad():
cir -= learning_rate * cir.grad
if epoch % 5e3 == 0:
print('------------------')
print('epoch:',epoch)
print('a:', cir)
print('grad:', cir.grad)
print('rate:',learning_rate)
print('loss:', l)
print('r:', r.item())
if l < jingdu:
print('精度足够，停止学习')
break
if epoch > epoch_max:
break
if l == repect:
repect_n += 1
else:
repect = l
repect_n = 0
if repect_n > 15:
print('达到收敛停止学习')
break
print('*****************************')
print('epoch:',epoch)
print('a:', cir)
print('grad:', cir.grad)
print('rate:',learning_rate)
print('loss:', l)
print('r:', r.item())
cir, r, l = model(points_2, cir, line)
print('圆心坐标:(', cir, '),半径:', r.item())
see = np.row_stack([np.array(inlier_cloud.points), cir.detach().numpy()])
inlier_cloud.points = o3d.utility.Vector3dVector(see)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
#o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
#空间圆可视化https://www.doc88.com/p-813917521845.html
def get_points_of_circle_3d(line, cir, r):
'''
已知圆心、半径、圆所在平面方程，计算该圆的散点和轴线散点
'''
A, B, C, D = line
#取平面上不贡献三个点，组成不共线两个向量
p1 = np.array([0, 0, -1 * D / C])
p2 = np.array([1, 0, (-1 * D - A) / C])
p3 = np.array([0, 1, (-1 * D - B) / C])
u1 = p1 - p2
u2 = p1 - p3
#法向量
n = np.cross(u1, u2)
n_cir = [cir + n * x for x in np.arange(-0.1, 0.1, 0.001)]
#print(n)
#圆平面建立坐标系
v = np.cross(u1, n)
#转为单位向量
u = u1 / (np.dot(u1, u1.T) ** 0.5)
v = v / (np.dot(v, v.T) ** 0.5)
#根据参数方程生成圆的散点
import math
p_cir = [cir + u * r * math.cos(x) + v * r * math.sin(x) for x in np.arange(0, 2*3.15 ,0.05)]
return np.array(p_cir), np.array(n_cir)
p_cir, n_cir = get_points_of_circle_3d(line.detach().numpy(), cir.detach().numpy(), r.detach().numpy())
see = np.row_stack([cir.detach().numpy(), p_cir, n_cir])
cir_cloud = o3d.geometry.PointCloud()
cir_cloud.points = o3d.utility.Vector3dVector(see)
cir_cloud.paint_uniform_color([0, 1.0, 0])
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, outlier_cloud, cir_cloud])
new_pcd = o3d.geometry.PointCloud()
new_pcd.points = o3d.utility.Vector3dVector(np.row_stack([np.array(inlier_cloud.points),
np.array(outlier_cloud.points),
np.array(outlier_cloud2.points),
np.array(cir_cloud.points)]))
new_pcd.colors = o3d.utility.Vector3dVector(np.row_stack([np.array(inlier_cloud.colors),
np.array(outlier_cloud.colors),
np.array(outlier_cloud2.colors),
np.array(cir_cloud.colors)]))
o3d.io.write_point_cloud(file_before + file + '/new.ply', new_pcd)
