update gaussian surface

pmd.py

@@ -156,13 +156,13 @@ def main(config_path, img_folder):
         # 使用掩码过滤出非边界点
         non_boundary_points = z_raw[mask]
         z_raw_aligned = non_boundary_points @ rotation_matrix.T
-        z_raw_aligned[:,2] = z_raw_aligned[:,2] - np.mean(z_raw_aligned[:, 2])
+        #z_raw_aligned[:,2] = z_raw_aligned[:,2] - np.mean(z_raw_aligned[:, 2])
 
         #non_boundary_points = smoothed
-        write_point_cloud(os.path.join(img_folder, str(i) + '_cloudpoint.txt'), z_raw_aligned[:, 0], z_raw_aligned[:, 1],  1000*z_raw_aligned[:, 2])
+        write_point_cloud(os.path.join(img_folder, str(i) + '_cloudpoint.txt'), np.round(z_raw_aligned[:, 0]), np.round(z_raw_aligned[:, 1]),  1000*z_raw_aligned[:, 2])
         total_cloud_point = np.vstack([total_cloud_point, np.column_stack((z_raw_aligned[:, 0], z_raw_aligned[:, 1], 1000*z_raw_aligned[:, 2]))])
     
-    if 0:
+    if debug:
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
 
@@ -197,7 +197,7 @@ def main(config_path, img_folder):
         # ax.set_ylabel('Y (mm)')
         # ax.set_zlabel('Z (mm)')
         # ax.set_title('smoothed 3D Point Cloud Visualization')
-        plt.show()
+       
 
     get_point_end = time.time()
     print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
@@ -205,8 +205,31 @@ def main(config_path, img_folder):
     
     print("\n5. 后处理")
     post_process_start = time.time()
-    z_filtered = post_process(total_cloud_point, debug=0)
-    write_point_cloud(os.path.join(img_folder, 'cloudpoint.txt'), np.round(total_cloud_point[:, 0]-np.mean(total_cloud_point[:, 0])), np.round(total_cloud_point[:, 1]-np.mean(total_cloud_point[:, 1])), z_filtered)
+    total_cloud_point[:,0] = np.round(total_cloud_point[:,0])
+    total_cloud_point[:,1] = np.round(total_cloud_point[:,1])
+    fitted_points = post_process(total_cloud_point, debug=0)
+    align_fitted, _= align2ref(fitted_points)
+    
+    write_point_cloud(os.path.join(img_folder, 'cloudpoint.txt'), np.round(align_fitted[:, 0]-np.mean(align_fitted[:, 0])), np.round(align_fitted[:, 1]-np.mean(align_fitted[:, 1])), align_fitted[:,2]-np.min(align_fitted[:,2]))
+    if debug:
+        fig = plt.figure()
+        ax = fig.add_subplot(111, projection='3d')
+
+        # 提取 x, y, z 坐标
+        x_vals = np.round(fitted_points[:, 0]-np.mean(fitted_points[:, 0]))
+        y_vals = np.round(fitted_points[:, 1]-np.mean(fitted_points[:, 1]))
+        z_vals = align_fitted[:,2]-np.min(align_fitted[:,2])
+
+        # 绘制3D点云
+        ax.scatter(x_vals, y_vals, z_vals, c=z_vals, cmap='viridis', marker='o')
+
+        # 设置轴标签和标题
+        ax.set_xlabel('X (mm)')
+        ax.set_ylabel('Y (mm)')
+        ax.set_zlabel('Z (mm)')
+        ax.set_title('3D Point Cloud Visualization')
+        plt.show()
+    
     post_process_end = time.time()
     print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
     print(f"   耗时: {post_process_end - post_process_start:.2f} 秒")
@@ -228,7 +251,10 @@ def main(config_path, img_folder):
 if __name__ == '__main__':
     config_path = 'config\\cfg_3freq_wafer.json'
     img_folder = 'D:\\data\\four_cam\\1008_storage\\20241009163255262'
+    json_path = os.path.join(img_folder, 'result.json')
 
     pmdstart(config_path, img_folder)
     point_cloud_path = os.path.join(img_folder, 'cloudpoint.txt')
+    theta_notch = 0
+    #get_eval_result(point_cloud_path, json_path, theta_notch, 0)
     

src/eval.py

@@ -262,39 +262,58 @@ def get_eval_result(pointcloud_path, json_path, theta_notch, debug):
     tree = cKDTree(points)
 
     # 定义要查询的中心点
-    x2 = np.mean(x)
-    y2 = np.mean(y)
-
-    ## 查找水平方向上 (y 接近 y2) 的最近点
-    horizontal_idx = np.where(np.abs(y - y2) < 1)[0]  # 使用一个小容差判断 y 是否接近 y2
-    if len(horizontal_idx) > 0:
-        horizontal_z = z[horizontal_idx]
-        horizontal_x = x[horizontal_idx]
-        #print(f"水平方向最近的点 (x, z): {list(zip(horizontal_x, horizontal_z))}")
-    else:
-        print("没有找到接近 y2 的水平方向点")
-
-    # 查找垂直方向上 (x 接近 x2) 的最近点
-    vertical_idx = np.where(np.abs(x - x2) < 1)[0]  # 使用一个小容差判断 x 是否接近 x2
-    if len(vertical_idx) > 0:
-        vertical_z = z[vertical_idx]
-        vertical_y = y[vertical_idx]
-        #print(f"垂直方向最近的点 (y, z): {list(zip(vertical_y, vertical_z))}")
-    else:
-        print("没有找到接近 x2 的垂直方向点")
-
-        # 检查水平和垂直方向是否找到点
-        if len(horizontal_x) > 0:
-            right_x = max(horizontal_x)
-        else:
-            print("找不到水平方向的点")
-            right_x = None
-
-        if len(vertical_y) > 0:
-            top_y = max(vertical_y)
-        else:
-            print("找不到垂直方向的点")
-            top_y = None
+    x2 = round(np.mean(x))
+    y2 = round(np.mean(y))
+
+    horizontal_indices = np.where(y == y2)[0]  # 找到 y == y_center 的索引
+    horizontal_x = x[horizontal_indices]  # 提取 x 值
+    horizontal_z = z[horizontal_indices]  # 提取对应的 z 值
+
+    sorted_horizontal_indices = np.argsort(horizontal_x)  # 对 x 排序，返回索引
+    #print('sorted_horizontal_indices = ', sorted_horizontal_indices)
+    horizontal_x_sorted = horizontal_x[sorted_horizontal_indices]
+    horizontal_z_sorted = horizontal_z[sorted_horizontal_indices]
+
+    # 提取经过中心点的垂直方向 (x = x_center) 的所有点
+    vertical_indices = np.where(x == x2)[0]  # 找到 x == x2 的索引
+    vertical_y = y[vertical_indices]  # 提取 y 值
+    vertical_z = z[vertical_indices]  # 提取对应的 z 值
+
+    # 按照 y 从小到大排序
+    sorted_vertical_indices = np.argsort(vertical_y)  # 对 y 排序，返回索引
+    vertical_y_sorted = vertical_y[sorted_vertical_indices]
+    vertical_z_sorted = vertical_z[sorted_vertical_indices]
+
+    # ## 查找水平方向上 (y 接近 y2) 的最近点
+    # horizontal_idx = np.where(np.abs(y - y2) == 0)[0]  # 使用一个小容差判断 y 是否接近 y2
+    # if len(horizontal_idx) > 0:
+    #     horizontal_z = z[horizontal_idx]
+    #     horizontal_x = x[horizontal_idx]
+    #     #print(f"水平方向最近的点 (x, z): {list(zip(horizontal_x, horizontal_z))}")
+    # else:
+    #     print("没有找到接近 y2 的水平方向点")
+
+    # # 查找垂直方向上 (x 接近 x2) 的最近点
+    # vertical_idx = np.where(np.abs(x - x2) == 0)[0]  # 使用一个小容差判断 x 是否接近 x2
+    # if len(vertical_idx) > 0:
+    #     vertical_z = z[vertical_idx]
+    #     vertical_y = y[vertical_idx]
+    #     #print(f"垂直方向最近的点 (y, z): {list(zip(vertical_y, vertical_z))}")
+    # else:
+    #     print("没有找到接近 x2 的垂直方向点")
+
+    #     # 检查水平和垂直方向是否找到点
+    #     if len(horizontal_x) > 0:
+    #         right_x = max(horizontal_x)
+    #     else:
+    #         print("找不到水平方向的点")
+    #         right_x = None
+
+    #     if len(vertical_y) > 0:
+    #         top_y = max(vertical_y)
+    #     else:
+    #         print("找不到垂直方向的点")
+    #         top_y = None
 
     top_x = x2
     right_y = y2
@@ -304,16 +323,36 @@ def get_eval_result(pointcloud_path, json_path, theta_notch, debug):
     #print(top_x, top_y)
     line_warpage_x, line_bow_x, z_values = get_mean_line_value(x, y, z, right_x, right_y)
     line_warpage_y, line_bow_y, z_values = get_mean_line_value(x, y, z, top_x, top_y)
+    if debug:
+        # 创建一个图
+        plt.figure()
+
+        # 绘制第一个列表
+        plt.plot(horizontal_z_sorted.tolist(), label='List 1', color='blue', marker='o')
+
+        # 绘制第二个列表
+        plt.plot(vertical_z_sorted.tolist(), label='List 2', color='red', marker='s')
+
+        # 添加标题和标签
+        plt.title('Plot of List 1 and List 2')
+        plt.xlabel('Index')
+        plt.ylabel('Values')
+
+        # 添加图例
+        plt.legend()
+
+        # 显示图形
+        plt.show()
 
     with open(json_path, 'w') as file:
         result_data = {
                 'x':{
-                    'height': horizontal_z.tolist(),
+                    'height': horizontal_z_sorted.tolist(),
                     'bow':line_bow_x,
                     'warpage':line_warpage_x
                 },
                 'y':{
-                    'height':vertical_z.tolist(),
+                    'height':vertical_z_sorted.tolist(),
                     'bow':line_bow_y,
                     'warpage':line_warpage_y
                 },

src/utils.py

@@ -17,6 +17,10 @@ import shutil
 import pandas as pd
 from sklearn.linear_model import LinearRegression
 from scipy.interpolate import Rbf
+from numpy.linalg import lstsq
+from scipy.optimize import curve_fit
+
+
 
 
 
@@ -772,16 +776,171 @@ def point_filter(data):
     return smoothed_z
 
 
-def post_process(points, debug):
+
+# 定义高斯曲面方程
+def gaussian_surface(X, A, x0, y0, sigma):
+    x, y = X
+    return A * np.exp(-((x - x0)**2 + (y - y0)**2) / (2 * sigma**2))
+
+
+def curve_fit_gaussian(points):
+    x = points[:, 0]
+    y = points[:, 1]
+    z = points[:, 2]
+
+    # 使用curve_fit来拟合高斯曲面
+    # 初始猜测参数 A, x0, y0, sigma
+    initial_guess = [1, np.mean(x), np.mean(y), 1]
+    
+    # 使用 curve_fit 进行非线性最小二乘拟合
+    popt, pcov = curve_fit(gaussian_surface, (x, y), z, p0=initial_guess)
+    
+    # 拟合的参数
+    A, x0, y0, sigma = popt
+    print(f"拟合参数: A={A}, x0={x0}, y0={y0}, sigma={sigma}")
+    
+    # 根据拟合的高斯曲面计算新的 z 值
+    z_fitted = gaussian_surface((x, y), A, x0, y0, sigma)
+    
+    # 生成新的点云
+    new_points = np.column_stack((x, y, z_fitted))
+
+    # 可视化原始点云和拟合的高斯曲面
+    # fig = plt.figure()
+    # ax = fig.add_subplot(111, projection='3d')
+
+    # # 绘制原始点云
+    # #ax.scatter(x, y, z, c='r', label='Original Points')
+
+    # # 绘制拟合的高斯曲面点云
+    # ax.scatter(x, y, z_fitted, c='b', label='Fitted Gaussian Surface')
+
+    # ax.set_title('3D Point Cloud and Fitted Gaussian Surface')
+    # ax.set_xlabel('X')
+    # ax.set_ylabel('Y')
+    # ax.set_zlabel('Z')
+    # ax.legend()
+
+    # plt.show()
+    
+    return new_points
+
+
+def curve_fit_2(points):
     x = points[:,0]
     y = points[:,1]
     z = points[:,2]
+    A = np.column_stack((x**2, y**2, x*y, x, y, np.ones_like(x)))
+
+    # 使用最小二乘法拟合二次曲面的系数
+    coefficients, _, _, _ = lstsq(A, z, rcond=None)
+
+    # 拟合的系数
+    a, b, c, d, e, f = coefficients
+    print("拟合的系数:", coefficients)
+
+    # 根据拟合的曲面方程计算新的 z 值
+    z_fitted = a * x**2 + b * y**2 + c * x * y + d * x + e * y + f
+
+    # 生成新的点云
+    new_points = np.column_stack((x, y, z_fitted))
+
+
+    # # 可视化原始点云和拟合的曲面
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+
+    # 绘制原始点云
+    #ax.scatter(x, y, z, c='r', label='Original Points')
+
+    # 绘制拟合的曲面点云
+    ax.scatter(x, y, z_fitted, c='b', label='Fitted Surface')
+
+    ax.set_title('3D Point Cloud and Fitted Quadratic Surface')
+    ax.set_xlabel('X')
+    ax.set_ylabel('Y')
+    ax.set_zlabel('Z')
+    ax.legend()
+
+    plt.show()
+    return new_points
+
+def curve_fit_3(points):
+    x = points[:,0]
+    y = points[:,1]
+    z = points[:,2]
+    # 构建三次曲面的设计矩阵
+    # 曲面模型：z = ax^3 + by^3 + cx^2y + dxy^2 + ex^2 + fy^2 + gxy + hx + iy + j
+    A = np.column_stack((x**3, y**3, x**2*y, x*y**2, x**2, y**2, x*y, x, y, np.ones_like(x)))
+
+    # 使用最小二乘法拟合三次曲面的系数
+    coefficients, _, _, _ = lstsq(A, z, rcond=None)
+
+    # 拟合的系数
+    a, b, c, d, e, f, g, h, i, j = coefficients
+    #print("拟合的系数:", coefficients)
+
+    # 根据拟合的曲面方程计算新的 z 值
+    z_fitted = a * x**3 + b * y**3 + c * x**2 * y + d * x * y**2 + e * x**2 + f * y**2 + g * x * y + h * x + i * y + j
+    # 生成新的点云
+    new_points = np.column_stack((x, y, z_fitted))
+
+
+    # # # 可视化原始点云和拟合的曲面
+    # fig = plt.figure()
+    # ax = fig.add_subplot(111, projection='3d')
+
+    # # 绘制原始点云
+    # #ax.scatter(x, y, z, c='r', label='Original Points')
+
+    # # 绘制拟合的曲面点云
+    # ax.scatter(x, y, z_fitted, c='b', label='Fitted Surface')
+
+    # ax.set_title('3D Point Cloud and Fitted Quadratic Surface')
+    # ax.set_xlabel('X')
+    # ax.set_ylabel('Y')
+    # ax.set_zlabel('Z')
+    # ax.legend()
+
+    # plt.show()
+    return new_points
+
+def remove_duplicates(points):
+    # 使用字典来去重，键为 (x, y)，值为 z
+    unique_points = {}
+    
+    for point in points:
+        x, y, z = point
+        
+        if (x, y) not in unique_points:
+            unique_points[(x, y)] = z  # 如果 x, y 坐标未出现过，保留该点
+    
+    # 将去重后的点云转换为 numpy 数组
+    new_points = np.array([[x, y, z] for (x, y), z in unique_points.items()])
+    
+    return new_points
+
+def post_process(points, debug):
+    uniq_points = remove_duplicates(points)
+    
+    x = uniq_points[:,0]
+    y = uniq_points[:,1]
+    z = uniq_points[:,2]
 
     close_point = remove_duplicate_points(points)
+
+    smoothed_point = moving_average_filter_z(close_point, 20)
+
+    fitted_points = curve_fit_gaussian(smoothed_point)
+   
+    return fitted_points
+
+    
     plane_points = fit_plane_and_adjust(close_point, 10)
-    smoothed_z = moving_average_filter_z(plane_points[:,2], 20)
+    
     z_outliers_removed = remove_outliers(smoothed_z, threshold=30.0)
     z_final = smooth_z_with_std(z_outliers_removed, 5)
+
     
     if debug:
         # 绘制3D点云
@@ -1334,7 +1493,7 @@ def cubic_interpolation(points, z_values, grid_size=(100, 100)):
     return grid_x, grid_y, grid_z
 
 
-def moving_average_filter_z(z_values, window_size=3):
+def moving_average_filter_z(points, window_size=3):
     """
     使用移动平均法对 z 值进行平滑，减少异常值。
     
@@ -1345,6 +1504,7 @@ def moving_average_filter_z(z_values, window_size=3):
     返回:
     smoothed_z: 平滑后的 z 值
     """
+    z_values = points[:,2]
     smoothed_z = np.copy(z_values)
     for i in range(len(z_values)):
         # 找到窗口内的邻居
@@ -1352,7 +1512,7 @@ def moving_average_filter_z(z_values, window_size=3):
         end_idx = min(len(z_values), i + window_size // 2 + 1)
         smoothed_z[i] = np.mean(z_values[start_idx:end_idx])
     
-    return smoothed_z
+    return np.column_stack((points[:,0], points[:,1], smoothed_z))