基于已知尺寸的点云多3D圆拟合及误差优化技术咨询

核心思路 #

要实现三个3D圆的精准拟合并结合已知尺寸修正误差，需按以下三步执行：

• 点云特征分离：从混合点云中拆分出外圆和两个内圆的点集合
• 带约束的拟合优化：先用RANSAC得到初始参数，再固定已知半径用最小二乘法优化中心与法向量，最小化误差
• 生成洁净点云：基于最终拟合参数生成无噪声的完美圆点云

一、点云特征分离 #

先通过RANSAC拟合外圆，提取外圆内点并从原始点云中移除；再对剩余点用DBSCAN聚类，分离出两个内圆的点云：

# 拟合外圆并移除外圆点云
cir_outer = pyrsc.Circle()
center_outer, normal_outer, radius_outer, inliers_outer = cir_outer.fit(points, thresh=0.4, maxIteration=10000)
# 提取非外圆点云（即两个内圆的点）
outliers_outer = np.setdiff1d(np.arange(len(points)), inliers_outer)
inner_points = points[outliers_outer]

# 用DBSCAN聚类分离两个内圆
pcd_inner = o3d.geometry.PointCloud()
pcd_inner.points = o3d.utility.Vector3dVector(inner_points)
labels = np.array(pcd_inner.cluster_dbscan(eps=1.0, min_points=100))
# 分离两个内圆的点云
inner1_points = inner_points[labels == 0]
inner2_points = inner_points[labels == 1]

二、带已知半径约束的拟合优化 #

对于每个圆，先通过RANSAC获取初始参数，再固定已知半径，用最小二乘法优化中心和法向量，使点到圆的距离误差最小：

def fit_circle_with_fixed_radius(points, fixed_radius):
    # 先RANSAC获取初始参数
    cir = pyrsc.Circle()
    center_init, normal_init, _, _ = cir.fit(points, thresh=0.3, maxIteration=5000)
    
    # 定义误差函数：点到圆的距离平方和
    def error_func(params):
        center = params[:3]
        normal = params[3:6]
        normal = normal / np.linalg.norm(normal)  # 保证法向量单位化
        total_error = 0.0
        for p in points:
            # 点到圆心向量
            vec = p - center
            # 投影到法向量方向的距离（点到圆所在平面的距离）
            dist_plane = np.abs(np.dot(vec, normal))
            # 投影到圆平面内的向量长度
            proj_len = np.linalg.norm(vec - np.dot(vec, normal)*normal)
            # 点到圆的距离（平面内到圆周的距离 + 平面外距离）
            dist_circle = np.abs(proj_len - fixed_radius)
            total_error += dist_plane**2 + dist_circle**2
        return total_error
    
    # 初始参数：中心+法向量
    init_params = np.concatenate([center_init, normal_init])
    # 优化求解
    from scipy.optimize import minimize
    result = minimize(error_func, init_params, method='L-BFGS-B')
    
    # 提取优化后的参数
    center_opt = result.x[:3]
    normal_opt = result.x[3:6]
    normal_opt = normal_opt / np.linalg.norm(normal_opt)
    return center_opt, normal_opt, fixed_radius

# 已知三个圆的真实半径
KNOWN_RADII = [25, 3, 5]
# 拟合外圆（带约束）
center_outer_opt, normal_outer_opt, radius_outer_opt = fit_circle_with_fixed_radius(points[inliers_outer], KNOWN_RADII[0])
# 拟合第一个内圆
center_inner1_opt, normal_inner1_opt, radius_inner1_opt = fit_circle_with_fixed_radius(inner1_points, KNOWN_RADII[1])
# 拟合第二个内圆
center_inner2_opt, normal_inner2_opt, radius_inner2_opt = fit_circle_with_fixed_radius(inner2_points, KNOWN_RADII[2])

三、生成洁净点云 #

根据优化后的参数，生成无噪声的完美3D圆点云：

def generate_clean_circle_points(center, normal, radius, num_points=1000):
    # 生成圆所在平面的正交基
    if np.allclose(normal, [0,0,1]) or np.allclose(normal, [0,0,-1]):
        u = np.array([1,0,0])
        v = np.array([0,1,0])
    else:
        u = np.cross(normal, [0,0,1])
        u = u / np.linalg.norm(u)
        v = np.cross(normal, u)
        v = v / np.linalg.norm(v)
    
    theta = np.linspace(0, 2*np.pi, num_points)
    points = center + radius * (np.cos(theta)[:,None]*u + np.sin(theta)[:,None]*v)
    return points

# 生成三个洁净圆的点云
clean_outer = generate_clean_circle_points(center_outer_opt, normal_outer_opt, radius_outer_opt)
clean_inner1 = generate_clean_circle_points(center_inner1_opt, normal_inner1_opt, radius_inner1_opt)
clean_inner2 = generate_clean_circle_points(center_inner2_opt, normal_inner2_opt, radius_inner2_opt)

# 合并并可视化
clean_points = np.concatenate([clean_outer, clean_inner1, clean_inner2])
clean_pcd = o3d.geometry.PointCloud()
clean_pcd.points = o3d.utility.Vector3dVector(clean_points)
clean_pcd.paint_uniform_color([0,1,0])
o3d.visualization.draw_geometries([clean_pcd])

完整可运行代码 #

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import pyransac3d as pyrsc
import open3d as o3d
from scipy.optimize import minimize

### 创建模拟点云
# 参数设置
radius = 25
num_points = 1000
small_circle_points = 500
noise_std_xy = 0.1
noise_std_z = 0.1
num_small_circles = 2

# 生成外圆点云
theta = np.linspace(0, 2*np.pi, num_points)
x = radius * np.cos(theta)
y = radius * np.sin(theta)
z = np.zeros_like(x)
x += np.random.normal(0, noise_std_xy, num_points)
y += np.random.normal(0, noise_std_xy, num_points)
z += np.random.normal(0, noise_std_z, num_points)

# 生成两个内圆点云
for i in range(num_small_circles):
    small_radius = 3 if i ==0 else 5
    small_theta = np.linspace(0, 2*np.pi, small_circle_points)
    small_x = small_radius * np.cos(small_theta)
    small_y = small_radius * np.sin(small_theta)
    small_z = np.zeros_like(small_x)
    
    small_x += np.random.normal(0, noise_std_xy, small_circle_points)
    small_y += np.random.normal(0, noise_std_xy, small_circle_points)
    small_z += np.random.normal(0, noise_std_z, small_circle_points)
    
    if i ==0:
        shift =12
        small_x += shift
        small_y += shift
    
    x = np.concatenate((x, small_x))
    y = np.concatenate((y, small_y))
    z = np.concatenate((z, small_z))

points = np.column_stack((x, y, z))

### 1. 点云特征分离
# 拟合外圆并提取外圆内点
cir_outer = pyrsc.Circle()
center_outer, normal_outer, radius_outer, inliers_outer = cir_outer.fit(points, thresh=0.4, maxIteration=10000)
# 获取内圆点云（移除外圆点）
outliers_outer = np.setdiff1d(np.arange(len(points)), inliers_outer)
inner_points = points[outliers_outer]

# DBSCAN聚类分离两个内圆
pcd_inner = o3d.geometry.PointCloud()
pcd_inner.points = o3d.utility.Vector3dVector(inner_points)
labels = np.array(pcd_inner.cluster_dbscan(eps=1.0, min_points=100))
inner1_points = inner_points[labels ==0]
inner2_points = inner_points[labels ==1]

### 2. 带固定半径约束的拟合优化
def fit_circle_with_fixed_radius(points, fixed_radius):
    # RANSAC初始拟合
    cir = pyrsc.Circle()
    center_init, normal_init, _, _ = cir.fit(points, thresh=0.3, maxIteration=5000)
    
    # 误差函数：点到圆的距离平方和
    def error_func(params):
        center = params[:3]
        normal = params[3:6]
        normal = normal / np.linalg.norm(normal)
        total_error =0.0
        for p in points:
            vec = p - center
            dist_plane = np.abs(np.dot(vec, normal))
            proj_len = np.linalg.norm(vec - np.dot(vec, normal)*normal)
            dist_circle = np.abs(proj_len - fixed_radius)
            total_error += dist_plane**2 + dist_circle**2
        return total_error
    
    # 优化求解
    init_params = np.concatenate([center_init, normal_init])
    result = minimize(error_func, init_params, method='L-BFGS-B')
    
    center_opt = result.x[:3]
    normal_opt = result.x[3:6]
    normal_opt = normal_opt / np.linalg.norm(normal_opt)
    return center_opt, normal_opt, fixed_radius

# 已知三个圆的真实半径
KNOWN_RADII = [25, 3, 5]
# 拟合三个圆
center_outer_opt, normal_outer_opt, radius_outer_opt = fit_circle_with_fixed_radius(points[inliers_outer], KNOWN_RADII[0])
center_inner1_opt, normal_inner1_opt, radius_inner1_opt = fit_circle_with_fixed_radius(inner1_points, KNOWN_RADII[1])
center_inner2_opt, normal_inner2_opt, radius_inner2_opt = fit_circle_with_fixed_radius(inner2_points, KNOWN_RADII[2])

### 3. 生成洁净点云
def generate_clean_circle_points(center, normal, radius, num_points=1000):
    # 构建圆平面的正交基
    if np.allclose(normal, [0,0,1]) or np.allclose(normal, [0,0,-1]):
        u = np.array([1,0,0])
        v = np.array([0,1,0])
    else:
        u = np.cross(normal, [0,0,1])
        u = u / np.linalg.norm(u)
        v = np.cross(normal, u)
        v = v / np.linalg.norm(v)
    
    theta = np.linspace(0, 2*np.pi, num_points)
    circle_points = center + radius * (np.cos(theta)[:,None]*u + np.sin(theta)[:,None]*v)
    return circle_points

# 生成洁净点云
clean_outer = generate_clean_circle_points(center_outer_opt, normal_outer_opt, radius_outer_opt)
clean_inner1 = generate_clean_circle_points(center_inner1_opt, normal_inner1_opt, radius_inner1_opt)
clean_inner2 = generate_clean_circle_points(center_inner2_opt, normal_inner2_opt, radius_inner2_opt)

# 可视化原始点云与洁净点云
sourcepcd = o3d.geometry.PointCloud()
sourcepcd.points = o3d.utility.Vector3dVector(points)
sourcepcd.paint_uniform_color([0.5,0.5,0.5])

clean_pcd = o3d.geometry.PointCloud()
clean_pcd.points = o3d.utility.Vector3dVector(np.concatenate([clean_outer, clean_inner1, clean_inner2]))
clean_pcd.paint_uniform_color([0,1,0])

o3d.visualization.draw_geometries([sourcepcd, clean_pcd])

内容的提问来源于stack exchange，提问作者Gururaj Horakeri