目錄

一、概述

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

def fit_plane_least_squares(points):"""使用最小二乘法直接求解擬合點云平面。參數:points (numpy.ndarray): 點云數據，形狀為 (N, 3)。返回:plane (tuple): 平面參數 (a, b, c, d)，其中 ax + by + cz + d = 0。"""# 構建矩陣 A 和向量 bA = np.c_[points[:, :2], np.ones(points.shape[0])]b = points[:, 2]# 求解線性系統 A^T A [a, b, d]^T = A^T bx, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)# 返回平面參數 (a, b, c, d)a, b, d = xc = -1.0  # 平面法向量的z分量return a, b, c, d

2.2完整代碼

import open3d as o3d
import numpy as np
import matplotlib.pyplot as pltdef fit_plane_least_squares(points):"""使用最小二乘法直接求解擬合點云平面。參數:points (numpy.ndarray): 點云數據，形狀為 (N, 3)。返回:plane (tuple): 平面參數 (a, b, c, d)，其中 ax + by + cz + d = 0。"""# 構建矩陣 A 和向量 bA = np.c_[points[:, :2], np.ones(points.shape[0])]b = points[:, 2]# 求解線性系統 A^T A [a, b, d]^T = A^T bx, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)# 返回平面參數 (a, b, c, d)a, b, d = xc = -1.0  # 平面法向量的z分量return a, b, c, ddef 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-10fig1 = 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