Open3D最小二乘法拟合平滑点云

一、代码

Python

import numpy as np
import open3d as o3d


def fit_plane_least_squares(points):
    # 计算中心点
    centroid = np.mean(points, axis=0)

    # 计算协方差矩阵
    cov_matrix = (points - centroid).T.dot(points - centroid)

    # 计算协方差矩阵的特征值和特征向量
    eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix)

    # 最小特征值对应的特征向量就是平面的法向量
    normal = eigenvectors[:, np.argmin(eigenvalues)]
    return centroid, normal


def smooth_point_cloud(pcd, radius):
    # Create a new PointCloud object
    smoothed_pcd = o3d.geometry.PointCloud()
    smoothed_points = []

    # Build KDTree for the input point cloud
    kdtree = o3d.geometry.KDTreeFlann(pcd)

    # For each point, perform least squares plane fitting
    for point in np.asarray(pcd.points):
        # Find points within the radius
        [k, idx, _] = kdtree.search_radius_vector_3d(point, radius)
        if k < 3:
            # If there are not eno