目录
一、概述
1.1原理
1.2实现步骤
二、代码实现
2.1关键代码
2.1.1定义残差函数
2.1.2拟合球面
2.2完整代码
三、实现效果
3.1原始点云
3.2拟合后点云
3.3结果数据
前期试读,后续会将博客加入下列链接的专栏,欢迎订阅
Open3D点云算法与点云深度学习案例汇总(长期更新)-CSDN博客
一、概述
非线性最小二乘法是拟合复杂曲线和曲面(如球)的常用方法。拟合三维空间中的球面可以通过非线性最小二乘法来实现,使用如 Levenberg-Marquardt 算法进行优化。
1.1原理
1.2实现步骤
1.生成或读取点云数据:
使用 Open3D 生成或读取三维空间中的点云数据。
2.定义目标函数:
定义用于非线性最小二乘法优化的目标函数。
3.初始参数估计:
选择初始的球心和半径估计值。
4.优化求解:
使用 scipy.optimize.least_squares 或其他优化算法进行非线性最小二乘法优化,拟合球面。
5.可视化结果:
使用 Open3D 可视化原始点云数据和拟合的球面。
二、代码实现
2.1关键代码
2.1.1定义残差函数
使用 residuals 函数计算球面拟合的残差。残差是每个点到球面的距离减去球的半径。
def residuals(params, points):
"""
计算残差,用于非线性最小二乘法优化。
参数:
params (numpy.ndarray): 球的参数 (a, b, c, r)。
points (numpy.ndarray): 点云数据,形状为 (N, 3)。
返回:
numpy.ndarray: 残差,形状为 (N,)。
"""
a, b, c, r = params
residuals = np.sqrt((points[:, 0] - a)**2 + (points[:, 1] - b)**2 + (points[:, 2] - c)**2) - r
return residuals
2.1.2拟合球面
使用 fit_sphere_least_squares 函数进行非线性最小二乘法优化,拟合球面。初始参数估计使用点云数据的均值和平均距离。
def fit_sphere_least_squares(points):
"""
使用非线性最小二乘法拟合球面。
参数:
points (numpy.ndarray): 点云数据,形状为 (N, 3)。
返回:
tuple: 拟合的球的参数 (a, b, c, r)。
"""
# 初始参数估计
center_estimate = np.mean(points, axis=0)
radius_estimate = np.mean(np.sqrt(np.sum((points - center_estimate)**2, axis=1)))
initial_params = np.hstack((center_estimate, radius_estimate))
# 非线性最小二乘法优化
result = least_squares(residuals, initial_params, args=(points,))
return result.x
2.2完整代码
import open3d as o3d
import numpy as np
from scipy.optimize import least_squares
def generate_noisy_sphere(center, radius, num_points=1000, noise_level=0.05):
"""
生成带有噪声的球面点云数据。
参数:
center (tuple): 球心的坐标 (a, b, c)。
radius (float): 球的半径。
num_points (int): 点的数量。
noise_level (float): 噪声水平。
返回:
numpy.ndarray: 生成的点云数据。
"""
phi = np.random.uniform(0, np.pi, num_points)
theta = np.random.uniform(0, 2 * np.pi, num_points)
x = center[0] + radius * np.sin(phi) * np.cos(theta)
y = center[1] + radius * np.sin(phi) * np.sin(theta)
z = center[2] + radius * np.cos(phi)
points = np.vstack((x, y, z)).T
noise = np.random.normal(0, noise_level, points.shape)
noisy_points = points + noise
return noisy_points
def residuals(params, points):
"""
计算残差,用于非线性最小二乘法优化。
参数:
params (numpy.ndarray): 球的参数 (a, b, c, r)。
points (numpy.ndarray): 点云数据,形状为 (N, 3)。
返回:
numpy.ndarray: 残差,形状为 (N,)。
"""
a, b, c, r = params
residuals = np.sqrt((points[:, 0] - a)**2 + (points[:, 1] - b)**2 + (points[:, 2] - c)**2) - r
return residuals
def fit_sphere_least_squares(points):
"""
使用非线性最小二乘法拟合球面。
参数:
points (numpy.ndarray): 点云数据,形状为 (N, 3)。
返回:
tuple: 拟合的球的参数 (a, b, c, r)。
"""
# 初始参数估计
center_estimate = np.mean(points, axis=0)
radius_estimate = np.mean(np.sqrt(np.sum((points - center_estimate)**2, axis=1)))
initial_params = np.hstack((center_estimate, radius_estimate))
# 非线性最小二乘法优化
result = least_squares(residuals, initial_params, args=(points,))
return result.x
def create_sphere_mesh(center, radius, resolution=20):
"""
创建一个球面的 Mesh,用于可视化。
参数:
center (tuple): 球心的坐标 (a, b, c)。
radius (float): 球的半径。
resolution (int): 球的分辨率。
返回:
open3d.geometry.TriangleMesh: 球面 Mesh 对象。
"""
mesh = o3d.geometry.TriangleMesh.create_sphere(radius=radius, resolution=resolution)
mesh.translate(center)
mesh.compute_vertex_normals()
return mesh
# 生成带有噪声的球面点云数据
center = (0.0, 0.0, 0.0)
radius = 1.0
num_points = 1000
noise_level = 0.05
points = generate_noisy_sphere(center, radius, num_points, noise_level)
# 使用非线性最小二乘法拟合球面
fitted_params = fit_sphere_least_squares(points)
fitted_center = fitted_params[:3]
fitted_radius = fitted_params[3]
print(f"拟合的球心: {fitted_center},拟合的半径: {fitted_radius}")
# 创建点云对象
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
o3d.visualization.draw_geometries([pcd], window_name="pcd",
width=800, height=600, left=50, top=50)
# 创建拟合的球面的 Mesh 对象
sphere_mesh = create_sphere_mesh(fitted_center, fitted_radius)
# 可视化点云和拟合的球面
o3d.visualization.draw_geometries([pcd, sphere_mesh], window_name="Nonlinear Least Squares Sphere Fitting",
width=800, height=600, left=50, top=50)
三、实现效果
3.1原始点云
3.2拟合后点云
3.3结果数据
拟合的球心: [-0.00122239 -0.00153872 -0.00223764],拟合的半径: 1.0035838562569899