MLS平滑濾波

1.前言

最近在學習，因此查閱相關資料，該怎么表述感覺有些困難

2.代碼

2.1代碼1

使用全局坐標系

參考：python點云移動最小二乘法(Moving Least Squares)平滑_移動最小二乘法python-CSDN博客

def Moving_Least_Squares_Smoothing_v1_explain( points, radius, tau, weights_name='gaussian'):"""如果輸入open3d點云，則points = np.asarray(inlier_cloud.points)功能：使用Moving Least Squares Smooth進行點云的平滑必要參數：points: 點云數據[[x1,y1,z1],[x2,y2,z2],...,[xn,yn,zn]]radius: 鄰居搜索半徑tau: 權重系數可選參數：weights_name: 權重函數，可選：gaussian:高斯權重函數。根據距離計算權重，權重隨epanechnikov:Epanechnikov核權重函數。在一定范圍內提供權重，超出這個范圍權重急劇下降。cubic_spline:三次樣條權重函數。使用三次樣條函數計算權重，提供平滑的權重變化。uniform:均勻權重函數。為所有點提供相同的權重。inverse_distance:距離反比權重函數。根據距離的反比例來計算權重。wendland:Wendland權重函數。使用Wendland函數計算局部光滑的權重。shepard_weight:Shepard權重函數。使用距離的負冪計算權重，權重隨距離增大而減小。"""x = points[:, 0]y = points[:, 1]z = points[:, 2]# 構建KD樹以快速找到鄰居tree = KDTree(np.vstack((x, y)).T)smoothed_points = np.zeros((len(x), 3))for i in range(len(x)):# 使用KD樹找到指定半徑內的所有鄰居idx = tree.query_ball_point([x[i], y[i]], r=radius)"""這是 SciPy KDTree 中的一個關鍵方法，用于查找指定半徑內的所有鄰近點。其核心功能是：以點 [x[i], y[i]] 為中心，在 radius 半徑范圍內，查找KD樹中所有滿足條件的鄰近點.返回值返回類型：list of int內容：包含所有鄰域點在原始點云數組中的索引值示例：[3, 5, 8, 12] 表示原始點云中索引為3,5,8,12的點在當前點的鄰域內"""# 獲取鄰居點x_neigh = x[idx]y_neigh = y[idx]z_neigh = z[idx]# 如果鄰域內沒有足夠的點，則跳過if len(x_neigh) < 3:continue# 計算鄰居點到當前點的距離distances = np.sqrt((x_neigh - x[i]) ** 2 + (y_neigh - y[i]) ** 2)# 計算權重weights = utils.choose_weight(weights_name, distances, tau)"""weights = np.exp(-0.5 * (distance / bandwidth) ** 2)高斯核函數輸出一個介于 0 到 1 的值，表示兩點之間的相似度（距離越近，值越接近1；距離越遠，值越接近0）。參數解釋distance：兩點之間的歐氏距離（或其他距離度量）。bandwidth：控制高斯曲線的寬度（平滑參數）。bandwidth越大：曲線越寬，遠距離的點仍有較高權重（更平滑）。bandwidth越小：曲線越窄，只有近距離的點才有顯著權重（更敏感）。計算步驟將距離歸一化：distance / bandwidth。平方后乘以 -0.5，使結果符合高斯分布的形式。通過 np.exp 計算指數，得到最終權重。"""# 二次曲面擬合: z = ax^2 + by^2 + cxy + dx + ey + f# 構建加權最小二乘問題A = np.vstack([x_neigh ** 2 * weights, y_neigh ** 2 * weights, x_neigh * y_neigh * weights, x_neigh * weights,y_neigh * weights, weights]).Tb = z_neigh * weights   #形狀：(n_neighbors,)"""設計矩陣AA = np.vstack([x_neigh ** 2 * weights,  # 二次項 x2y_neigh ** 2 * weights,  # 二次項 y2x_neigh * y_neigh * weights,  # 交叉項 xyx_neigh * weights,       # 線性項 xy_neigh * weights,       # 線性項 yweights                  # 常數項]).T形狀：(n_neighbors, 6)numpy.vstack(tup)功能：沿第一個軸（垂直方向）拼接多個數組輸入：元組 tup 包含要堆疊的數組序列輸出：堆疊后的新數組stacked = np.array([[x12w1, x22w2, ..., xm2wm],  # 行1[y12w1, y22w2, ..., ym2wm],  # 行2[x1y1w1, x2y2w2, ..., xmymwm],  # 行3[x1w1, x2w2, ..., xmwm],     # 行4[y1w1, y2w2, ..., ymwm],     # 行5[w1, w2, ..., wm]            # 行6])結果形狀：(6, m)m 是鄰近點數量轉置操作：A = stacked.T轉置后形狀：(m, 6)pythonA = [[x12w1, y12w1, x1y1w1, x1w1, y1w1, w1],  # 點1[x22w2, y22w2, x2y2w2, x2w2, y2w2, w2],  # 點2...,[xm2wm, ym2wm, xmymwm, xmwm, ymwm, wm]  # 點m]"""# 解線性方程組得到擬合參數fit_params = np.linalg.lstsq(A, b, rcond=None)[0]     #返回六個參數"""A：加權設計矩陣 (m_neighbors, 6)b：加權觀測 z 值 (m_neighbors,)返回：二次曲面參數 [a, b, c, d, e, f]用最小二乘法求解線性方程組 A?x≈b 的最優解，即找到參數 x 使誤差平方和最小。np.linalg.lstsq	NumPy 提供的最小二乘解函數（least squares）。A	輸入的設計矩陣（形狀為 (m, n)），每行是一個樣本，每列是一個特征。b	目標值向量（形狀為 (m,) 或 (m, k)，對應回歸問題中的觀測值）。rcond=None	禁用秩的截斷閾值（默認行為），避免舊版本 NumPy 的警告。[0]	lstsq 返回多個值（解、殘差、秩、奇異值），這里只取最優參數 x數學意義最小化以下目標函數：見b本如果 A 是滿秩的（即列線性無關），解是唯一的。如果 A 秩虧（列相關），返回最小范數解（Moore-Penrose 偽逆）"""# 計算當前點的平滑值smoothed_z = fit_params[0] * x[i] ** 2 + fit_params[1] * y[i] ** 2 + fit_params[2] * x[i] * y[i] + \fit_params[3] * x[i] + fit_params[4] * y[i] + fit_params[5]smoothed_points[i] = [x[i], y[i], smoothed_z]"""這段代碼使用擬合的二次曲面參數，計算當前點的平滑高度值：$(x[i], y[i])$：當前點的原始坐標{fit_params} = [a, b, c, d, e, f]$：擬合參數參數含義參數索引	符號	幾何意義	對z值的影響0	a	x2系數	控制x方向的曲率1	b	y2系數	控制y方向的曲率2	c	xy系數	控制扭曲/扭轉3	d	x系數	控制x方向的坡度4	e	y系數	控制y方向的坡度5	f	常數項	基準高度偏移幾何解釋曲率修正：$ax^2 + by^2$ 補償局部曲率扭曲校正：$cxy$ 消除非正交畸變坡度調整：$dx + ey$ 修正傾斜基準校準：$f$ 確保高度連續性平滑效果分析項	平滑作用	頻率響應x2/y2	抑制高頻噪聲	低通濾波xy	消除斜向噪聲	方向濾波x/y	補償線性趨勢	帶通濾波常數項	保持基準高度	直流保持""""""這段代碼是在 用二次多項式曲面（二次曲面）來平滑（擬合）一組三維點，并把平滑后的 z 值存回 smoothed_points[i]。fit_params[0] 到 fit_params[5] 是 二次曲面參數,這些參數是之前通過最小二乘法（如 np.linalg.lstsq）擬合得到的把原始 x[i]、y[i] 和平滑后的 z 組成一個新點，存回 smoothed_points。"""result =  smoothed_pointspcd_mls = o3d.geometry.PointCloud()pcd_mls.points = o3d.utility.Vector3dVector(result)return pcd_mls

2.2代碼2

參考：python——移動最小二乘擬合曲線/曲面_python 移動最小二乘法曲線擬合-CSDN博客

#2025.8.3
#點云俠  MLSimport numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.spatial import KDTreedef cubic_spline_3d(r):"""三維權函數（緊支域[0,1]）"""r = np.clip(r, 0, 1)return np.where(r <= 0.5,2 / 3 - 4 * r ** 2 + 4 * r ** 3,4 / 3 - 4 * r + 4 * r ** 2 - 4 / 3 * r ** 3)def mls_surface_fit(points, grid_x, grid_y, s_max=0.5, degree=2):"""MLS曲面擬合核心函數:param points: 點云數據 (n,3) [x,y,z]:param grid_x, grid_y: 擬合網格坐標:param s_max: 支持域半徑:param degree: 多項式階數:return: 擬合曲面Z值"""# 構建KD樹加速鄰域搜索tree = KDTree(points[:, :2])# 初始化擬合曲面Z_fit = np.zeros_like(grid_x)grid_points = np.column_stack([grid_x.ravel(), grid_y.ravel()])# 確定基函數 (二次曲面: 6個基函數)basis_funcs = [lambda x, y: 1,lambda x, y: x,lambda x, y: y,lambda x, y: x * y,lambda x, y: x ** 2,lambda x, y: y ** 2]if degree == 1:  # 線性曲面basis_funcs = basis_funcs[:3]for idx, (x, y) in enumerate(grid_points):# 搜索鄰域點dists, idxs = tree.query([x, y], k=50, distance_upper_bound=s_max)"""用于查找最近的k個點，同時設置最大搜索距離限制。核心功能：以點 [x, y] 為中心，查找最近的50個點，但不超過 s_max 距離.dists	array	鄰居點到中心點的距離（長度≤k）idxs	array	鄰居點在原始數組中的索引（長度≤k）"""## 創建有效點掩碼（過濾無窮遠點）valid = dists < float('inf')# 提取有效鄰域點local_points = points[idxs[valid]]if len(local_points) < len(basis_funcs) + 1:  #至少大于四個， 曲面至少大于3個點， 曲線至少大于2個點continue  # 鄰域點不足# 計算權重r = dists[valid] / s_maxweights = cubic_spline_3d(r)# 構造設計矩陣A和觀測向量bA = np.zeros((len(local_points), len(basis_funcs)))  #矩陣大小 mx6b = local_points[:, 2]for i, func in enumerate(basis_funcs):A[:, i] = [func(pt[0] - x, pt[1] - y) for pt in local_points]#A[:i]"""A[:, i] 是 NumPy 數組的切片語法，用于取出矩陣（二維數組）A 的第 i 列符號	含義A	一個二維 NumPy 數組（矩陣），形狀為 (行數, 列數)。:	表示“所有行”。i	表示“第 i 列”。""""""這段代碼用于構建標準移動最小二乘（MLS）的設計矩陣和觀測向量，核心是：初始化設計矩陣 A準備觀測向量 b用局部坐標系中的基函數填充 AA = np.zeros((len(local_points), len(basis_funcs)))  # 矩陣大小 m x n    功能：初始化設計矩陣  維度：m = len(local_points)：鄰域點數n = len(basis_funcs)：基函數個數示例：二次曲面：6個基函數 → 6列線性平面：3個基函數 → 3列b = local_points[:, 2]功能：提取觀測值向量內容：所有鄰域點的 z 坐標形狀：(m,) 向量for i, func in enumerate(basis_funcs):  #（x,y)當前點， pt近鄰點A[:, i] = [func(pt[0] - x, pt[1] - y) for pt in local_points]功能：填充設計矩陣的每一列關鍵操作：遍歷每個基函數 func對每個鄰域點：計算局部坐標：Δx = pt[0]-x, Δy = pt[1]-y計算基函數值：func(Δx, Δy)將結果存入當前列"""# 加權最小二乘求解W = np.diag(weights)  ## 將權重向量轉換為對角矩陣AWA = A.T @ W @ A"""矩陣	維度	說明A	(n, p)	    n 個觀測，p 個特征W	(n, n)	    對角權重矩陣，對角線元素為 w?, w?, …, w?A?	(p, n)	    A 的轉置A? @ W @ A	(p, p)	最終結果""""""維度變化：$\mathbf{A}^T$: n×m$\mathbf{W}$: m×m$\mathbf{A}$: m×n結果: n×n 矩陣物理意義：加權信息矩陣（Fisher信息矩陣）"""AWb = A.T @ W @ b"""維度變化：$\mathbf{A}^T$: n×m$\mathbf{W}$: m×m$\mathbf{b}$: m×1結果: n×1 向量物理意義：加權觀測向量"""try:theta = np.linalg.solve(AWA, AWb)except np.linalg.LinAlgError:theta = np.linalg.lstsq(AWA, AWb, rcond=None)[0]"""方法	              原理	         適用場景	        優點     	缺點np.linalg.solve	直接求解（LU分解）	矩陣滿秩且條件數好	速度快、精度高	對病態矩陣不穩定np.linalg.lstsq	最小二乘（SVD分解）	秩虧或病態矩陣	數值穩定、提供最小范數解	計算量大當正規方程矩陣病態時：解空間為超平面而非單點lstsq 返回最短的解向量對應最平滑的曲面"""# 計算當前點擬合高度z_fit = sum(theta[i] * func(0, 0) for i, func in enumerate(basis_funcs))"""theta[i]：第 i 個基函數的擬合系數func(0, 0)：在局部原點 (0,0) 計算基函數值sum：將所有基函數的貢獻相加，  ：由于（x,y)=(0,0),因此 6個系數---僅常數項不為0"""Z_fit.flat[idx] = z_fit"""Z_fit：網格高度矩陣（二維數組）.flat[idx]：通過展平索引訪問元素idx：當前網格點在展平數組中的索引Z_fit：二維網格矩陣（形狀同 grid_x/grid_y）.flat：展平視圖（一維索引訪問）idx：當前網格點索引等效操作：Z_fit[row, col] = z_fit"""return Z_fit
#數學證明
# 定理：局部坐標系下 MLS 擬合曲面在原點處高度等于常數項系數"""
方法1：使用全局坐標系， 6個系數全部使用
方法2：點云俠： 使用局部坐標系，  6個系數僅使用常數項局部坐標系的優勢
簡化計算：只需常數項系數
幾何直觀：$\theta_0$ 直接表示中心點高度
數值穩定：避免高階項貢獻
物理一致：與曲面定義原點一致"""#
# # 示例：擬合鞍形曲面
# np.random.seed(42)
#
# # 生成帶噪聲的鞍形曲面數據
# x = np.random.uniform(-2, 2, 500)
# y = np.random.uniform(-2, 2, 500)
# z = x ** 2 - y ** 2 + np.random.normal(0, 0.1, 500)
# points = np.column_stack([x, y, z])
#
# # 創建擬合網格
# grid_x, grid_y = np.meshgrid(np.linspace(-2, 2, 50),
#                              np.linspace(-2, 2, 50))
#
# # MLS曲面擬合
# Z_fit = mls_surface_fit(points, grid_x, grid_y, s_max=1.0, degree=2)
#
# # 可視化
# # 設置中文顯示
# plt.rcParams['font.sans-serif'] = ['LiSu']  # 顯示中文
# plt.rcParams['axes.unicode_minus'] = False  # 正常顯示負號
# fig = plt.figure(figsize=(12, 6))
# ax1 = fig.add_subplot(121, projection='3d')
# ax1.scatter(x, y, z, c=z, cmap='viridis', s=5)
# ax1.set_title('原始點云數據')
#
# ax2 = fig.add_subplot(122, projection='3d')
# ax2.plot_surface(grid_x, grid_y, Z_fit, cmap='plasma', alpha=0.8)
# ax2.set_title('MLS曲面擬合結果')
# plt.tight_layout()
# plt.show()