Open3D 最小二乘法拟合空间曲线（高阶多项式）

目录

一、概述

1.1原理

1.2实现步骤

二、代码实现

1.1关键函数

1.2完整代码

三、实现效果

---

前期试读，后续会将博客加入下列链接的专栏，欢迎订阅

Open3D点云算法与点云深度学习案例汇总（长期更新）-CSDN博客

一、概述

1.1原理

        最小二乘法拟合空间曲线涉及构建一个合适的模型，并通过最小化数据点到模型曲线的距离来确定模型参数。这里我们使用是多项式曲线模型：

1.2实现步骤

1.读取点云数据：
        - 使用 Open3D 读取或生成二维点云数据。
2.构建设计矩阵 A 和观测向量 B：
        - 设计矩阵 𝐴 包含点云数据的各项多项式项。
        - 观测向量 B 包含点云数据的平方和项。
3.解线性方程 AX=B：
        - 使用最小二乘法求解该方程，获得拟合的圆的参数。
4.可视化结果：
        - 使用 Open3D 可视化原始点云数据和拟合的圆。

二、代码实现

1.1关键函数

        使用 fit_high_degree_polynomial 函数拟合高阶多项式曲线。通过 np.polyfit 函数进行拟合，得到多项式的系数。多项式的阶数越高，拟合曲线就越准确。需要根据自己实际点云情况去选择

def fit_polynomial_curve(points, degree=5):
    """
    使用最小二乘法拟合多项式曲线。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 2)。
    degree (int): 多项式的阶数。

    返回:
    numpy.ndarray: 拟合的多项式的系数。
    """
    x = points[:, 0]
    y = points[:, 1]

    coeffs = np.polyfit(x, y, degree)
    return coeffs

1.2完整代码

import open3d as o3d
import numpy as np

def generate_noisy_sine_like_curve(num_points=1000, noise_level=0.05):
    """
    生成带有噪声的类似正弦函数的曲线点云数据。

    参数:
    num_points (int): 点的数量。
    noise_level (float): 噪声水平。

    返回:
    numpy.ndarray: 生成的点云数据。
    """
    x = np.linspace(-10, 10, num_points)
    y = np.sin(x)
    points = np.vstack((x, y)).T

    noise = np.random.normal(0, noise_level, points.shape)
    noisy_points = points + noise

    return noisy_points

def fit_polynomial_curve(points, degree=5):
    """
    使用最小二乘法拟合多项式曲线。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 2)。
    degree (int): 多项式的阶数。

    返回:
    numpy.ndarray: 拟合的多项式的系数。
    """
    x = points[:, 0]
    y = points[:, 1]

    coeffs = np.polyfit(x, y, degree)
    return coeffs

def create_polynomial_curve_mesh(coeffs, x_range, resolution=100):
    """
    创建一个多项式曲线的 Mesh，用于可视化。

    参数:
    coeffs (numpy.ndarray): 多项式的系数。
    x_range (tuple): x 的范围。
    resolution (int): 曲线分辨率。

    返回:
    open3d.geometry.LineSet: 多项式曲线 Mesh 对象。
    """
    x = np.linspace(x_range[0], x_range[1], resolution)
    y = np.polyval(coeffs, x)
    curve_points = np.vstack((x, y)).T

    # 创建线框用于可视化曲线
    lines = [[i, i + 1] for i in range(resolution - 1)]
    line_set = o3d.geometry.LineSet()
    line_set.points = o3d.utility.Vector3dVector(np.c_[curve_points, np.zeros(resolution)])
    line_set.lines = o3d.utility.Vector2iVector(lines)
    line_set.colors = o3d.utility.Vector3dVector([[1, 0, 0]] * len(lines))  # 红色

    return line_set

# 生成带有噪声的类似正弦函数的曲线点云数据
num_points = 1000
noise_level = 0.05
points = generate_noisy_sine_like_curve(num_points, noise_level)

# 使用最小二乘法拟合多项式曲线
degree = 15
coeffs = fit_polynomial_curve(points, degree)

# 创建点云对象
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.c_[points, np.zeros(points.shape[0])])

# 创建拟合的多项式曲线的 Mesh 对象
x_range = (-10, 10)
curve_mesh = create_polynomial_curve_mesh(coeffs, x_range)

# 可视化点云和拟合的多项式曲线
o3d.visualization.draw_geometries([pcd, curve_mesh], window_name="Least Squares Polynomial Curve Fitting",
                                  width=800, height=600, left=50, top=50)

print(f"拟合的多项式曲线系数: {coeffs}")

三、实现效果