1024 for show

pmd.py

@@ -4,6 +4,7 @@ import os
 import sys
 import pandas as pd
 
+
 sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 import time
 import glob
@@ -11,9 +12,9 @@ import numpy as np
 np.random.seed(42)
 from datetime import datetime
 import json
-from src.utils import get_meshgrid, get_world_points, get_camera_points, get_screen_points, write_point_cloud, get_white_mask, get_meshgrid_contour, post_process, find_notch
+from src.utils import get_meshgrid, get_world_points, get_camera_points, get_screen_points, write_point_cloud, get_white_mask, get_meshgrid_contour, post_process, find_notch, post_process_with_grad
 from src.phase import extract_phase, unwrap_phase
-from src.recons import reconstruction_cumsum
+from src.recons import reconstruction_cumsum, poisson_recons_with_smoothed_gradients
 from src.pcl_postproc import smooth_pcl, align2ref
 import matplotlib.pyplot as plt
 from src.calibration import calibrate_world, calibrate_screen, map_screen_to_world
@@ -50,16 +51,17 @@ def main(config_path, img_folder):
     smooth = True
     align = True
     denoise = True
-    cammera_img_path = 'D:\\data\\four_cam\\calibrate\\calibrate-1008'
+    #cammera_img_path = 'D:\\data\\four_cam\\calibrate\\calibrate-1008'
     screen_img_path = 'D:\\data\\four_cam\\calibrate\\cam3-screen-1008'
-    #cammera_img_path = 'D:\\data\\four_cam\\calibrate\\calibration_0913'
+    cammera_img_path = 'D:\\data\\four_cam\\calibrate\\calibrate-1016'
     #screen_img_path = 'D:\\data\\four_cam\\calibrate\\screen0920'
 
     print(f"开始执行时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
     print("\n1. 相机标定")
     preprocess_start = time.time()
 
-    cam_para_path = os.path.join('config', cfg['cam_params'])
+    #cam_para_path = os.path.join('config', cfg['cam_params'])
+    cam_para_path = 'D:\\code\\pmd-python\\config\\cam_params.pkl'
     if os.path.exists(cam_para_path):
     #if False:
         with open(cam_para_path, 'rb') as pkl_file:
@@ -72,12 +74,12 @@ def main(config_path, img_folder):
         for i in range(n_cam):
             cam_img_path = glob.glob(os.path.join(cammera_img_path, camera_subdir[i], "*.bmp"))
             cam_img_path.sort()
-            print('cam_img_path = ', cam_img_path)
+            #print('cam_img_path = ', cam_img_path)
             cam_param_raw = calibrate_world(cam_img_path, i, cfg['world_chessboard_size'], cfg['world_square_size'], debug=0)
             cam_params.append(cam_param_raw)
         with open(cam_para_path, 'wb') as pkl_file:
            pickle.dump(cam_params, pkl_file) 
-    
+
     print("\n2. 屏幕标定")
     screen_cal_start = time.time()
 
@@ -118,32 +120,33 @@ def main(config_path, img_folder):
     print("\n4. 获得不同坐标系下点的位置")
     get_point_start = time.time()
     total_cloud_point = np.empty((0, 3))
+    total_gradient = np.empty((0, 4))
     total_boundary_point = np.empty((0, 3))
     for i in range(n_cam):
         print('cam_id = ', i)
         contours_point = get_meshgrid_contour(binary_masks[i], save_path, debug=False)
-        world_points, world_points_boundary, world_points_boundary_3 = get_world_points(contours_point, cam_params[i], i, grid_spacing, cfg['d'], erosion_pixels=15, debug=0)
+        world_points, world_points_boundary, world_points_boundary_3 = get_world_points(contours_point, cam_params[i], i, grid_spacing, cfg['d'], erosion_pixels=2, debug=0)
         camera_points, u_p, v_p = get_camera_points(world_points, cam_params[i], save_path, i, debug=0)
         point_data = {'x_w': world_points[:, 0], 'y_w': world_points[:, 1], 'z_w': world_points[:, 2], 
                     'x_c': camera_points[:, 0], 'y_c': camera_points[:, 1], 'z_c': camera_points[:, 2],  
                     'u_p': u_p, 'v_p': v_p}
         screen_points = get_screen_points(point_data, x_uns[i], y_uns[i], screen_params, screen_to_world, cfg, save_path, i, debug=debug)
         #plot_coords(world_points, camera_points, screen_points)
-        z_raw, aligned, smoothed, denoised = reconstruction_cumsum(world_points, camera_points, screen_points, save_path, i, debug=0, smooth=smooth, align=align, denoise=denoise)
+        z_raw, aligned, smoothed, denoised, gradient_xy = reconstruction_cumsum(world_points, camera_points, screen_points, save_path, i, debug=0, smooth=smooth, align=align, denoise=denoise)
         
         z_raw_xy = np.round(z_raw[:, :2]).astype(int)
         
-        # 创建布尔掩码，初始为 True
-        mask = np.ones(len(z_raw_xy), dtype=bool)
+        # # 创建布尔掩码，初始为 True
+        # mask = np.ones(len(z_raw_xy), dtype=bool)
 
-        # 遍历每个边界点，标记它们在 aligned 中的位置
-        for boundary_point in world_points_boundary:
-            # 标记与当前边界点相同 xy 坐标的行
-            mask &= ~np.all(z_raw_xy == boundary_point[:2], axis=1)
+        # # 遍历每个边界点，标记它们在 aligned 中的位置
+        # for boundary_point in world_points_boundary:
+        #     # 标记与当前边界点相同 xy 坐标的行
+        #     mask &= ~np.all(z_raw_xy == boundary_point[:2], axis=1)
         
-        # 使用掩码过滤出非边界点
-        non_boundary_points = z_raw[mask]
-        non_boundary_aligned, rotation_matrix = align2ref(non_boundary_points)
+        # # 使用掩码过滤出非边界点
+        # non_boundary_points = z_raw[mask]
+        # non_boundary_aligned, rotation_matrix = align2ref(non_boundary_points)
         
         # 创建布尔掩码，初始为 True
         mask = np.ones(len(z_raw_xy), dtype=bool)
@@ -154,15 +157,19 @@ def main(config_path, img_folder):
             mask &= ~np.all(z_raw_xy == boundary_point[:2], axis=1)
         
         # 使用掩码过滤出非边界点
-        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])
+        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 = non_boundary_points
 
         #non_boundary_points = smoothed
         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])
+        np.savetxt(os.path.join(img_folder, str(i) + '_gradient.txt'), gradient_xy, fmt='%.10f', delimiter=',')
         total_cloud_point = np.vstack([total_cloud_point, np.column_stack((z_raw_aligned[:, 0], z_raw_aligned[:, 1], 1000*z_raw_aligned[:, 2]))])
+        total_gradient = np.vstack([total_gradient, np.column_stack((gradient_xy[:, 0], gradient_xy[:, 1], gradient_xy[:, 2], gradient_xy[:, 3]))])
     
-    if debug:
+    if 0:
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
 
@@ -178,7 +185,7 @@ def main(config_path, img_folder):
         ax.set_xlabel('X (mm)')
         ax.set_ylabel('Y (mm)')
         ax.set_zlabel('Z (mm)')
-        ax.set_title('3D Point Cloud Visualization')
+        ax.set_title('3D Point Cloud Visualization gradient')
         plt.show()
 
         # fig = plt.figure()
@@ -186,7 +193,7 @@ def main(config_path, img_folder):
         # smoothed_total = smooth_pcl(total_cloud_point, 3)
         #  # 提取 x, y, z 坐标
         # x_vals = smoothed_total[:, 0]
-        # y_vals = smoothed_total[:, 1]
+        # y_vals = smoothed_total[:, 1] 
         # z_vals = smoothed_total[:, 2]
 
         # # 绘制3D点云
@@ -207,11 +214,14 @@ def main(config_path, img_folder):
     post_process_start = time.time()
     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)
+    #fitted_points = post_process(total_cloud_point, debug=0)
+    fitted_points = post_process_with_grad(img_folder, 0)
+    #fitted_points = total_cloud_point
+    #align_fitted, _ = align2ref(fitted_points)
+    align_fitted = 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:
+    if 0:
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
 
@@ -248,13 +258,30 @@ 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'
+    #img_folder = 'D:\\data\\four_cam\\1008_storage\\pingjing_20241017092315228\\'   #'D:\\data\\four_cam\\betone_1011\\20241011142348762-1'
+   # img_folder = 'D:\\huchao\\inspect_server_202409241013_py\\storage\\20241023193453708\\'
+    img_folder = 'D:\\data\\four_cam\\betone_1011\\20241011144821292-4\\'
     json_path = os.path.join(img_folder, 'result.json')
 
+    # 20241011142348762-1  20241011142901251-2   20241011143925746-3   20241011144821292-4 
+    #     0.60                0.295                0.221                  0.346
+
+    # x max =  653.5925
+    # y max =  692.1735648
+    # x max =  251.0775
+    # y max =  293.05856482
+    # x max =  184.7275
+    # y max =  239.00919669
+    # x max =  342.2525
+    # y max =  386.92087185
+
     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)
+    #fitted_points = post_process_with_grad(img_folder, 1)
+
+    
+    
     

src/calibration/get_camera_params.py

@@ -7,7 +7,6 @@ from aprilgrid import Detector
 # 读取文件夹中的所有棋盘格图像
 # images = glob.glob('calibration/world_calibration/*.bmp')
 
-
 def show_detect_result(img, detections):
     colors = [(255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0)]
     for detection in detections:
@@ -26,6 +25,18 @@ def show_detect_result(img, detections):
     cv2.waitKey(0)
     cv2.destroyAllWindows()
 
+
+def validate_corners(corners, image_shape):
+    """确保角点在图像有效范围内"""
+    h, w = image_shape
+    for corner in corners:
+        x, y = corner.ravel()
+        if not (0 <= x < w and 0 <= y < h):
+            print(f"Invalid corner: ({x}, {y}) is out of image bounds.")
+            return False
+    return True
+
+
 # 生成 AprilGrid 标定板的 3D 物体坐标，并考虑标签之间的间隙
 def generate_3d_points(grid_size, tag_size, tag_spacing):
     """
@@ -81,10 +92,7 @@ def calibrate_world(calibration_images_path, cam_id, world_chessboard_size=(6,6)
         ax.set_zlabel('Z (mm)')
         ax.set_title('3D Visualization of AprilGrid Points')
         plt.show()
-    #print('object_point_single = ', object_point_single)
-    # print('cam_id = ', calibration_path + 'cam_id_' + str(cam_id) + '*.bmp')
-    # calibration_images_path = glob.glob(calibration_path + 'cam_id_' + str(cam_id) + '*.bmp')
-    # print(calibration_path + 'cam_id_' + str(cam_id)+'*.bmp')
+    
     # 用于保存所有图像的检测到的角点和 ID
     all_corners = []
     all_object_points = []

src/calibration/get_screen_params.py

@@ -24,8 +24,8 @@ def calibrate_screen(screen_img_path, cam_mtx, cam_dist, chessboard_size, square
         img = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
        
         # import pdb; pdb.set_trace()
-        #dst = cv2.undistort(img, cam_mtx, cam_dist, None, None)
-        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        dst = cv2.undistort(img, cam_mtx, cam_dist, None, None)
+        gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
         # alpha = 3  # 对比度保持不变
         # beta = 0    # 增加亮度
         # bright_image = cv2.convertScaleAbs(gray, alpha=alpha, beta=beta)

src/eval.py

@@ -207,7 +207,8 @@ def get_eval_result(pointcloud_path, json_path, theta_notch, debug):
     
     x = data[:, 0]
     y = data[:, 1]
-    z = data[:, 2]
+    z = 1000 * data[:, 2]
+    
     max_z = max(z)
     min_z = min(z)
     pv = max_z - min_z

src/recons.py

@@ -325,8 +325,8 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
 
     # 计算反射光线向量
     reflection_vector = np.array([world_points[:, 0] - screen_points[:, 0],
-                                world_points[:, 1] - screen_points[:, 1],
-                                world_points[:, 2] - screen_points[:, 2]])
+                                  world_points[:, 1] - screen_points[:, 1],
+                                  world_points[:, 2] - screen_points[:, 2]])
     
     reflection_vector = reflection_vector / np.linalg.norm(reflection_vector, axis=0)
 
@@ -335,13 +335,20 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
     normal_vector = normal_vector / np.linalg.norm(normal_vector, axis=0)
  
     normal_vector = np.nan_to_num(normal_vector, nan=0.0)
+
+    #print('normal_vector = ', normal_vector[0])
+
+
     # 计算法向量在X方向的梯度
     gradient_x = normal_vector[0]
 
     # 计算法向量在Y方向的梯度
     gradient_y = normal_vector[1]
-    # gradient_x = np.zeros(gradient_x.shape)
-    # gradient_y = np.zeros(gradient_x.shape)
+    #gradient_x = np.zeros(gradient_x.shape)
+    #gradient_y = np.zeros(gradient_x.shape)
+
+    #gradient_x = gradient_x - np.mean(gradient_x)
+    #gradient_y = gradient_y - np.mean(gradient_y)
     # import pdb
     # pdb.set_trace()
     # 计算z值，假设初始z值为0
@@ -350,6 +357,10 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
     
     #z_raw = poisson_recons(x, y, gradient_x, gradient_y)
     z_raw = poisson_recons_with_smoothed_gradients(x, y, gradient_x, gradient_y)
+
+    #print('gradient_x  = ', x.shape, y.shape, gradient_x.shape, gradient_y.shape)
+
+    gradient_xy = np.column_stack((x, y, gradient_x, gradient_y))
     
        # z_raw = least_squares_recons(x, y, gradient_x, gradient_y)
         # import pdb; pdb.set_trace()
@@ -365,15 +376,17 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
         #print('-- nanmean after alignment: mean - {}; std - {}'.format(np.nanmean(points_aligned[:, 2]), np.nanstd(points_aligned[:, 2])))
     if smooth:
         #print("-- point cloud is being smoothed.")
-        points_smoothed = smooth_pcl(points_aligned, 3)
+        points_smoothed = None
+        #points_smoothed = smooth_pcl(points_aligned, 3)
         # print('raw_shape = ', raw_data.shape)
         # print('smoothed_shape = ', points_smoothed.shape)
         # print('raw min max = ', np.min(raw_data[:,0]), np.max(raw_data[:,0]), np.min(raw_data[:,1]), np.max(raw_data[:,1]))
         # print('smoothed min max = ', np.min(points_smoothed[:,0]), np.max(points_smoothed[:,0]), np.min(points_smoothed[:,1]), np.max(points_smoothed[:,1]))
     if denoise:
-        z = points_aligned[:, 2]
-        a, b, c = fit_plane(x, y, z)
-        points_denoise = remove_noise(x, y, z, a, b, c, thresh=0.01)
+        points_denoise = None
+        # z = points_aligned[:, 2]
+        # a, b, c = fit_plane(x, y, z)
+        # points_denoise = remove_noise(x, y, z, a, b, c, thresh=0.01)
 
     if debug:
         #fig, axes = plt.subplots(1, 2, figsize=(12, 6))
@@ -438,4 +451,8 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
 
    
     
-    return np.column_stack((x, y, z_raw)), points_aligned, points_smoothed, points_denoise, 
+    return np.column_stack((x, y, z_raw)), points_aligned, points_smoothed, points_denoise, gradient_xy
+
+
+
+

src/utils.py

@@ -19,7 +19,12 @@ from sklearn.linear_model import LinearRegression
 from scipy.interpolate import Rbf
 from numpy.linalg import lstsq
 from scipy.optimize import curve_fit
-
+from aprilgrid import Detector
+from scipy.sparse.linalg import spsolve
+from scipy.spatial import Delaunay
+from scipy.sparse.linalg import lsqr
+from collections import defaultdict
+from scipy.sparse import lil_matrix
 
 
 
@@ -179,6 +184,7 @@ def fit_sector(contour_points, grid_spacing, erosion_pixels):
     kernel = np.ones((erosion_pixels, erosion_pixels), np.uint8)
     eroded_img = cv2.erode(img, kernel, iterations=1)
 
+
     # 提取腐蚀后的轮廓
     contours, _ = cv2.findContours(eroded_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
     if len(contours) == 0:
@@ -576,9 +582,7 @@ def get_white_mask(white_path, bin_thresh=15, debug=False):
         max_index, max_contour = max(enumerate(valid_contours), key=lambda x: cv2.boundingRect(x[1])[2] * cv2.boundingRect(x[1])[3]) 
         cv2.drawContours(mask, [max_contour], -1, (255, 255, 255), thickness=cv2.FILLED)
     if debug:
-        cv2.namedWindow('erode_thresh_img',0)
-        cv2.imshow('erode_thresh_img', erode_thresh_img)
-        cv2.waitKey(0)
+       
    
         cv2.namedWindow('mask',0)
         cv2.imshow('mask', mask)
@@ -788,23 +792,58 @@ def curve_fit_gaussian(points):
     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=max(z), x0=mean(x), y0=mean(y), sigma=较大初始值
+    initial_guess = [np.max(z), np.mean(x), np.mean(y), np.std(x) / 2]
+
+    # 进行非线性最小二乘拟合
+    popt, _ = 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)
+    #print(f"拟合参数: A={A}, x0={x0}, y0={y0}, sigma={sigma}")
+    x_mean = np.mean(x)
+    y_mean = np.mean(y)
+    x0 = x_mean
+    y0 = y_mean
+    sigma = 2 * sigma
+    #print(f"拟合参数1: A={A}, x0={x0}, y0={y0}, sigma={sigma}")
     
+
+    # 生成平滑的 z_fitted
+    z_fitted = gaussian_surface((x, y), A, x0, y0, sigma)
+
+    # 调整 z_fitted，使其最大值与原始 z 的最大值一致
+    z_fitted = z_fitted / np.max(z_fitted) * np.max(z)
+    #print('np.max(z_fitted) = ', np.max(z_fitted))
+
     # 生成新的点云
     new_points = np.column_stack((x, y, z_fitted))
 
+    return new_points
+
+
+
+
+    # # 使用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))
+
+    # return new_points
+
     # 可视化原始点云和拟合的高斯曲面
     # fig = plt.figure()
     # ax = fig.add_subplot(111, projection='3d')
@@ -822,8 +861,9 @@ def curve_fit_gaussian(points):
     # ax.legend()
 
     # plt.show()
+    #return new_points
+    
     
-    return new_points
 
 
 def curve_fit_2(points):
@@ -921,6 +961,7 @@ def remove_duplicates(points):
     return new_points
 
 def post_process(points, debug):
+
     uniq_points = remove_duplicates(points)
     
     x = uniq_points[:,0]
@@ -935,111 +976,331 @@ def post_process(points, debug):
    
     return fitted_points
 
+
+
+def get_neighbors(tri, index):
+    """Helper function to get the neighbors of a point in the Delaunay triangulation."""
+    neighbors = set()
+    for simplex in tri.simplices:
+        if index in simplex:
+            neighbors.update(simplex)
+    neighbors.remove(index)  # Remove the point itself from its neighbors
+    return list(neighbors)
+
+
+def expand_to_rectangle_with_unit_spacing(x, y, x_gradient, y_gradient):
+    # 1. 计算最小外接矩形的边界
+    x_min, x_max = np.min(x), np.max(x)
+    y_min, y_max = np.min(y), np.max(y)
+
+    # 2. 根据边界计算矩形的宽度和高度，间隔为1
+    width = int(np.ceil(x_max - x_min)) + 1  # 确保包含所有点，并加1保证整数
+    height = int(np.ceil(y_max - y_min)) + 1
+
+    # 3. 创建规则网格，间隔为1
+    x_grid = np.arange(x_min, x_max + 1, 1)  # x坐标从x_min到x_max，间隔为1
+    y_grid = np.arange(y_min, y_max + 1, 1)  # y坐标从y_min到y_max，间隔为1
+    x_expand, y_expand = np.meshgrid(x_grid, y_grid)
+
+    # 4. 初始化梯度矩阵，填充为 0
+    x_grad_expand = np.zeros_like(x_expand)
+    y_grad_expand = np.zeros_like(y_expand)
+
+    # 5. 找到每个点在网格中的位置，插入对应的梯度
+    for i in range(len(x)):
+        # 找到对应的网格索引
+        x_idx = int(x[i] - x_min)
+        y_idx = int(y[i] - y_min)
+        
+        # 将对应点的梯度填入矩阵中
+        x_grad_expand[y_idx, x_idx] = x_gradient[i]
+        y_grad_expand[y_idx, x_idx] = y_gradient[i]
+
+    return x_grad_expand, y_grad_expand, x_expand, y_expand
+
+
+
+def remove_duplicates_4d_with_std_neighbor(points, std_multiplier=2):
+    """
+    去重并处理z1和z2值，如果z1或z2与全局均值差异大于标准差的倍数，则替换为相邻点的值。
+    
+    Parameters:
+    points : np.ndarray
+        四维点集 (x, y, z1, z2)。
+    std_multiplier : float
+        用于判断异常值的标准差倍数，默认为2倍。
+    
+    Returns:
+    new_points : np.ndarray
+        处理后的去重点集。
+    """
     
-    plane_points = fit_plane_and_adjust(close_point, 10)
+    # 使用 defaultdict 来保存 (x, y) 键，并存储 z1 和 z2 的列表
+    unique_points = defaultdict(lambda: [[], []])  # 第一个列表存 z1，第二个列表存 z2
     
-    z_outliers_removed = remove_outliers(smoothed_z, threshold=30.0)
-    z_final = smooth_z_with_std(z_outliers_removed, 5)
+    # 遍历点，按 (x, y) 键存储 z1 和 z2 的值
+    for point in points:
+        x, y, z1, z2 = point
+        unique_points[(x, y)][0].append(z1)  # 将 z1 加入列表
+        unique_points[(x, y)][1].append(z2)  # 将 z2 加入列表
+    
+    # 计算全局 z1 和 z2 的均值和标准差
+    all_z1 = points[:, 2]
+    all_z2 = points[:, 3]
+    z1_mean_global = np.mean(all_z1)
+    z2_mean_global = np.mean(all_z2)
+    z1_std_global = np.std(all_z1)
+    z2_std_global = np.std(all_z2)
+
+    # 准备存储新点
+    new_points = []
+    
+    # 初始化相邻点值为全局均值，保证第一次有默认值
+    previous_z1 = z1_mean_global
+    previous_z2 = z2_mean_global
+    
+    # 对每个 (x, y) 进行处理
+    for (x, y), (z1_list, z2_list) in unique_points.items():
+        # 初始化最小距离和对应的 z1, z2
+        min_distance = float('inf')
+        best_z1 = None
+        best_z2 = None
+        
+        # 遍历该 (x, y) 的所有 z1 和 z2 值
+        for z1, z2 in zip(z1_list, z2_list):
+            # 检查 z1 和 z2 是否超出标准差的倍数，若超出则替换为相邻点的值
+            if abs(z1 - z1_mean_global) > std_multiplier * z1_std_global:
+                z1 = previous_z1  # 用相邻点的 z1 值替换
+            if abs(z2 - z2_mean_global) > std_multiplier * z2_std_global:
+                z2 = previous_z2  # 用相邻点的 z2 值替换
+            
+            # 计算与全局均值的距离
+            distance = np.sqrt((z1 - z1_mean_global)**2 + (z2 - z2_mean_global)**2)
+            
+            # 找到与全局均值最接近的点
+            if distance < min_distance:
+                min_distance = distance
+                best_z1 = z1
+                best_z2 = z2
+        
+        # 更新相邻点值
+        previous_z1 = best_z1
+        previous_z2 = best_z2
+        
+        # 添加该点到新点列表
+        new_points.append([x, y, best_z1, best_z2])
+    
+    # 转换为 numpy 数组
+    new_points = np.array(new_points)
+    
+    return new_points
 
+
+
+def southwell_integrate_smooth(x_grad_expand, y_grad_expand, x_expand, y_expand, reference_point=(0, 0)):
+    """
+    Southwell 积分方法，用于根据梯度场重建标量场，加入平滑处理以保证结果连续。
+    
+    Parameters:
+    x_grad_expand: numpy.ndarray
+        x方向的梯度矩阵。
+    y_grad_expand: numpy.ndarray
+        y方向的梯度矩阵。
+    x_expand: numpy.ndarray
+        网格的x坐标矩阵。
+    y_expand: numpy.ndarray
+        网格的y坐标矩阵。
+    reference_point: tuple
+        起始积分点的坐标索引，默认为(0, 0)。
+    
+    Returns:
+    Z_reconstructed: numpy.ndarray
+        重建的标量场。
+    """
+   
+    # 检查是否为二维数组
+    H, W = x_grad_expand.shape
     
-    if debug:
-        # 绘制3D点云
-        fig = plt.figure()
-        ax = fig.add_subplot(111, projection='3d')
-        ax.scatter(x, y, z, c=z, cmap='viridis', marker='o', s=2)
-        ax.set_title('3D Point Cloud with raw Points')
+    # 初始化标量场，设为零
+    Z_reconstructed = np.zeros((H, W))
+    
+    # 创建一个布尔矩阵，跟踪已处理的点
+    visited = np.zeros((H, W), dtype=bool)
+    
+    # 从参考点开始，标量值为0
+    ref_y, ref_x = reference_point
+    visited[ref_y, ref_x] = True
+    
+    # 创建队列，用于遍历
+    queue = [(ref_y, ref_x)]
+    
+    # 遍历队列中的点
+    while queue:
+        current_y, current_x = queue.pop(0)
+        current_z = Z_reconstructed[current_y, current_x]
+        
+        # 处理四个方向的邻居 (上，下，左，右)
+        neighbors = [
+            (current_y - 1, current_x, y_grad_expand[current_y - 1, current_x] if current_y > 0 else 0),  # 上
+            (current_y + 1, current_x, y_grad_expand[current_y, current_x] if current_y < H - 1 else 0),  # 下
+            (current_y, current_x - 1, x_grad_expand[current_y, current_x - 1] if current_x > 0 else 0),  # 左
+            (current_y, current_x + 1, x_grad_expand[current_y, current_x] if current_x < W - 1 else 0)   # 右
+        ]
+        
+        # 遍历邻居点
+        for ny, nx, grad in neighbors:
+            if 0 <= ny < H and 0 <= nx < W and not visited[ny, nx]:
+                # 通过邻居梯度方向的平均值来平滑积分
+                if ny != current_y:  # y方向积分
+                    neighbor_grad = (y_grad_expand[current_y, current_x] + grad) / 2  # 平滑处理
+                    Z_reconstructed[ny, nx] = current_z + neighbor_grad * (y_expand[ny, nx] - y_expand[current_y, current_x])
+                else:  # x方向积分
+                    neighbor_grad = (x_grad_expand[current_y, current_x] + grad) / 2  # 平滑处理
+                    Z_reconstructed[ny, nx] = current_z + neighbor_grad * (x_expand[ny, nx] - x_expand[current_y, current_x])
+                
+                # 标记为已访问
+                visited[ny, nx] = True
+                
+                # 将邻居点加入队列
+                queue.append((ny, nx))
+    
+    return Z_reconstructed
+
+
+
+def compute_divergence(x_grad_expand, y_grad_expand):
+    """
+    计算梯度场的散度（div_G），用于泊松重建。
+    
+    Parameters:
+    x_grad_expand: numpy.ndarray
+        x方向的梯度矩阵。
+    y_grad_expand: numpy.ndarray
+        y方向的梯度矩阵。
+    
+    Returns:
+    div_G: numpy.ndarray
+        梯度场的散度矩阵。
+    """
+    H, W = x_grad_expand.shape
+    div_G = np.zeros((H, W))
 
-        fig = plt.figure()
-        ax = fig.add_subplot(111, projection='3d')
-        ax.scatter(x, y, smoothed_z, c=smoothed_z, cmap='viridis', marker='o', s=2)
-        ax.set_title('3D Point Cloud with smoothed_z Points')
+    # 计算 x 方向的梯度差分
+    div_G[:, 1:] += x_grad_expand[:, 1:] - x_grad_expand[:, :-1]
     
-        fig = plt.figure()
-        ax = fig.add_subplot(111, projection='3d')
-        ax.scatter(x, y, z_outliers_removed, c=z_outliers_removed, cmap='viridis')
-        ax.set_title('3D Point Cloud with remove_outliers Points')
+    # 计算 y 方向的梯度差分
+    div_G[1:, :] += y_grad_expand[1:, :] - y_grad_expand[:-1, :]
+    
+    return div_G
 
-        fig = plt.figure()
-        ax = fig.add_subplot(111, projection='3d')
-        ax.scatter(x, y, z_final, c=z_final, cmap='viridis')
-        ax.set_title('3D Point Cloud with z_final Points')
-        ax.set_zlim(0,10)
-        
+def poisson_reconstruct(x_grad_expand, y_grad_expand):
+    """
+    使用泊松重建方法重建标量场。
     
-        plt.show()
+    Parameters:
+    x_grad_expand: numpy.ndarray
+        x方向的梯度矩阵。
+    y_grad_expand: numpy.ndarray
+        y方向的梯度矩阵。
+    
+    Returns:
+    Z_reconstructed: numpy.ndarray
+        重建的标量场。
+    """
+    H, W = x_grad_expand.shape
+    
+    # 计算散度
+    div_G = compute_divergence(x_grad_expand, y_grad_expand)
+    
+    # 构建泊松方程的离散化矩阵（稀疏矩阵）
+    A = lil_matrix((H * W, H * W))
+    b = div_G.flatten()
+
+    for y in range(H):
+        for x in range(W):
+            index = x + y * W
+            
+            if x == 0 or x == W - 1 or y == 0 or y == H - 1:
+                # 边界点，直接固定为 0（Dirichlet 边界条件）
+                A[index, index] = 1
+                b[index] = 0
+            else:
+                # 内部点，应用泊松方程的离散形式
+                A[index, index] = -4
+                A[index, index - 1] = 1  # 左边的点
+                A[index, index + 1] = 1  # 右边的点
+                A[index, index - W] = 1  # 上边的点
+                A[index, index + W] = 1  # 下边的点
 
-    return z_final
+    # 使用 scipy 的稀疏求解器来解泊松方程
+    Z_flat = spsolve(A.tocsr(), b)
+    Z_reconstructed = Z_flat.reshape((H, W))
     
+    return Z_reconstructed
 
 
 
-def plot_camera(ax, rotation_matrix, translation_vector, camera_id, scale=1.0, color='r'):
-    """
-    绘制单个相机的三维表示：位置、方向和视野
-    R: 3x3旋转矩阵 (从世界坐标系到相机坐标系)
-    t: 3x1平移向量 (相机位置)
-    scale: 控制相机大小的比例因子
-    color: 相机视野的颜色
-    """
-    # 相机的中心
-    camera_center = -rotation_matrix.T @ translation_vector
-    print('camera_center = ', camera_center)
-    camera_center = np.squeeze(camera_center)  # 确保 camera_center 是 1D 数组
-    #print('camera_center = ', camera_center)
+def post_process_with_grad(img_folder, debug=False):
+    total_point_grad = np.empty((0, 4))
+    for i in range(4):
+        txt_file = img_folder + str(i) + '_gradient.txt'
+        total_point_grad = np.vstack([total_point_grad, np.loadtxt(os.path.join(txt_file), delimiter=',')])
+     
+    print(total_point_grad.shape)
 
-    # 定义相机的四个角点在相机坐标系中的位置
-    camera_points = np.array([
-        [0, 0, 0],  # 相机中心
-        [1, 1, 2],  # 视野的左上角
-        [1, -1, 2],  # 视野的左下角
-        [-1, -1, 2],  # 视野的右下角
-        [-1, 1, 2]  # 视野的右上角
-    ]) * scale
+    total_point_grad = remove_duplicates_4d_with_std_neighbor(total_point_grad, 3)
 
-    # 将相机坐标系下的点转换到世界坐标系
-    camera_points_world = (rotation_matrix.T @ camera_points.T).T + camera_center.T
-    # 绘制相机位置（红色圆点表示相机中心）
-    ax.scatter(camera_center[0], camera_center[1], camera_center[2], color=color, marker='o', s=100)
+    print(total_point_grad.shape) 
     
-    # 绘制相机的 X、Y、Z 坐标轴
-    axis_length = scale * 3  # 坐标轴箭头长度
-    x_axis = rotation_matrix[:, 0]  # X 轴方向
-    y_axis = rotation_matrix[:, 1]  # Y 轴方向
-    z_axis = rotation_matrix[:, 2]  # Z 轴方向
+    x = total_point_grad[:,0]
+    y = total_point_grad[:,1]
 
-    # X 轴箭头（红色）
-    ax.quiver(camera_center[0], camera_center[1], camera_center[2],
-              x_axis[0], x_axis[1], x_axis[2],
-              length=axis_length, color='r', arrow_length_ratio=0.5, label='X axis')
+    x_gradient = total_point_grad[:,2]
+    y_gradient = total_point_grad[:,3]
 
-    # Y 轴箭头（绿色）
-    ax.quiver(camera_center[0], camera_center[1], camera_center[2],
-              y_axis[0], y_axis[1], y_axis[2],
-              length=axis_length, color='g', arrow_length_ratio=0.5, label='Y axis')
+    mean_x_grad = np.mean(x_gradient)
+    mean_y_grad = np.mean(y_gradient)
 
-    # Z 轴箭头（蓝色）
-    ax.quiver(camera_center[0], camera_center[1], camera_center[2],
-              z_axis[0], z_axis[1], z_axis[2],
-              length=axis_length, color='b', arrow_length_ratio=0.5, label='Z axis')
+    x_gradient =  (x_gradient - mean_x_grad)
+    y_gradient =  (y_gradient - mean_y_grad)
+    
+    # 使用最小外接矩形，并填充梯度
+    x_grad_expand, y_grad_expand, x_expand, y_expand = expand_to_rectangle_with_unit_spacing(x, y, x_gradient, y_gradient)   
+    #print(x_grad_expand.shape, y_grad_expand.shape, x_expand.shape, y_expand.shape)
+    # 调用 Southwell 重建函数
+    #Z_reconstructed = southwell_integrate_smooth(x_grad_expand, y_grad_expand, x_expand, y_expand, (0, 0))
 
-    # 绘制相机中心
-    ax.scatter(camera_center[0], camera_center[1], camera_center[2], color=color, marker='o', s=100, label="Camera")
-    ax.text(camera_center[0], camera_center[1], camera_center[2], f'Cam {camera_id}', color='black', fontsize=12)
+    Z_reconstructed = poisson_reconstruct(x_grad_expand, y_grad_expand)
+    
+    z1_lookup = {(x1, y1): z1 for x1, y1, z1 in np.column_stack((x_expand.reshape(-1,1), y_expand.reshape(-1,1), Z_reconstructed.reshape(-1,1)))}
 
-    #绘制相机视野四个角点与相机中心的连线
-    for i in range(1, 5):
-        ax.plot([camera_center[0], camera_points_world[i, 0]],
-                [camera_center[1], camera_points_world[i, 1]],
-                [camera_center[2], camera_points_world[i, 2]], color=color)
+    z1_values = np.array([z1_lookup.get((x, y), np.nan) for x, y in np.column_stack((x, y))])
+    fitted_points = post_process(np.column_stack((x,y,z1_values)), debug=0)
 
-    # 绘制相机的四边形视野
-    ax.plot([camera_points_world[1, 0], camera_points_world[2, 0], camera_points_world[3, 0], camera_points_world[4, 0], camera_points_world[1, 0]],
-            [camera_points_world[1, 1], camera_points_world[2, 1], camera_points_world[3, 1], camera_points_world[4, 1], camera_points_world[1, 1]],
-            [camera_points_world[1, 2], camera_points_world[2, 2], camera_points_world[3, 2], camera_points_world[4, 2], camera_points_world[1, 2]], color=color)
+    # 可视化梯度场
+    if debug:
+        fig = plt.figure()
+        ax = fig.add_subplot(111, projection='3d')
+
+        #提取 x, y, z 坐标
+        x_vals = fitted_points[:,0]
+        y_vals = fitted_points[:,1]
+        z_vals = fitted_points[:,2]
+        # x_vals = x
+        # y_vals = y
+        # z_vals = z1_values
+        # 绘制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('post_process 3D Point Cloud Visualization')
+        plt.show()
+    
+    return fitted_points
 
-    # # 添加相机方向箭头
-    # ax.quiver(camera_center[0], camera_center[1], camera_center[2],
-    #           rotation_matrix[0, 2], rotation_matrix[1, 2], rotation_matrix[2, 2], length=scale, color='g', arrow_length_ratio=1, label="Direction")
 
 
 def transform_image_to_cam0(img, K_cam_i, R_cam_i, T_cam_i, K_cam0, R_cam0, T_cam0, depth_map):
@@ -1089,60 +1350,6 @@ def transform_image_to_cam0(img, K_cam_i, R_cam_i, T_cam_i, K_cam0, R_cam0, T_ca
     return output_img
 
 
-
-def camera_calibrate_vis():
-    img_folder = 'D:\\data\\four_cam\\0927_storage\\20240927161124014'
-    config_path = 'D:\\code\\pmd-python\\config\\cfg_3freq_wafer.json'
-    cfg = json.load(open(config_path, 'r'))
-    with open(os.path.join('D:\\code\\pmd-python\\config\\', cfg['cam_params']), 'rb') as pkl_file:
-        cam_params = pickle.load(pkl_file)
-    fig = plt.figure()
-    ax = fig.add_subplot(111, projection='3d')
-
-    # 绘制每个相机
-    for ii in range(4):
-        plot_camera(ax, cam_params[ii]['rotation_matrix'], cam_params[ii]['translation_vector'], ii, scale=40, color='r')
-
-    # 绘制物体 (假设是一个简单的平面物体，位于 z = 0 平面上)
-    plane_x, plane_y = np.meshgrid(np.linspace(-500, 500, 10), np.linspace(-500, 500, 10))
-    plane_z = np.zeros_like(plane_x)  # z = 0 的平面
-    ax.plot_surface(plane_x, plane_y, plane_z, color='blue', alpha=0.3)
-
-    # 设置轴的范围和标签
-    ax.set_xlabel('X axis')
-    ax.set_ylabel('Y axis')
-    ax.set_zlabel('Z axis')
-    ax.set_xlim([-50, 300])
-    ax.set_ylim([-50, 300])
-    ax.set_zlim([0, 500])
-
-    K_cam0 = cam_params[0]['camera_matrix']
-    R_cam0 = cam_params[0]['rotation_matrix']
-    T_cam0 = cam_params[0]['translation_vector']
-
-    for i in range(4):
-        img_path = img_folder + '\\' + str(i) + '_frame_24.bmp'
-        img = cv2.imread(img_path, 0)
-        print('max gray = ', np.max(img))
-        if i > 0:
-            img = cv2.imread(img_path, 0)
-            K_cam_i = cam_params[i]['camera_matrix']
-            R_cam_i = cam_params[i]['rotation_matrix']
-            T_cam_i = cam_params[i]['translation_vector']
-            # 示例图像和深度图
-            depth_map = img
-
-            # 将图像转换到 cam0 坐标系下
-            #aligned_img = transform_image_to_cam0(img, K_cam_i, R_cam_i, T_cam_i, K_cam0, R_cam0, T_cam0, depth_map)
-            
-            # cv2.namedWindow('Aligned Image', 0)
-            # cv2.imshow('Aligned Image', aligned_img)
-            # cv2.waitKey(0)
-            # cv2.destroyAllWindows()
-    
-    plt.show()
-
-
 def rename_gamma():
     img_path = 'D:\\code\\Gamma\\Gamma\\cam3_original_pictures\\'
     i = 0
@@ -1303,8 +1510,6 @@ def apply_local_average_smoothing(z_reconstructed, window_size=3):
 
 
 
-
-
 def average_duplicate_points(points):
     """
     处理点云，计算每个 (x, y) 坐标中所有 z 值的均值。
@@ -1323,6 +1528,7 @@ def average_duplicate_points(points):
     
     return unique_points[['x', 'y', 'z']].to_numpy()
 
+
 def remove_duplicate_points(points):
     """
     处理点云，保留每个 (x, y) 坐标中与全局 z 值均值最近的一个 z 值。
@@ -1446,6 +1652,135 @@ def laplacian_smoothing(points, z_values, iterations=10, alpha=0.1):
 
 
 
+def project_points_to_camera(P_world, K, R, t):
+    """
+    将世界坐标系中的 3D 点投影到相机的图像平面。
+    
+    Parameters:
+    P_world: numpy.ndarray
+        世界坐标系中的 3D 点，形状为 (N, 3)。
+    K: numpy.ndarray
+        相机内参矩阵，形状为 (3, 3)。
+    R: numpy.ndarray
+        相机外参的旋转矩阵，形状为 (3, 3)。
+    t: numpy.ndarray
+        相机外参的平移向量，形状为 (3, 1)。
+    
+    Returns:
+    p_image: numpy.ndarray
+        投影到图像平面的 2D 点，形状为 (N, 2)。
+    """
+    # 将世界坐标系中的点转换为相机坐标系
+    P_cam = R @ P_world.T + t
+    P_cam = P_cam.T
+    
+    # 将相机坐标系中的点投影到图像平面
+    P_image_homogeneous = K @ P_cam.T
+    P_image_homogeneous = P_image_homogeneous.T
+    
+    # 转换为 2D 图像坐标
+    p_image = P_image_homogeneous[:, :2] / P_image_homogeneous[:, 2][:, np.newaxis]
+    
+    return p_image
+
+def cross_validate_camera_pair(P_world, K_A, R_A, t_A, K_B, R_B, t_B, p_B_measured):
+    """
+    验证相机 A 的标定点在相机 B 中的投影误差。
+    
+    Parameters:
+    P_world: numpy.ndarray
+        标定板上的 3D 点的世界坐标，形状为 (N, 3)。
+    K_A, R_A, t_A: numpy.ndarray
+        相机 A 的内参矩阵、旋转矩阵和平移向量。
+    K_B, R_B, t_B: numpy.ndarray
+        相机 B 的内参矩阵、旋转矩阵和平移向量。
+    p_B_measured: numpy.ndarray
+        相机 B 实际测量到的标定点，形状为 (N, 2)。
+    
+    Returns:
+    reprojection_error: float
+        计算得到的投影误差。
+    """
+    # 将 P_world 投影到相机 B 的图像平面
+    R_BA = R_B @ np.linalg.inv(R_A)  # 相机 A 和相机 B 的旋转矩阵关系
+    t_BA = t_B - R_B @ np.linalg.inv(R_A) @ t_A  # 相机 A 和相机 B 的平移向量关系
+    p_B_projected = project_points_to_camera(P_world, K_B, R_BA, t_BA)
+
+    # 计算投影误差
+    reprojection_error = np.mean(np.linalg.norm(p_B_projected - p_B_measured, axis=1))
+    
+    return reprojection_error
+
+def plot_camera(ax, rotation_matrix, translation_vector, camera_id, scale=1.0, color='r'):
+    """
+    绘制单个相机的三维表示：位置、方向和视野
+    R: 3x3旋转矩阵 (从世界坐标系到相机坐标系)
+    t: 3x1平移向量 (相机位置)
+    scale: 控制相机大小的比例因子
+    color: 相机视野的颜色
+    """
+    # 相机的中心
+    camera_center = -rotation_matrix.T @ translation_vector
+    #print('camera_center = ', camera_center)
+    camera_center = np.squeeze(camera_center)  # 确保 camera_center 是 1D 数组
+    #print('camera_center = ', camera_center)
+
+    # 定义相机的四个角点在相机坐标系中的位置
+    camera_points = np.array([
+        [0, 0, 0],  # 相机中心
+        [1, 1, 2],  # 视野的左上角
+        [1, -1, 2],  # 视野的左下角
+        [-1, -1, 2],  # 视野的右下角
+        [-1, 1, 2]  # 视野的右上角
+    ]) * scale
+
+    # 将相机坐标系下的点转换到世界坐标系
+    camera_points_world = (rotation_matrix.T @ camera_points.T).T + camera_center.T
+    # 绘制相机位置（红色圆点表示相机中心）
+    ax.scatter(camera_center[0], camera_center[1], camera_center[2], color=color, marker='o', s=100)
+    
+    # 绘制相机的 X、Y、Z 坐标轴
+    axis_length = scale * 3  # 坐标轴箭头长度
+    x_axis = rotation_matrix[:, 0]  # X 轴方向
+    y_axis = rotation_matrix[:, 1]  # Y 轴方向
+    z_axis = rotation_matrix[:, 2]  # Z 轴方向
+
+    # X 轴箭头（红色）
+    ax.quiver(camera_center[0], camera_center[1], camera_center[2],
+              x_axis[0], x_axis[1], x_axis[2],
+              length=axis_length, color='r', arrow_length_ratio=0.5, label='X axis')
+
+    # Y 轴箭头（绿色）
+    ax.quiver(camera_center[0], camera_center[1], camera_center[2],
+              y_axis[0], y_axis[1], y_axis[2],
+              length=axis_length, color='g', arrow_length_ratio=0.5, label='Y axis')
+
+    # Z 轴箭头（蓝色）
+    ax.quiver(camera_center[0], camera_center[1], camera_center[2],
+              z_axis[0], z_axis[1], z_axis[2],
+              length=axis_length, color='b', arrow_length_ratio=0.5, label='Z axis')
+
+    # 绘制相机中心
+    ax.scatter(camera_center[0], camera_center[1], camera_center[2], color=color, marker='o', s=100, label="Camera")
+    ax.text(camera_center[0], camera_center[1], camera_center[2], f'Cam {camera_id}', color='black', fontsize=12)
+
+    #绘制相机视野四个角点与相机中心的连线
+    for i in range(1, 5):
+        ax.plot([camera_center[0], camera_points_world[i, 0]],
+                [camera_center[1], camera_points_world[i, 1]],
+                [camera_center[2], camera_points_world[i, 2]], color=color)
+
+    # 绘制相机的四边形视野
+    ax.plot([camera_points_world[1, 0], camera_points_world[2, 0], camera_points_world[3, 0], camera_points_world[4, 0], camera_points_world[1, 0]],
+            [camera_points_world[1, 1], camera_points_world[2, 1], camera_points_world[3, 1], camera_points_world[4, 1], camera_points_world[1, 1]],
+            [camera_points_world[1, 2], camera_points_world[2, 2], camera_points_world[3, 2], camera_points_world[4, 2], camera_points_world[1, 2]], color=color)
+
+    # # 添加相机方向箭头
+    # ax.quiver(camera_center[0], camera_center[1], camera_center[2],
+    #           rotation_matrix[0, 2], rotation_matrix[1, 2], rotation_matrix[2, 2], length=scale, color='g', arrow_length_ratio=1, label="Direction")
+
+
+
 def bilinear_interpolation(points, z_values, grid_size=(100, 100)):
     """
     使用双线性插值对点云 z 值进行平滑。
@@ -1515,49 +1850,40 @@ def moving_average_filter_z(points, window_size=3):
     return np.column_stack((points[:,0], points[:,1], smoothed_z))
 
 
-
-def rbf_interpolation(points, z_values, grid_size=(100, 100)):
-    """
-    使用径向基函数 (RBF) 对点云 z 值进行插值和平滑。
-    
-    参数:
-    points: (n, 2) 形状的 NumPy 数组，表示 (x, y) 坐标的点
-    z_values: 点云的 z 值数组
-    grid_size: 定义插值后生成的网格大小
-    
-    返回:
-    grid_z: 插值后的平滑 z 值
-    """
-    # 定义插值网格
-    x_min, x_max = points[:, 0].min(), points[:, 0].max()
-    y_min, y_max = points[:, 1].min(), points[:, 1].max()
-    grid_x, grid_y = np.mgrid[x_min:x_max:grid_size[0]*1j, y_min:y_max:grid_size[1]*1j]
-
-    # 使用径向基函数进行插值
-    rbf = Rbf(points[:, 0], points[:, 1], z_values, function='linear')
-    grid_z = rbf(grid_x, grid_y)
-    
-    return grid_x, grid_y, grid_z
-
-
 def show_cloud_point(pointcloud_path):
     points = np.loadtxt(os.path.join(pointcloud_path), delimiter=',')
     
-    
 
 
+def img_diff():
+    img_dir1 = 'D:\\code\\Gamma\\Gamma\\3_pattern\\'
+    img_dir2 = 'D:\\code\\Gamma\\Gamma\\patterns\\'
+    for img in os.listdir(img_dir1):
+        img_path1 = img_dir1 + img
+        img_path2 = img_dir2 + img
+        src1 = cv2.imread(img_path1)
+        src2 = cv2.imread(img_path2)
+        diff = 50 * cv2.absdiff(src1, src2)
+        print(np.mean(diff))
+        cv2.namedWindow('diff', 0)
+        cv2.imshow('diff', diff)
+        cv2.waitKey(0)
+        
+        
+
 
 
 
 if __name__ == '__main__':
+
     # white_path = 'D:\\data\\four_cam\\1008_storage\\'
     # cam_num = 1
-    # #find_notch(white_path, cam_num)
+    #find_notch(white_path, cam_num)
     # for img in os.listdir(white_path):
     #     if img.endswith('.bmp'):
     #         get_white_mask(white_path + img, bin_thresh=12)
-    #camera_calibrate_vis()
-    img_folder = 'D:\\data\\four_cam\\1008_storage\\20241008210412543'
-    point_cloud_path = os.path.join(img_folder, 'cloudpoint.txt')
-    show_cloud_point(point_cloud_path)
+    # img_folder = 'D:\\data\\four_cam\\1008_storage\\20241008210412543'
+    # point_cloud_path = os.path.join(img_folder, 'cloudpoint.txt')
+    # show_cloud_point(point_cloud_path)
+    img_diff()