先用最小二乘法确定二维平面内的圆心坐标和半径
https://blog.csdn.net/weixin_53610475/article/details/131227331想要运行的更加精确，可以更改代码中的学习率，学习率越小误差越大，但计算的快；太大的话不收敛，要合适。
也可以更改步骤epoch_max = 5 * 1e5
我有个待解决的问题，就是产生的损失很大，不能像参考文献那样损失很小，这可能会产生一定误差。
代码：
import open3d as o3d
import numpy as np
from numpy.linalg import det
import random
import torch
import matplotlib.pyplot as plt
class CircleFitter:
""""
读取pcd文件路径，在二维平面内使用最小二乘法拟合圆,估算圆心和半径
"""
def __init__(self):
pass
@staticmethod
def fit_circle_least_squares(points):
A = np.c_[2*points[:,0], 2*points[:,1], np.ones(points.shape[0])]
B = np.sum(points**2, axis=1)
C, _, _, _ = np.linalg.lstsq(A, B, rcond=None)
a, b, c = C
center = np.array([a, b])
radius = np.sqrt(center[0]**2 + center[1]**2 + c)
return center, radius
def circle_center_guess(self, file):
# 读取PCD文件
self.pcd = o3d.io.read_point_cloud(file)
# 将点云转换为numpy数组
self.points = np.asarray(self.pcd.points)
# 假设端面在Z轴上，投影到XY平面
points_2d = self.points[:, :2]
points_2d = np.float32(points_2d) # 这一步其实是多余的，因为numpy通常会自动处理类型
# 拟合圆
center, radius = self.fit_circle_least_squares(points_2d)
# center = (round(center[0], 8), round(center[1], 8))
radius = round(radius, 8)
center = np.array([round(center[0], 8), round(center[1], 8)])
# 将中心坐标转换为字典
# center_dict = {
# "x": f"{center[0]:.8f}",
# "y": f"{center[1]:.8f}"
# }
# 可选：在点云上绘制拟合结果（可视化）
plt.scatter(points_2d[:, 0], points_2d[:, 1], s=1)
circle = plt.Circle(center, radius, color='r', fill=False)
plt.gca().add_patch(circle)
plt.gca().set_aspect('equal', adjustable='box')
plt.show()
return center
# # 使用示例
# fitter = CircleFitter()
# center_dict = fitter.circle_center_guess('F:/Desktop/test/duanmian/point_cloud_00007 - Cloud.pcd')
# print("center", center_dict)
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_path = r'F:/Desktop/test/duanmian/point_cloud_00007 - Cloud.pcd'
pcd = o3d.io.read_point_cloud(file_path)
# pcd = o3d.io.read_point_cloud(file+ '/point_cloud_00000.ply')
# num_points = len(pcd.points)
# print(f"点云中点的数量: {num_points}")
#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))
#点云预处理,均匀降采样
pcd = pcd.uniform_down_sample(every_k_points = 100)
pcd.paint_uniform_color([1.0, 0, 0])
# o3d.visualization.draw_geometries([pcd])
# o3d.io.write_point_cloud( r'F:/Desktop/dalainyike/duanmian/old.ply', pcd)
plane_model, inliers = pcd.segment_plane(distance_threshold=1,
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)
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
print('------开始滤波------')
#参考https://blog.csdn.net/skycol/article/details/127429843
#统计滤波
# nb_neighbors:最近k个点 std_ratio:基于标准差的阈值，越小滤除点越多
# cl,ind = inlier_cloud.remove_statistical_outlier(nb_neighbors=2, 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)
# outlier_cloud2.paint_uniform_color([0, 0, 0]) # 黑色
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud2])
inlier_cloud_filtered, ind = inlier_cloud.remove_statistical_outlier(nb_neighbors=3, std_ratio=1)
# 保留的点云
inlier_cloud_filtered.paint_uniform_color([1.0, 0, 0]) # 红色
# 异常值点云（需要从原始点云中获取）
outlier_cloud2 = inlier_cloud.select_by_index(np.delete(np.arange(len(inlier_cloud.points)), ind))
outlier_cloud2.paint_uniform_color([0, 0,0]) # 黑色
# 显示点云
o3d.visualization.draw_geometries([inlier_cloud_filtered, outlier_cloud2])
#估测圆心坐标和半径
fitter = CircleFitter()
center_dict = fitter.circle_center_guess(file_path)
# print("center", center_dict)
points_2 = np.array(inlier_cloud.points) #* 100
guess_center=center_dict#使用最小二乘法拟合圆，来粗略估计圆心
cir =torch.from_numpy(guess_center)
cir.requires_grad = True
points_2 = torch.from_numpy(points_2)
line = torch.Tensor(np.array([a, b, c, d]))#平面方程参数
learning_rate_o = 1e-8##适当调整学习率
learning_rate_2 = 1e-5
learning_rate_3 = 1
learning_rate_4 = 8
repect_n = 0
repect = 0
epoch = 0
jingdu = 2
# 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())
# 将 PyTorch Tensor 转换为 numpy 数组
cir_np = cir.detach().numpy()
# 创建一个球体来代替圆心
center_sphere = o3d.geometry.TriangleMesh.create_sphere(radius=3.0) # 调整半径以加粗
center_sphere.translate(cir_np) # 将球体移动到圆心位置
center_sphere.paint_uniform_color([1, 0, 0]) # 将球体上色为红色
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(-20, 20, 0.1)]#沿着单位法线方向从圆心生成点,+-20数值可以加以调整，以便更直观的体现法向量
#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*np.pi ,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])
o3d.visualization.draw_geometries([inlier_cloud,outlier_cloud2, center_sphere,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(r'F:/Desktop/dalianyike/duanmian/new.pcd', new_pcd)
效果图：
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
