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