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

目录

一、概述

1.1最小二乘法原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.2完整代码

三、实现效果

3.1原始点云

3.2matplotlib可视化

3.3平面拟合方程


前期试读,后续会将博客加入该专栏,欢迎订阅

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

一、概述

1.1最小二乘法原理

最小二乘法(Least Squares Method) 是一种用于数据拟合的数学优化方法,通过最小化误差平方和来找到最佳拟合参数。在拟合平面时,我们使用最小二乘法来确定平面方程的参数,使得点云数据中的点到该平面的垂直距离的平方和最小。

1.2实现步骤

1.3应用场景

  1. **计算机视觉和图像处理:**在物体表面拟合、3D重建和立体视觉中,帮助理解物体的几何形状和结构。
  2. 地理信息系统(GIS)和遥感:在地形建模和分析中,用于生成数字高程模型(DEM)和分析地貌特征。
  3. **机器人学和导航:**在路径规划和SLAM中,帮助机器人感知环境并进行定位和导航。
  4. **工程和结构分析:**在土木工程和建筑中,用于测量建筑物和结构物的平整度和倾斜度。
  5. **医学图像处理:**在医学成像中,用于分析器官和组织的表面特征,辅助诊断和治疗

二、代码实现

2.1关键函数

fit_plane_least_squares函数中,我们将点云数据的 x 和 y 坐标以及一个常数 1 作为矩阵 A,将 z 坐标作为向量 b。求解线性系统后,我们获得了平面的参数 a, b 和 d。平面方程为 ax + by + cz + d = 0,因此 c = -1

python 复制代码
def fit_plane_least_squares(points):
    """
    使用最小二乘法直接求解拟合点云平面。

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

    返回:
    plane (tuple): 平面参数 (a, b, c, d),其中 ax + by + cz + d = 0。
    """
    # 构建矩阵 A 和向量 b
    A = np.c_[points[:, :2], np.ones(points.shape[0])]
    b = points[:, 2]

    # 求解线性系统 A^T A [a, b, d]^T = A^T b
    x, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)

    # 返回平面参数 (a, b, c, d)
    a, b, d = x
    c = -1.0  # 平面法向量的z分量
    return a, b, c, d

2.2完整代码

python 复制代码
import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt

def fit_plane_least_squares(points):
    """
    使用最小二乘法直接求解拟合点云平面。

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

    返回:
    plane (tuple): 平面参数 (a, b, c, d),其中 ax + by + cz + d = 0。
    """
    # 构建矩阵 A 和向量 b
    A = np.c_[points[:, :2], np.ones(points.shape[0])]
    b = points[:, 2]

    # 求解线性系统 A^T A [a, b, d]^T = A^T b
    x, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)

    # 返回平面参数 (a, b, c, d)
    a, b, d = x
    c = -1.0  # 平面法向量的z分量
    return a, b, c, d

def plot_fitted_plane(points, plane_params):
    """
    绘制点云和拟合平面的网格。

    参数:
    points (numpy.ndarray): 点云数据,形状为 (N, 3)。
    plane_params (tuple): 平面参数 (a, b, c, d),其中 ax + by + cz + d = 0。
    """
    A, B, C, D = plane_params

    # 检查 C 值,避免除零错误
    if np.isclose(C, 0):
        C = 1e-10

    fig1 = plt.figure()
    ax1 = fig1.add_subplot(111, projection='3d')
    ax1.set_xlabel("x")
    ax1.set_ylabel("y")
    ax1.set_zlabel("z")

    # 获取xyz坐标及最值用于plot绘图
    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()

# -----------------------------读取点云--------------------------------
pcd = o3d.io.read_point_cloud("tilted_plane_noise.pcd")

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

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

# 调用绘制网格平面的函数
plot_fitted_plane(points, plane_params)

三、实现效果

3.1原始点云

3.2matplotlib可视化

3.3平面拟合方程

python 复制代码
平面拟合结果为:-0.004528 * x + 0.363171 * y + -1.000000 * z + 0.002728 = 0
相关推荐
pianmian13 小时前
python数据结构基础(7)
数据结构·算法
好奇龙猫5 小时前
【学习AI-相关路程-mnist手写数字分类-win-硬件:windows-自我学习AI-实验步骤-全连接神经网络(BPnetwork)-操作流程(3) 】
人工智能·算法
sp_fyf_20245 小时前
计算机前沿技术-人工智能算法-大语言模型-最新研究进展-2024-11-01
人工智能·深度学习·神经网络·算法·机器学习·语言模型·数据挖掘
香菜大丸6 小时前
链表的归并排序
数据结构·算法·链表
jrrz08286 小时前
LeetCode 热题100(七)【链表】(1)
数据结构·c++·算法·leetcode·链表
oliveira-time6 小时前
golang学习2
算法
老艾的AI世界7 小时前
新一代AI换脸更自然,DeepLiveCam下载介绍(可直播)
图像处理·人工智能·深度学习·神经网络·目标检测·机器学习·ai换脸·视频换脸·直播换脸·图片换脸
南宫生7 小时前
贪心算法习题其四【力扣】【算法学习day.21】
学习·算法·leetcode·链表·贪心算法
懒惰才能让科技进步8 小时前
从零学习大模型(十二)-----基于梯度的重要性剪枝(Gradient-based Pruning)
人工智能·深度学习·学习·算法·chatgpt·transformer·剪枝
Ni-Guvara8 小时前
函数对象笔记
c++·算法