Open3D 基于最小二乘法拟合点云平面（SVD分解法）

SVD 分解法拟合点云平面在多个领域有广泛应用，包括但不限于：

计算机视觉：在 3D 重建、物体识别和跟踪等应用中，通过拟合平面可以进行平面检测和分割。
机器人导航：在 SLAM（同时定位与地图构建）中，通过拟合平面可以检测和建模环境中的平面特征，有助于机器人导航和定位。
地形建模：在地理信息系统（GIS）中，通过拟合平面可以对地形数据进行建模和分析，用于生成数字高程模型（DEM）。
建筑和工程：在建筑和工程中，通过拟合平面可以对建筑物的墙壁、地面等进行建模和测量，进行结构分析和监测

二、代码实现

2.1关键函数

def fit_plane_svd(points):
    """
    使用 SVD 分解法拟合点云平面。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 3)。

    返回:
    plane (tuple): 平面参数 (a, b, c, d)，其中 ax + by + cz + d = 0。
    """
    # 1. 计算点云质心（均值）
    centroid = np.mean(points, axis=0)

    # 2. 中心化点云数据
    centered_points = points - centroid

    # 3. 计算中心化点云数据的 SVD 分解
    U, S, Vt = np.linalg.svd(centered_points)

    # 4. Vt 的最后一行对应于最小特征值的特征向量，是平面的法向量
    normal_vector = Vt[2, :]

    # 5. 计算平面参数 d
    d = -np.dot(normal_vector, centroid)

    # 返回平面参数 (a, b, c, d)
    return normal_vector[0], normal_vector[1], normal_vector[2], d, centroid

2.2完整代码

import open3d as o3d
import numpy as np
from matplotlib import pyplot as plt


def fit_plane_svd(points):
    """
    使用 SVD 分解法拟合点云平面。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 3)。

    返回:
    plane (tuple): 平面参数 (a, b, c, d)，其中 ax + by + cz + d = 0。
    """
    # 1. 计算点云质心（均值）
    centroid = np.mean(points, axis=0)

    # 2. 中心化点云数据
    centered_points = points - centroid

    # 3. 计算中心化点云数据的 SVD 分解
    U, S, Vt = np.linalg.svd(centered_points)

    # 4. Vt 的最后一行对应于最小特征值的特征向量，是平面的法向量
    normal_vector = Vt[2, :]

    # 5. 计算平面参数 d
    d = -np.dot(normal_vector, centroid)

    # 返回平面参数 (a, b, c, d)
    return normal_vector[0], normal_vector[1], normal_vector[2], d, centroid


def create_plane_mesh(plane_params, width, height):
    """
    创建一个平面网格。

    参数:
    plane_params (tuple): 平面参数 (a, b, c, d)。
    width (float): 平面的宽度。
    height (float): 平面的高度。

    返回:
    plane_mesh (open3d.geometry.TriangleMesh): 平面网格。
    """
    # 创建平面网格
    plane_mesh = o3d.geometry.TriangleMesh()
    vertices = [
        [width / 2, height / 2, 0],
        [-width / 2, height / 2, 0],
        [-width / 2, -height / 2, 0],
        [width / 2, -height / 2, 0],
    ]
    triangles = [
        [0, 1, 2],
        [0, 2, 3],
    ]
    plane_mesh.vertices = o3d.utility.Vector3dVector(vertices)
    plane_mesh.triangles = o3d.utility.Vector3iVector(triangles)
    plane_mesh.paint_uniform_color([1, 0, 0])

    # 平面法向量和平面上的一点
    normal = np.array(plane_params[:3])
    centroid = plane_params[4]

    # 计算平面旋转矩阵
    z_axis = np.array([0, 0, 1])
    rotation_axis = np.cross(z_axis, normal)
    rotation_angle = np.arccos(np.dot(z_axis, normal))

    # 检查旋转轴是否为零向量
    if np.linalg.norm(rotation_axis) > 0:
        rotation_axis = rotation_axis / np.linalg.norm(rotation_axis)
        R = o3d.geometry.get_rotation_matrix_from_axis_angle(rotation_axis * rotation_angle)
        plane_mesh.rotate(R, center=(0, 0, 0))

    # 平移平面网格到拟合平面位置
    plane_mesh.translate(centroid)

    return plane_mesh

def create_fitted_plane_points(plane_params, points):
    """
    创建拟合平面的点云。

    参数:
    plane_params (tuple): 平面参数 (a, b, c, d)。
    points (numpy.ndarray): 原始点云数据，形状为 (N, 3)。

    返回:
    fitted_plane_points (numpy.ndarray): 拟合平面的点云数据，形状为 (N, 3)。
    """
    A, B, C, D = plane_params[:4]
    xy = points[:, :2]
    z = -(A * xy[:, 0] + B * xy[:, 1] + D) / C
    fitted_plane_points = np.column_stack((xy, z))
    return fitted_plane_points

if __name__ == '__main__':

    # -----------------------------读取点云--------------------------------
    pcd = o3d.io.read_point_cloud(r"E:\work\Open3D\open3d20231128\Blog_Cloud\pcd_data\palen_noise.pcd")

    # 检查并移除 NaN 和无穷大值
    pcd = pcd.remove_non_finite_points()

    # ----------------基于 SVD 分解法的最小二乘拟合平面-----------------------
    points = np.asarray(pcd.points)  # 获取点云数据
    plane_params = fit_plane_svd(points)
    A, B, C, D, centroid = plane_params
    print('平面拟合结果为：%.6f * x + %.6f * y + %.6f * z + %.6f = 0' % (A, B, C, D))

    # =======================================拟合点云平面网格==============================================
    # 获取点云质心用于平移平面
    center = np.mean(points, axis=0)

    # 获取xyz坐标及最值用于生成平面网格
    min_pt = np.amin(points, axis=0)  # 获取坐标最小值
    max_pt = np.amax(points, axis=0)  # 获取坐标最大值

    # 创建拟合的平面网格，增加宽度和高度以覆盖整个点云范围
    plane_mesh = create_plane_mesh(plane_params, max_pt[0] - min_pt[0] + 1, max_pt[1] - min_pt[1] + 1)

    # --------------------------可视化拟合结果(一）----------------------------
    o3d.visualization.draw_geometries([pcd, plane_mesh], window_name="原始点云和拟合平面网格",
                                      width=1024, height=768,
                                      left=50, top=50,
                                      mesh_show_back_face=False)

    #=======================================拟合点云平面==============================================
    # 创建拟合平面的点云
    fitted_plane_points = create_fitted_plane_points(plane_params, points)

    # 创建 Open3D 点云对象
    fitted_plane_pcd = o3d.geometry.PointCloud()
    fitted_plane_pcd.points = o3d.utility.Vector3dVector(fitted_plane_points)
    fitted_plane_pcd.paint_uniform_color([0, 1, 0])  # 将拟合平面的点云设为绿色
    fitted_plane_pcd.translate([0, 0, 1])
    pcd.paint_uniform_color([1, 0, 0])  # 将拟合平面的点云设为红色

    # --------------------------可视化拟合结果----------------------------
    o3d.visualization.draw_geometries([pcd, fitted_plane_pcd], window_name="原始点云和拟合平面点云",
                                      width=1024, height=768,
                                      left=50, top=50,
                                      mesh_show_back_face=False)

    #===================================matplotlib可视化==============================================
    fig1 = plt.figure()
    ax1 = fig1.add_subplot(111, projection='3d')
    ax1.set_xlabel("x")
    ax1.set_ylabel("y")
    ax1.set_zlabel("z")
    # 获取xyz坐标及最值用于plot绘图
    points = np.asarray(pcd.points)  # 获取点坐标
    min_pt = np.amin(points, axis=0)  # 获取坐标最小值
    max_pt = np.amax(points, axis=0)  # 获取坐标最大值
    ax1.scatter(points[:, 0], points[:, 1], points[:, 2], c='r', marker='^')
    x_p = np.linspace(min_pt[0], max_pt[0], 100)
    y_p = np.linspace(min_pt[1], max_pt[1], 100)
    XFit, YFit = np.meshgrid(x_p, y_p)

    ZFit = -(D + A * XFit + B * YFit) / C
    ax1.plot_wireframe(XFit, YFit, ZFit, rstride=10, cstride=10)
    plt.show()

三、实现效果

3.1原始点云

3.2拟合后的网格平面

3.3拟合后的点云平面

3.4matplotlib可视化

3.5平面拟合方程

平面拟合结果为：-0.004233 * x + 0.343034 * y + -0.939313 * z + 0.002577 = 0

原文地址：https://blog.csdn.net/qq_47947920/article/details/140471830

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：DTD 简介
下一篇：最全攻略：网络小白也能看懂的交换机连接方法

leetcode289:生命游戏
根据，简称为，是英国数学家约翰·何顿·康威在 1970 年发明的细胞自动机。给定一个包含m × n个格子的面板，每一个格子都可以看成是一个细胞。每个细胞都具有一个初始状态：1即为（live），或0即为
阅读更多2024-10-20
MongoDB数据恢复
注意：两个MongoDB的版本要一致，本文使用的是mongo:4.2.24。先把K8S上面的MongoDB 容器停止（可以把副本改成0）。1、将容器挂载MongoDB的数据目录备份到本地。经常是数据文
阅读更多2024-10-20
C#中实现事务
C#中实现事务
阅读更多2024-10-20
【LeetCode每日一题】——560.和为 K 的子数组
给你一个整数数组 nums 和一个整数 k ，请你统计并返回该数组中和为 k 的子数组的个数。子数组是数组中元素的连续非空序列。
阅读更多2024-10-20
「漏洞复现」满客宝智慧食堂系统 selectUserByOrgId 未授权访问漏洞
请勿利用文章内的相关技术从事非法测试，由于传播、利用此文所提供的信息而造成的任何直接或者间接的后果及损失，均由使用者本人负责，作者不为此承担任何责任。工具来自网络，安全性自测，如有侵权请联系删除。本次
阅读更多2024-10-20
React面试题目（从基本到高级）
React前端面试常见题目涵盖了React的基础概念、组件、状态管理、生命周期、性能优化等多个方面。
阅读更多2024-10-20
12.个人博客系统（Java项目基于spring和vue）
1 在校学习的学生，可用于日常学习使用或是毕业设计使用 2 毕业一到两年的开发人员，用于锻炼自己的独立功能模块设计能力，增强代码编写能力。 3 亦可以部署为商化项目使用。 4 需要完整资料及源码
阅读更多2024-10-20
YoloV8改进策略：注意力改进|DeBiFormer，可变形双级路由注意力|引入DeBiLevelRoutingAttention注意力模块（全网首发）
本次改进的核心在于将DeBiLevelRoutingAttention模块嵌入到YoloV8的主干网络中，具体位置是在SPPF（Spatial Pyramid Pooling Fast）模块之后。这一
阅读更多2024-10-20
word取消自动单词首字母大写
情况说明：在word输入单词后首字母会自动变成大写取消单词首字母大写步骤：（1）点击菜单栏文件（2）点击“更多”——>“选项”（3）点击“校对”——>“自动更正选项”（4）取消“句首字母大
阅读更多2024-10-20
web前端网页用户注册页面
【代码】web前端网页用户注册页面。
阅读更多2024-10-20

Open3D 基于最小二乘法拟合点云平面（SVD分解法）

一、概述

1.1SVD原理

1.2实现步骤

1.3应用

二、代码实现

2.1关键函数

2.2完整代码

三、实现效果

3.1原始点云

3.2拟合后的网格平面

3.3拟合后的点云平面

3.4matplotlib可视化

3.5平面拟合方程

相关文章