目录
一、概述
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应用场景
- 计算机视觉和图像处理：在物体表面拟合、3D重建和立体视觉中，帮助理解物体的几何形状和结构。
- 地理信息系统（GIS）和遥感：在地形建模和分析中，用于生成数字高程模型（DEM）和分析地貌特征。
- 机器人学和导航：在路径规划和SLAM中，帮助机器人感知环境并进行定位和导航。
- 工程和结构分析：在土木工程和建筑中，用于测量建筑物和结构物的平整度和倾斜度。
- 医学图像处理：在医学成像中，用于分析器官和组织的表面特征，辅助诊断和治疗
二、代码实现
2.1关键函数
在 fit_plane_least_squares 函数中，我们将点云数据的 x 和 y 坐标以及一个常数 1 作为矩阵 A，将 z 坐标作为向量 b。求解线性系统后，我们获得了平面的参数 a, b 和 d。平面方程为 ax + by + cz + d = 0，因此 c = -1
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, d2.2完整代码
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平面拟合方程
平面拟合结果为：-0.004528 * x + 0.363171 * y + -1.000000 * z + 0.002728 = 0
