fix get white mask

config/cfg_3freq_wafer.json

@@ -7,7 +7,7 @@
     "num_freq": 3,
     "screenPixelSize": [1600, 1200],
     "screenorigin_point": [548, 1050],
-    "Axis_conversion": [[1, 1],[1, 1],[-1, -1],[-1, -1]],
+    "Axis_conversion": [[-1, -1],[-1, -1],[-1, -1],[-1, -1]],
     "k": 32.0,
     "pixelSize_mm": 0.253125,
     "grid_spacing":1,

main.py

@@ -1,204 +0,0 @@
-import cv2
-import os
-import sys
-sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
-import time
-import glob
-import json
-import argparse
-import pickle
-import matplotlib.pyplot as plt
-
-from datetime import datetime
-import numpy as np
-import scipy.io as sio
-from scipy.io import savemat, loadmat
-np.random.seed(42)
-
-from src.utils import binarize, get_meshgrid, get_world_points, get_camera_points, get_screen_points, write_point_cloud, \
-              get_white_mask, get_world_points_from_mask, get_meshgrid_contour, post_process
-from src.phase import extract_phase, unwrap_phase
-from src.recons import reconstruction_cumsum
-from src.calibration import calibrate_world, calibrate_screen, map_screen_to_world
-from src.vis import plot_coords
-from src.eval import get_eval_result, find_notch
-
-def list_directories(path):
-    # List all items in the given directory and filter only directories
-    return [item for item in os.listdir(path) if os.path.isdir(os.path.join(path, item))]
-
-def parse_args():
-
-    parser = argparse.ArgumentParser(description="")
-    parser.add_argument("--img_path", type=str, default='D:\\file\\20240913-data\\20240913105234292')  
-    parser.add_argument("--camera_path", type=str, default='D:\\file\\标定数据\\标定数据0913\\calibration_0913')
-    parser.add_argument("--screen_path", type=str, default='D:\\file\\screen0920')
-    parser.add_argument("--cfg", type=str, default="config/cfg_3freq_wafer.json")
-    #parser.add_argument("--cfg", type=str, default="D:\code\pmdrecons-python\config\cfg_3freq_wafer.json")
-    parser.add_argument("--save_path", type=str, default="debug")
-    parser.add_argument("--grid_spacing", type=int, default=1)  
-    parser.add_argument("--debug", action='store_true')
-    parser.add_argument("--smooth", action='store_true')
-    parser.add_argument("--align", action='store_true')
-    parser.add_argument("--denoise", action='store_true')
-    args = parser.parse_args()
-    return args 
-
-
-def main(cfg, img_folder, camera_path, screen_path, save_path, grid_spacing, debug):
-    n_cam = 4
-    num_freq = cfg['num_freq']
-    start_time = time.time()
-    #debug = True
-    print(f"开始执行时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
-    print("\n1. 相机标定")
-    preprocess_start = time.time()
-    
-    camera_subdir = list_directories(camera_path)
-    camera_subdir.sort()
-    assert len(camera_subdir) == 4, f"found {len(camera_subdir)} cameras, should be 4"
-    cam_para_path = os.path.join(camera_path, "cam_params.pkl")
-
-    if os.path.exists(cam_para_path):
-        with open(cam_para_path, 'rb') as pkl_file:
-            cam_params = pickle.load(pkl_file)
-    else:
-        cam_params = []
-        for i in range(n_cam):
-            cam_img_path = glob.glob(os.path.join(camera_path, camera_subdir[i], "*.bmp"))
-            cam_img_path.sort()
-            cam_param_raw = calibrate_world(cam_img_path, i, cfg['world_chessboard_size'], cfg['world_square_size'], debug=False)
-            cam_params.append(cam_param_raw)
-        with open(cam_para_path, 'wb') as pkl_file:
-           pickle.dump(cam_params, pkl_file)
-   
-    preprocess_end = time.time()
-    print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
-    print(f"   耗时: {preprocess_end - preprocess_start:.2f} 秒") 
-    # import pdb; pdb.set_trace()
- 
-    print("\n2. 屏幕标定")
-    screen_cal_start = time.time()
-    screen_img_path = glob.glob(os.path.join(screen_path, "*.bmp"))
-    screen_para_path = os.path.join(screen_path, "screen_params.pkl")    
-    if os.path.exists(screen_para_path):
-        with open(screen_para_path, 'rb') as pkl_file:
-            screen_params = pickle.load(pkl_file)[0]
-    else:
-        screen_params = calibrate_screen(screen_img_path, cam_params[0]['camera_matrix'], cam_params[0]['distortion_coefficients'], cfg['screen_chessboard_size'], cfg['screen_square_size'], debug=True)
-        with open(screen_para_path, 'wb') as pkl_file:
-            pickle.dump([screen_params], pkl_file)
-
-    screen_to_world = map_screen_to_world(screen_params, cam_params[0])  
-    screen_cal_end = time.time()
-    print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
-    print(f"   耗时: {screen_cal_end - screen_cal_start:.2f} 秒")
-   
-    print("\n3. 相位提取，相位展开")
-    phase_start = time.time()
-    x_uns, y_uns = [], []
-    binary_masks = []
-    for cam_id in range(n_cam):
-        white_path = os.path.join(img_folder, f'{cam_id}_frame_24.bmp')
-        binary = get_white_mask(white_path)
-        binary_masks.append(binary)
-        phases = extract_phase(img_folder, cam_id, binary, cam_params[cam_id]['camera_matrix'], cam_params[cam_id]['distortion_coefficients'], num_freq=num_freq)
-        x_un, y_un = unwrap_phase(phases, save_path, num_freq, debug=False)
-        x_uns.append(x_un)
-        y_uns.append(y_un)
-    
-    phase_end = time.time()
-    print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
-    print(f"   耗时: {phase_end - phase_start:.2f} 秒")
- 
-    print("\n4. 获得不同坐标系下点的位置")
-    get_point_start = time.time()
-    total_cloud_point = np.empty((0, 3))
-    for i in range(n_cam):
-        if i > 5:
-            continue
-        contours_point = get_meshgrid_contour(binary_masks[i], grid_spacing, save_path, debug=False)
-        world_points = get_world_points(contours_point, cam_params[i], grid_spacing, cfg['d'], save_path, debug=debug)
-        camera_points, u_p, v_p = get_camera_points(world_points, cam_params[i], save_path, i, debug=debug)
-        
-        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, smoothed, aligned, denoise = reconstruction_cumsum(world_points, camera_points, screen_points, save_path, i, debug=debug, smooth=args.smooth, align=args.align, denoise=args.denoise)
-        write_point_cloud(os.path.join(img_folder, str(i) + '_cloudpoint.txt'), world_points[:, 0], world_points[:, 1], 1000 * denoise[:,2])
-
-        total_cloud_point = np.vstack([total_cloud_point, np.column_stack((denoise[:, 0], denoise[:, 1], 1000 * denoise[:,2]))])
-        print('point cloud has been written in file')
-
-    write_point_cloud(os.path.join(img_folder, 'cloudpoint.txt'), total_cloud_point[:, 0], total_cloud_point[:, 1], total_cloud_point[:,2])
-    
-    if debug:
-        fig = plt.figure()
-        ax = fig.add_subplot(111, projection='3d')
-
-        # 提取 x, y, z 坐标
-        x_vals = total_cloud_point[:, 0]
-        y_vals = total_cloud_point[:, 1]
-        z_vals = total_cloud_point[:, 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()
-    get_point_end = time.time()
-    print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
-    print(f"   耗时: {get_point_end - get_point_start:.2f} 秒")
-    
-    print("\n5. 后处理")
-    post_process_start = time.time()
-    post_process(args.img_path, debug)
-    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} 秒")
-
-    print("\n6. 评估")
-    eval_start = time.time()
-    point_cloud_path = os.path.join(img_folder, str(i) + '_cloudpoint.txt')
-    json_path = os.path.join(img_folder, str(i) + 'result.json')
-    theta_notch = 0
-    get_eval_result(point_cloud_path, json_path, theta_notch)
-    post_process(args.img_path, debug)
-    eval_end = time.time()
-    print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
-    print(f"   耗时: {eval_end - eval_start:.2f} 秒")
-
-
-    return True
-        
-
-if __name__ == '__main__': 
-
-    start_time = time.time()
-    
-    args = parse_args()
-    img_folder = args.img_path
-    
-    
-    cfg = json.load(open(args.cfg, 'r'))
-    if not os.path.exists(args.save_path):
-        os.makedirs(args.save_path)
-    
-    args.smooth = True
-    args.align = True
-    args.denoise = True
-    args.debug = 0
-
-    main(cfg, img_folder, args.camera_path, args.screen_path, args.save_path, args.grid_spacing, args.debug)
-
-    end_time = time.time()
-    print(f"总运行时间: {end_time - start_time:.2f} 秒")
-    

pmd.py

@@ -1,6 +1,8 @@
+#encoding=utf-8
 import time
 import os
 import sys
+
 sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 import time
 import glob
@@ -8,7 +10,6 @@ import numpy as np
 np.random.seed(42)
 from datetime import datetime
 import json
-from scipy.io import savemat, loadmat
 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
 from src.phase import extract_phase, unwrap_phase
 from src.recons import reconstruction_cumsum
@@ -33,26 +34,55 @@ def pmdstart(config_path, img_folder):
 
 
 def main(config_path, img_folder):
+
     cfg = json.load(open(config_path, 'r'))
     n_cam = 4
     num_freq = cfg['num_freq']
-    save_path = None
+    save_path = 'debug'
     debug = False
     grid_spacing = cfg['grid_spacing']
     num_freq = cfg['num_freq']
     smooth = True
     align = True
     denoise = True
+    cammera_img_path = 'D:\\data\\four_cam\\calibrate\\calibration_0913'
+    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()
 
-    with open(os.path.join('config', cfg['cam_params']), 'rb') as pkl_file:
-        cam_params = pickle.load(pkl_file)
-    with open(os.path.join('config', cfg['screen_params']), 'rb') as pkl_file:
-            screen_params = pickle.load(pkl_file)[0]
+    cam_para_path = os.path.join('config', cfg['cam_params'])
+    if os.path.exists(cam_para_path):
+    #if False:
+        with open(cam_para_path, 'rb') as pkl_file:
+            cam_params = pickle.load(pkl_file)
+    else:
+        cam_params = []
+        camera_subdir = [item for item in os.listdir(cammera_img_path) if os.path.isdir(os.path.join(cammera_img_path, item))]
+        camera_subdir.sort()
+        assert len(camera_subdir) == 4, f"found {len(camera_subdir)} cameras, should be 4"
+        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()
+            cam_param_raw = calibrate_world(cam_img_path, i, cfg['world_chessboard_size'], cfg['world_square_size'], debug=debug)
+            cam_params.append(cam_param_raw)
+        with open(cam_para_path, 'wb') as pkl_file:
+           pickle.dump(cam_params, pkl_file) 
+
+    
     
+    screen_img_path = glob.glob(os.path.join(screen_img_path, "*.bmp"))
+    screen_para_path = os.path.join('config', cfg['screen_params'])   
+    if os.path.exists(screen_para_path):
+    #if False:
+        with open(screen_para_path, 'rb') as pkl_file:
+            screen_params = pickle.load(pkl_file)[0]
+    else:
+        screen_params = calibrate_screen(screen_img_path, cam_params[0]['camera_matrix'], cam_params[0]['distortion_coefficients'], cfg['screen_chessboard_size'], cfg['screen_square_size'], debug=True)
+        with open(screen_para_path, 'wb') as pkl_file:
+            pickle.dump([screen_params], pkl_file)
+
     preprocess_end = time.time()
     print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
     print(f"   耗时: {preprocess_end - preprocess_start:.2f} 秒") 
@@ -71,7 +101,8 @@ def main(config_path, img_folder):
     x_uns, y_uns = [], []
     binary_masks = []
     for cam_id in range(n_cam):
-        white_path = os.path.join(img_folder, f'{cam_id}_frame_24.bmp')
+        print('cam_id = ', cam_id)
+        white_path = os.path.join(img_folder, f'{cam_id}_frame_24.bmp') 
         binary = get_white_mask(white_path)
         binary_masks.append(binary)
         phases = extract_phase(img_folder, cam_id, binary, cam_params[cam_id]['camera_matrix'], cam_params[cam_id]['distortion_coefficients'], num_freq=num_freq)
@@ -79,6 +110,7 @@ def main(config_path, img_folder):
         x_uns.append(x_un)
         y_uns.append(y_un)
     
+
     phase_end = time.time()
     print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
     print(f"   耗时: {phase_end - phase_start:.2f} 秒")
@@ -87,26 +119,19 @@ def main(config_path, img_folder):
     get_point_start = time.time()
     total_cloud_point = np.empty((0, 3))
     for i in range(n_cam):
-        if i > 5:
-            continue
         contours_point = get_meshgrid_contour(binary_masks[i], grid_spacing, save_path, debug=False)
-        world_points = get_world_points(contours_point, cam_params[i], grid_spacing, cfg['d'], save_path, debug=debug)
+        world_points = get_world_points(contours_point, cam_params[i], i, grid_spacing, cfg['d'], save_path, debug=debug)
         camera_points, u_p, v_p = get_camera_points(world_points, cam_params[i], save_path, i, debug=debug)
-        
         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, smoothed, aligned, denoised = reconstruction_cumsum(world_points, camera_points, screen_points, save_path, i, debug=debug, smooth=smooth, align=align, denoise=denoise)
-        write_point_cloud(os.path.join(img_folder, str(i) + '_cloudpoint.txt'), world_points[:, 0], world_points[:, 1], 1000 * denoised[:,2])
-
-        total_cloud_point = np.vstack([total_cloud_point, np.column_stack((denoised[:, 0], denoised[:, 1], 1000 * denoised[:,2]))])
-
-    write_point_cloud(os.path.join(img_folder, 'cloudpoint.txt'), total_cloud_point[:, 0], total_cloud_point[:, 1], total_cloud_point[:,2])
+        z, smoothed, aligned, denoised = reconstruction_cumsum(world_points, camera_points, screen_points, save_path, i, debug=False, smooth=smooth, align=align, denoise=denoise)
+        write_point_cloud(os.path.join(img_folder, str(i) + '_cloudpoint.txt'), world_points[:, 0], world_points[:, 1], 1000 * z)
+        total_cloud_point = np.vstack([total_cloud_point, np.column_stack((world_points[:, 0], world_points[:, 1], 1000 * z))])
     
-    if debug:
+    if 1:
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
 
@@ -131,26 +156,26 @@ def main(config_path, img_folder):
     
     print("\n5. 后处理")
     post_process_start = time.time()
-    post_process(img_folder, debug)
+    #total_cloud_point = post_process(img_folder, debug=True)
+    #write_point_cloud(os.path.join(img_folder, 'cloudpoint.txt'), total_cloud_point[:, 0], total_cloud_point[:, 1], total_cloud_point[:,2])
     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} 秒")
 
     print("\n6. 评估")
     eval_start = time.time()
-    point_cloud_path = os.path.join(img_folder, str(i) + '_cloudpoint.txt')
-    json_path = os.path.join(img_folder, str(i) + 'result.json')
+    point_cloud_path = os.path.join(img_folder, 'cloudpoint.txt')
+    json_path = os.path.join(img_folder, 'result.json')
     theta_notch = 0
-    get_eval_result(point_cloud_path, json_path, theta_notch)
+    get_eval_result(point_cloud_path, json_path, theta_notch, 0)
     eval_end = time.time()
     print(f"   完成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
     print(f"   耗时: {eval_end - eval_start:.2f} 秒")
 
-
     return True
 
 if __name__ == '__main__':
-    config_path = 'D:\\code\\pmdrecons-fourcam\\config\\cfg_3freq_wafer.json'
-    img_folder = 'D:\\file\\20240913-data\\20240913105234292'
+    config_path = 'config\\cfg_3freq_wafer.json'
+    img_folder = 'D:\\data\\four_cam\\0927_storage\\20240927161651920'
 
     pmdstart(config_path, img_folder)

src/calibration/get_camera_params.py

@@ -9,20 +9,22 @@ from aprilgrid import Detector
 
 
 def show_detect_result(img, detections):
+    colors = [(255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0)]
     for detection in detections:
         corners = detection.corners
         tag_id = detection.tag_id
         center = np.mean(corners, axis=0).astype(int)[0]
-        for corner in corners:
-            cv2.circle(img, tuple(int(x) for x in corner[0]), 5, (255, 0, 0), -1)
+        for i, corner in enumerate(corners):
+            color = colors[i % len(colors)]
+            cv2.circle(img, tuple(int(x) for x in corner[0]), 25, color, -1)
         cv2.circle(img, center, 5, (255, 255, 0), -1)
         cv2.putText(img, str(tag_id), (center[0]-40, center[1]), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
         cv2.polylines(img, [np.int32(corners)], isClosed=True, color=(0, 255, 255), thickness=2)
 
-    cv2.namedWindow('AprilGrid Detection', 0)
-    cv2.imshow('AprilGrid Detection', img)
-    cv2.waitKey(0)
-    cv2.destroyAllWindows()
+    # cv2.namedWindow('AprilGrid Detection', 0)
+    # cv2.imshow('AprilGrid Detection', img)
+    # cv2.waitKey(0)
+    # cv2.destroyAllWindows()
 
 # 生成 AprilGrid 标定板的 3D 物体坐标，并考虑标签之间的间隙
 def generate_3d_points(grid_size, tag_size, tag_spacing):
@@ -59,7 +61,7 @@ def calibrate_world(calibration_images_path, cam_id, world_chessboard_size=(6,6)
     # grid_size = (5, 6)  # 标定板的行数和列数
     # tag_size = 25.3125    # 每个标签的边长（单位：毫米）
     object_point_single = generate_3d_points(world_chessboard_size, world_square_size, world_square_spacing)  
-    if debug:
+    if 0:
         # 可视化3D点
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')

src/eval.py

@@ -38,7 +38,7 @@ def remove_noise(x, y, z, a, b, c, thresh=0.01):
 
     # fig = plt.figure(figsize=(10, 7))
     # ax = fig.add_subplot(111, projection='3d')
-    # scatter = ax.scatter(x_filtered, y_filtered, z_filtered, c=z_filtered, cmap='viridis', marker='o')
+    # scatter = ax.scatter(x_keep, y_keep, z_keep, c=z_keep, cmap='viridis', marker='o')
 
     # cbar = plt.colorbar(scatter, ax=ax, shrink=0.5, aspect=5)
     # cbar.set_label('Z values')
@@ -109,7 +109,7 @@ def get_mean_line_value(x, y, z, far_point_x, far_point_y, dist_thresh=0.5):
     A = -(y2 - y1) if x2 != x1 else 1
     B = x2 - x1 if x2 != x1 else 0
     C = -(x2 * y1 - x1 * y2) if x2 != x1 else - y1
-    dist_thresh = 0.5
+    dist_thresh = 2
     flat_data = np.column_stack((x, y))
     distances = np.abs(A * x + B * y + C) / np.sqrt(A**2 + B**2)
     intersections = flat_data[distances < dist_thresh]
@@ -117,11 +117,13 @@ def get_mean_line_value(x, y, z, far_point_x, far_point_y, dist_thresh=0.5):
     tree = cKDTree(flat_data)
     _, idx = tree.query(intersections, k=1)
     z_values = z[idx]
-
-    z_plane_values = (a * intersections[:, 0] + b * intersections[:, 1] + c)
-    warpage_diff = (z_values - z_plane_values)
-    #print(warpage_diff)
-    line_warpage = max(warpage_diff) - min(warpage_diff)
+    if len(z_values) == 0:
+        return 0, 0, 0
+    else:
+        z_plane_values = (a * intersections[:, 0] + b * intersections[:, 1] + c)
+        warpage_diff = (z_values - z_plane_values)
+        
+        line_warpage = max(warpage_diff) - min(warpage_diff)
     
     distances_to_far_point = np.sqrt((intersections[:, 0] - far_point_x)**2 + (intersections[:, 1] - far_point_y)**2)
 
@@ -296,7 +298,7 @@ def find_notch(white_path):
     # cv2.waitKey(0)
     return angle_rad, thresh_img
 
-def get_eval_result(pointcloud_path, json_path, theta_notch):
+def get_eval_result(pointcloud_path, json_path, theta_notch, debug):
     data = np.loadtxt(os.path.join(pointcloud_path), delimiter=',')
     
     x = data[:, 0]
@@ -313,10 +315,17 @@ def get_eval_result(pointcloud_path, json_path, theta_notch):
     support_points = get_support_points(x_filtered, y_filtered, z_filtered)
     a_bow, b_bow, c_bow = fit_plane(support_points[:,0], support_points[:,1], support_points[:,2])
 
-    center_int = (round(np.mean(x_filtered)), round(np.mean(y_filtered)))
+    #center_int = (round(np.mean(x_filtered)), round(np.mean(y_filtered)))
+    center_int = (np.mean(x_filtered), np.mean(y_filtered))
     #print('center_int = ', center_int)
-    mask = (data[:, 0] == center_int[0]) & (data[:, 1] == center_int[1])
-    z_center = data[mask, 2][0]
+    distances = np.sqrt((data[:, 0] - center_int[0]) ** 2 + (data[:, 1] - center_int[1]) ** 2)
+    # 找到最近的点
+    nearest_index = np.argmin(distances)  # 找到最小距离对应的索引
+
+    # 获取最近点的 z 值
+    z_center = data[nearest_index, 2]
+    #mask = (data[:, 0] == center_int[0]) & (data[:, 1] == center_int[1])
+    #z_center = data[mask, 2][0]
 
     fit_z_center = a_bow*center_int[0] + b_bow*center_int[1] + c_bow
     bow = (z_center - fit_z_center)
@@ -332,19 +341,58 @@ def get_eval_result(pointcloud_path, json_path, theta_notch):
     print('max_line_bow = ', max_line_bow)
     #print('max_warpage_z = ', max_warpage_z)
 
-    x2 = int(round(np.mean(x)))  
-    y2 = int(round(np.mean(y)))
-    horizontal_mask = (y == y2)
-    horizontal_z = z[horizontal_mask]  # 水平方向对应的 z 值
-    horizontal_x = x[horizontal_mask]
+    # x2 = int(round(np.mean(x)))  
+    # y2 = int(round(np.mean(y)))
+    # horizontal_mask = (y == y2)
+    # horizontal_z = z[horizontal_mask]  # 水平方向对应的 z 值
+    # horizontal_x = x[horizontal_mask]
    
-    # 提取垂直方向的 z 值（即 x == x2 的所有点）
-    vertical_mask = (x == x2)
-    vertical_z = z[vertical_mask]  # 垂直方向对应的 z 值
-    vertical_y = y[vertical_mask]
+    # # 提取垂直方向的 z 值（即 x == x2 的所有点）
+    # vertical_mask = (x == x2)
+    # vertical_z = z[vertical_mask]  # 垂直方向对应的 z 值
+    # vertical_y = y[vertical_mask]
+    # 创建二维坐标数组
+    points = np.column_stack((x, y))  # 将 x 和 y 组合成二维坐标点
+
+    # 构建 KD 树
+    tree = cKDTree(points)
+
+    # 定义要查询的中心点
+    x2 = np.mean(x)
+    y2 = np.mean(y)
+
+    # 查询水平和垂直方向上最近的点
+    dist_horizontal, idx_horizontal = tree.query([x2, y2], k=1)  # 最近的点，返回距离和索引
+
+    # 查找水平方向 (y == y2) 最近的点
+    horizontal_idx = tree.query_ball_point([x2, y2], r=1)  # 设置半径 r 作为容差
+    horizontal_z = z[horizontal_idx]
+    horizontal_x = x[horizontal_idx]
+
+    # 查找垂直方向 (x == x2) 最近的点
+    vertical_idx = tree.query_ball_point([x2, y2], r=1)  # 同样的容差
+    vertical_z = z[vertical_idx]
+    vertical_y = y[vertical_idx]
+
+    # 检查水平和垂直方向是否找到点
+    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
+
     
-    top_x, top_y = x2, max(vertical_y)
-    right_x, right_y = max(horizontal_x), y2
+    #top_x, top_y = x2, max(vertical_y)
+    #right_x, right_y = max(horizontal_x), y2
     #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)
@@ -376,7 +424,7 @@ def get_eval_result(pointcloud_path, json_path, theta_notch):
         }
         #print(result_data)
         json.dump(result_data, file, indent=4)
-    debug = False
+
     if debug:
         plt.scatter(x, y, c=z, cmap='viridis')
         plt.colorbar()

src/phase.py

@@ -73,7 +73,6 @@ def compute_phase(img_data: np.ndarray, mask: np.ndarray) -> np.ndarray:
         np.ndarray: 计算得到的相位数组
     """
     I1, I2, I3, I4 = [np.ma.masked_where(mask == 0, img_data[:, :, i]) for i in range(4)]
-
     phase = np.arctan2(I3 - I1, I4 - I2) + np.pi
     return phase.filled(fill_value=np.nan)
 
@@ -158,11 +157,11 @@ def unwrap_phase(phases, save_path, num_freq: int = 2, k: int = 32, debug: bool
 
         plt.imshow(high_freq_y_phase)
         plt.title("high_freq_x_phase")
-        plt.savefig(os.path.join(save_path, "high_freq_x_phase.png"))
+        #plt.savefig(os.path.join(save_path, "high_freq_x_phase.png"))
         plt.show()
         plt.imshow(high_freq_x_phase)
         plt.title("high_freq_y_phase")
-        plt.savefig(os.path.join(save_path, "high_freq_y_phase.png"))
+        #plt.savefig(os.path.join(save_path, "high_freq_y_phase.png"))
         plt.show()
 
         x, y = np.meshgrid(np.arange(high_freq_x_phase.shape[1]), np.arange(high_freq_x_phase.shape[0]))
@@ -185,7 +184,7 @@ def unwrap_phase(phases, save_path, num_freq: int = 2, k: int = 32, debug: bool
 
         # 调整子图之间的间距
         plt.tight_layout()
-        plt.savefig(os.path.join(save_path, "phase_unwrapped.png"))
+        #plt.savefig(os.path.join(save_path, "phase_unwrapped.png"))
         plt.show()
 
     return y_phase_unwrapped, x_phase_unwrapped

src/recons.py

@@ -12,6 +12,10 @@ import plotly.io as pio
 from scipy.spatial import Delaunay
 from src.pcl_postproc import smooth_pcl, align2ref
 from src.eval import fit_plane, remove_noise
+from scipy.ndimage import gaussian_filter
+from scipy.optimize import least_squares
+import scipy.sparse as sp
+
 
 def save_scatter_plot(x, y, z):
  
@@ -45,39 +49,110 @@ def save_scatter_plot(x, y, z):
     # 保存为HTML文件，可以在浏览器中查看
     pio.write_html(fig, file='interactive_3d_scatter.html', auto_open=True)
 
+
+
+
+
+# def build_laplacian_matrix(points, triangles):
+#     N = len(points)
+#     A = lil_matrix((N, N))
+#     for tri in triangles:
+#         for i in range(3):
+#             A[tri[i], tri[i]] += 3  # 每个顶点累加对角线上的值
+#             for j in range(3):
+#                 if i != j:
+#                     A[tri[i], tri[j]] -= 1  # 对应三角形顶点之间建立连接
+#     return A.tocsr()
+
+# def poisson_recons(x, y, gx, gy, debug=False):
+
+#     n = len(x)
+#     # theta = np.linspace(0, 2 * np.pi, n, endpoint=False)
+#     points = np.vstack((x, y)).T
+#     # Delaunay triangulation
+#     tri = Delaunay(points)
+#     triangles = tri.simplices
+
+#     # Build Laplacian matrix based on Delaunay triangulation
+#     L = build_laplacian_matrix(points, triangles)
+
+#     # Calculate divergence from synthetic gradients
+#     div_g = np.zeros(n)
+#     for i in range(n):
+#         div_g[i] = gx[i] + gy[i]  # Simple approximation
+
+#     # Solve the Poisson equation using the sparse matrix
+#     z_reconstructed = spsolve(L, div_g)
+
+#     # Color mapping based on the mean value of the z-coordinates at the vertices of each triangle
+#     # facecolors = z_reconstructed[triangles].mean(axis=1)
+#     return z_reconstructed
+
+
+# def least_squares_recons(x, y, gx, gy):
+#     """
+#     使用最小二乘法从梯度场重建 z 值。
+#     """
+#     def residuals(z, x, y, gx, gy):
+#         # 计算梯度的差值作为残差，确保形状一致
+#         dzdx = np.diff(z) / np.nan_to_num(np.diff(x), nan=1e-6, posinf=1e6, neginf=-1e6)
+#         dzdy = np.diff(z) / np.nan_to_num(np.diff(y), nan=1e-6, posinf=1e6, neginf=-1e6)
+
+#         return np.concatenate((dzdx - gx[:-1], dzdy - gy[:-1]))
+
+#     # 初始估计
+#     z_init = np.zeros(len(x))
+#     print('residuals =',residuals)
+
+#     # 使用最小二乘法优化
+#     res = least_squares(residuals, z_init, args=(x, y, gx, gy))
+
+#     return res.x
+
+
+
 def build_laplacian_matrix(points, triangles):
-    N = len(points)
-    A = lil_matrix((N, N))
+    """
+    根据 Delaunay 三角剖分构建 Laplacian 矩阵
+    points: 点的坐标
+    triangles: 三角剖分后的三角形
+    """
+    n = len(points)
+    L = sp.lil_matrix((n, n))
+    # 遍历每个三角形
     for tri in triangles:
         for i in range(3):
-            A[tri[i], tri[i]] += 3  # 每个顶点累加对角线上的值
-            for j in range(3):
-                if i != j:
-                    A[tri[i], tri[j]] -= 1  # 对应三角形顶点之间建立连接
-    return A.tocsr()
+            for j in range(i + 1, 3):
+                vi, vj = tri[i], tri[j]
+                # Laplacian 矩阵的值 (权重)
+                L[vi, vj] = -1
+                L[vj, vi] = -1
+        for i in range(3):
+            L[tri[i], tri[i]] += 2  # 对角线元素
+    
+    return L.tocsc()
 
-def poisson_recons(x, y, gx, gy, debug=False):
 
+def poisson_recons(x, y, gx, gy, debug=False, smooth_sigma=1):
     n = len(x)
-    # theta = np.linspace(0, 2 * np.pi, n, endpoint=False)
     points = np.vstack((x, y)).T
-    # Delaunay triangulation
     tri = Delaunay(points)
     triangles = tri.simplices
 
-    # Build Laplacian matrix based on Delaunay triangulation
+    # 构建 Laplacian 矩阵
     L = build_laplacian_matrix(points, triangles)
 
-    # Calculate divergence from synthetic gradients
-    div_g = np.zeros(n)
-    for i in range(n):
-        div_g[i] = gx[i] + gy[i]  # Simple approximation
+    # 对梯度场进行高斯平滑，减少边缘噪声
+    if smooth_sigma > 0:
+        gx = gaussian_filter(gx, sigma=smooth_sigma)
+        gy = gaussian_filter(gy, sigma=smooth_sigma)
+
+    # 计算散度 (divergence)
+    div_g = gx + gy
 
-    # Solve the Poisson equation using the sparse matrix
+    # 求解泊松方程
     z_reconstructed = spsolve(L, div_g)
 
-    # Color mapping based on the mean value of the z-coordinates at the vertices of each triangle
-    # facecolors = z_reconstructed[triangles].mean(axis=1)
     return z_reconstructed
 
 def reconstruction_cumsum(world_points, camera_points, screen_points, save_path, cam_id, debug=False, smooth=False, align=True, denoise=True):
@@ -137,12 +212,13 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
     # import pdb; pdb.set_trace()
         z = np.nan_to_num(z, nan=0.0)
     else:
-        z = poisson_recons(x, y, gradient_x, gradient_y)
+        z_raw = poisson_recons(x, y, gradient_x, gradient_y)
+       # z_raw = least_squares_recons(x, y, gradient_x, gradient_y)
         # import pdb; pdb.set_trace()
     #print('-- point cloud: mean - {}; std - {}'.format(np.nanmean(z), np.nanstd(z)))
     if smooth:
         #print("-- point cloud is being smoothed.")
-        smoothed_points = smooth_pcl(np.column_stack((x, y, z)))
+        smoothed_points = smooth_pcl(np.column_stack((x, y, z_raw)))
         #print('-- nanmean after smoothing: mean - {}; std - {}'.format(np.nanmean(smoothed_points[:, 2]), np.nanstd(smoothed_points[:, 2])))
     if align:
         points_aligned = align2ref(smoothed_points)
@@ -245,4 +321,4 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, save_path,
         plt.close()
 
     # return points_aligned[:,2]
-    return z, smoothed_points[:,2], points_aligned[:,2], points_denoise
+    return z_raw, smoothed_points[:,2], points_aligned[:,2], points_denoise

src/utils.py

@@ -6,6 +6,15 @@ import scipy.io as sio
 import matplotlib.pyplot as plt
 from scipy.interpolate import griddata
 import glob
+from scipy import stats
+from scipy.optimize import minimize
+from collections import defaultdict
+from scipy.signal import savgol_filter
+from scipy.ndimage import uniform_filter, gaussian_filter
+import json
+import pickle
+from aprilgrid import Detector
+
 
 
 def read_and_undis(img_path, mtx, dist):
@@ -127,6 +136,7 @@ def fit_sector(contour_points, grid_spacing):
     # 生成 xy 平面上的网格点
     xx, yy = np.meshgrid(np.arange(x, x + w, grid_spacing), np.arange(y, y + h, grid_spacing))
     grid_points_xy = np.vstack([xx.ravel(), yy.ravel()]).T
+    
 
     # 过滤出轮廓内部的点，并记录它们的索引
     inside_points_xy = []
@@ -191,7 +201,7 @@ def get_meshgrid_contour(binary, grid_spacing, save_path, debug=False):
     # image = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE)
     # binary = binarize(image)
     # Detect contours
-    kernel = np.ones((5, 5), np.uint8)
+    kernel = np.ones((1, 1), np.uint8)
 
     # 应用腐蚀操作
     erosion = cv2.erode(binary, kernel, iterations=1)
@@ -201,11 +211,14 @@ def get_meshgrid_contour(binary, grid_spacing, save_path, debug=False):
     max_index, max_contour = max(enumerate(contours), key=lambda x: cv2.boundingRect(x[1])[2] * cv2.boundingRect(x[1])[3])
     # max_index, max_contour1 = max(enumerate(contours1), key=lambda x: cv2.boundingRect(x[1])[2] * cv2.boundingRect(x[1])[3])
 
-    # output_image = cv2.cvtColor(erosion, cv2.COLOR_GRAY2BGR)  # 转换为彩色图像用于绘制
-    # cv2.drawContours(output_image, [max_contour1], -1, (255, 255, 0), thickness=2)
-    # cv2.namedWindow('output_image', 0)
-    # cv2.imshow('output_image', output_image)
-    # cv2.waitKey(0)
+    output_image = cv2.cvtColor(erosion, cv2.COLOR_GRAY2BGR)  # 转换为彩色图像用于绘制
+
+    if debug:
+        cv2.drawContours(output_image, [max_contour], -1, (255, 255, 0), thickness=2)
+        cv2.namedWindow('output_image', 0)
+        cv2.imshow('output_image', output_image)
+        cv2.waitKey(0)
+        cv2.destroyAllWindows()
     #print('max_contour = ',max_contour[0])
     max_contour_lst = []
     for pt in max_contour:
@@ -391,7 +404,7 @@ def affine_img2world(grid_points, A, Rc2w, Tc2w, d):
     # return [x_w_data, y_w_data, z_w_data]
     return intersection_points_world
 
-def get_world_points(contour_points, world_mat_contents, grid_spacing, d, save_path, debug=False):
+def get_world_points(contour_points, world_mat_contents, cam_id, grid_spacing, d, save_path, debug=False):
     
     A = world_mat_contents['camera_matrix']
     Rw2c = world_mat_contents['rotation_matrix']
@@ -424,7 +437,7 @@ def get_world_points(contour_points, world_mat_contents, grid_spacing, d, save_p
 
         # 调整子图之间的间距
         plt.tight_layout()
-        plt.savefig(os.path.join(save_path, "world_points.png"))
+        plt.savefig(os.path.join(save_path, str(cam_id) + "_world_points.png"))
         # 显示图像
         plt.show()
     # import pdb; pdb.set_trace()
@@ -545,7 +558,10 @@ def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_p
         u_now = point_data['u_p'][idx]
         v_now = point_data['v_p'][idx]
         u_now, v_now = int(u_now), int(v_now)
-       
+        #print('x_phase_unwrapped shape = ',x_phase_unwrapped.shape, y_phase_unwrapped.shape)
+       # print('v_now, u_now = ', v_now, u_now)
+        # if u_now > x_phase_unwrapped.shape[1]-1:
+        #     u_now = x_phase_unwrapped.shape[1] - 2
         phase_x_now = x_phase_unwrapped[v_now, u_now]  # 注意这里的索引要按照Python的行列顺序，与MATLAB相反 raw
         phase_y_now = y_phase_unwrapped[v_now, u_now]
 
@@ -597,10 +613,23 @@ def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_p
 def get_white_mask(white_path, bin_thresh=12):  
     gray = cv2.imread(white_path, cv2.COLOR_BGR2GRAY)
     mask = np.zeros_like(gray)
+    
     _,  thresh_img = cv2.threshold(gray, bin_thresh, 255, cv2.THRESH_BINARY)
-    contours, _ = cv2.findContours(thresh_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-    max_index, max_contour = max(enumerate(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)
+    kernel = np.ones((8, 1), np.uint8)  # 垂直核大小为 5x1，表示垂直方向腐蚀
+    erode_thresh_img = cv2.erode(thresh_img, kernel, iterations=1)
+    img_height, img_width = erode_thresh_img.shape[:2]
+    contours, _ = cv2.findContours(erode_thresh_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    valid_contours = [
+        contour for contour in contours 
+        if cv2.boundingRect(contour)[2] < 0.8*img_width and cv2.boundingRect(contour)[3] < 0.8 * img_height
+    ]
+    # 确保有有效的轮廓
+    if valid_contours:
+        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)
+    # cv2.namedWindow('mask',0)
+    # cv2.imshow('mask', mask)
+    # cv2.waitKey(0)
     return mask
 
 # def get_world_points_from_mask(binary_image, world_mat_contents, d, save_path, debug=False):
@@ -674,9 +703,125 @@ def transform_point_cloud(point_cloud, rotation_matrix, translation_vector):
     translated_point_cloud = rotated_point_cloud + translation_vector
     return translated_point_cloud
 
+# 定义拟合圆的函数
+def calc_radius(x, y, xc, yc):
+    """计算每个点到圆心的距离"""
+    return np.sqrt((x - xc)**2 + (y - yc)**2)
+
+def fit_circle_post(x, y):
+    """拟合一个圆并返回圆心和半径"""
+    # 初始猜测圆心为数据的平均值
+    x_m = np.mean(x)
+    y_m = np.mean(y)
+    
+    def optimize_func(params):
+        xc, yc = params
+        radii = calc_radius(x, y, xc, yc)
+        return np.var(radii)  # 最小化半径的方差，使得所有点接近正圆
+    
+    result = minimize(optimize_func, [x_m, y_m])
+    xc, yc = result.x
+    radii = calc_radius(x, y, xc, yc)
+    radius_mean = np.mean(radii)
+    return xc, yc, radius_mean
+
+
+def remove_repeat_z(total_cloud_point):
+    # 计算所有 z 的全局均值
+    global_mean_z = np.mean(total_cloud_point[:, 2])
+
+    # 使用字典来记录每个 (x, y) 对应的 z 值
+    xy_dict = defaultdict(list)
+
+    # 将数据分组存入字典，键为 (x, y)，值为所有对应的 z 值
+    for x_val, y_val, z_val in total_cloud_point:
+        xy_dict[(x_val, y_val)].append(z_val)
+
+    # 构建新的点云数据
+    new_data = []
+
+    # 对每个 (x, y)，选择距离全局均值最近的 z 值
+    for (x_val, y_val), z_vals in xy_dict.items():
+        if len(z_vals) > 1:
+            # 多个 z 值时，选择与全局均值最接近的 z
+            closest_z = min(z_vals, key=lambda z: abs(z - global_mean_z))
+        else:
+            # 只有一个 z 值时，直接使用
+            closest_z = z_vals[0]
+        
+        # 将结果保存为新的点
+        new_data.append([x_val, y_val, closest_z])
 
+    # 将新点云数据转化为 NumPy 数组
+    new_data = np.array(new_data)
+    return new_data
 
 
+def make_circle(data):
+    
+    # 计算所有 z 的全局均值
+    global_mean_z = np.mean(data[:, 2])
+
+    # Step 1: 确定最小外接圆
+    # 使用 xy 坐标计算最小外接圆
+    xy_vals = data[:, :2].astype(np.float32)
+    (x_center, y_center), radius = cv2.minEnclosingCircle(xy_vals)
+
+    # 获取最小外接圆的中心和半径
+    x_center, y_center, radius = int(x_center), int(y_center), int(radius)
+
+    # Step 2: 使用 IQR 进行异常 z 值检测和替换
+    # 计算 z 的四分位数
+    Q1 = np.percentile(data[:, 2], 25)
+    Q3 = np.percentile(data[:, 2], 75)
+    IQR = Q3 - Q1
+    # 将异常的 z 值替换为均值
+    #data[:, 2] = np.where((data[:, 2] < lower_bound) | (data[:, 2] > upper_bound), global_mean_z, data[:, 2])
+
+    # Step 3: 填补最小外接圆内的空缺点
+    # 创建一个用于存储已存在的 (x, y) 坐标的集合
+    existing_xy_set = set((int(x), int(y)) for x, y in data[:, :2])
+
+    # 遍历最小外接圆范围内的所有 (x, y) 坐标
+    filled_data = []
+    for x in range(x_center - radius, x_center + radius + 1):
+        for y in range(y_center - radius, y_center + radius + 1):
+            # 判断 (x, y) 是否在圆内
+            if np.sqrt((x - x_center)**2 + (y - y_center)**2) <= radius:
+                if (x, y) in existing_xy_set:
+                    # 如果 (x, y) 已存在，则保留原始数据
+                    z_val = data[(data[:, 0] == x) & (data[:, 1] == y), 2][0]
+                else:
+                    # 如果 (x, y) 不存在，填充 z 的全局均值
+                    z_val = global_mean_z
+                filled_data.append([x, y, z_val])
+
+    # 转换为 NumPy 数组
+    filled_data = np.array(filled_data)
+    return filled_data
+   
+def point_filter(data):
+    data[:, 0] = np.round(data[:, 0]).astype(int)
+    data[:, 1] = np.round(data[:, 1]).astype(int)
+    # 构建平面网格，假设 grid_x 和 grid_y 已经生成
+    grid_size = 100  # 网格大小根据你的数据情况设定
+    grid_x, grid_y = np.meshgrid(np.linspace(min(data[:, 0]), max(data[:, 0]), grid_size),
+                                np.linspace(min(data[:, 1]), max(data[:, 1]), grid_size))
+
+    # 插值，将 (x, y, z) 数据转换为平面网格
+    grid_z = griddata((data[:, 0], data[:, 1]), data[:, 2], (grid_x, grid_y), method='linear')
+    print('grid_x = ', grid_x)
+    print('grid_y = ', grid_y)
+    print('grid_z = ', grid_z)
+    smoothed_z = uniform_filter(grid_z, size=5)
+    print('smoothed_z range = ',np.max(smoothed_z) - np.min(smoothed_z))
+
+    fig = plt.figure(figsize=(10, 8))
+    ax = fig.add_subplot(111, projection='3d')
+    ax.plot_surface(grid_x, grid_y, smoothed_z, cmap='viridis', alpha=0.7)
+    plt.show()
+    return smoothed_z
+
 def post_process(txt_path, debug):
     #txt_path = 'D:\\file\\20240913-data\\20240913105139948\\'
     total_cloud_point = np.empty((0, 3))
@@ -728,13 +873,29 @@ def post_process(txt_path, debug):
         # print('i = min y = ',i , np.min(data[i][:, 1]))
         data[i][:, 2] = data[i][:, 2] - np.mean(data[i][:, 2])
         total_cloud_point = np.vstack([total_cloud_point, np.column_stack((data[i][:, 0], data[i][:, 1], data[i][:, 2]))])
+  
+    # 将 xy 坐标四舍五入为整数
+    total_cloud_point[:, 0] = np.round(total_cloud_point[:, 0]).astype(int)
+    total_cloud_point[:, 1] = np.round(total_cloud_point[:, 1]).astype(int)
+
+    new_data = remove_repeat_z(total_cloud_point)
+    print('new_data range = ',np.max(new_data[:, 2]) - np.min(new_data[:, 2]))
+    filter_data = make_circle(new_data)
+    print('filter_data range = ',np.max(filter_data[:, 2]) - np.min(filter_data[:, 2]))
+    filter_data[:, 2] = np.where(filter_data[:, 2] > 4, 4, filter_data[:, 2])
+    filter_data[:, 2] = np.where(filter_data[:, 2] < -2, -2, filter_data[:, 2])
+    #print('filter_data = ', filter_data)
+    #smoothed_data = point_filter(filter_data)
+    
+
+
     if debug:
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
         # 提取 x, y, z 坐标
-        x_vals = total_cloud_point[:, 0]
-        y_vals = total_cloud_point[:, 1]
-        z_vals = total_cloud_point[:, 2]
+        x_vals = filter_data[:,0]
+        y_vals = filter_data[:,1]
+        z_vals = filter_data[:,2]
 
         # 绘制3D点云
         ax.scatter(x_vals, y_vals, z_vals, c=z_vals, cmap='viridis', marker='o')
@@ -744,5 +905,185 @@ def post_process(txt_path, debug):
         ax.set_ylabel('Y (mm)')
         ax.set_zlabel('Z (um)')
         ax.set_title('3D Point Cloud Visualization')
+        plt.show()
+    return filter_data
+
+
+
+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 transform_image_to_cam0(img, K_cam_i, R_cam_i, T_cam_i, K_cam0, R_cam0, T_cam0, depth_map):
+    """
+    将相机 i 的图像转换到相机 0 的坐标系下，并重新投影到 cam0 的图像平面
+    img: 相机 i 拍摄的图像 (H, W, 3)
+    K_cam_i: 相机 i 的内参矩阵 (3, 3)
+    R_cam_i: 相机 i 的旋转矩阵 (3, 3)
+    T_cam_i: 相机 i 的平移向量 (3,)
+    K_cam0: 相机 cam0 的内参矩阵 (3, 3)
+    R_cam0: 相机 cam0 的旋转矩阵 (3, 3)
+    T_cam0: 相机 cam0 的平移向量 (3,)
+    depth_map: 相机 i 的深度图 (H, W) 或由立体视觉计算得到
+    返回：对齐到 cam0 的图像
+    """
+    H, W = img.shape[:2]
+    
+    # 生成像素坐标网格
+    u, v = np.meshgrid(np.arange(W), np.arange(H))
+    
+    # 将图像坐标转换为齐次坐标
+    img_pts = np.stack([u.ravel(), v.ravel(), np.ones_like(u).ravel()], axis=1).T  # (3, N)
+
+    # 反投影到三维点，计算相机 i 坐标系下的三维点
+    depth_vals = depth_map.ravel()
+    X_cam_i = np.linalg.inv(K_cam_i) @ (img_pts * depth_vals)  # (3, N)
 
-        plt.show()
+    # 将三维点从相机 i 坐标系转换到 cam0 坐标系
+    R_inv_cam_i = np.linalg.inv(R_cam_i)
+    X_cam0 = R_cam0 @ R_inv_cam_i @ (X_cam_i - T_cam_i.reshape(3, 1)) + T_cam0.reshape(3, 1)
+
+    # 将三维点投影回 cam0 图像平面
+    img_pts_cam0 = K_cam0 @ X_cam0
+    img_pts_cam0 /= img_pts_cam0[2, :]  # 归一化
+    
+    # 获取重新投影后的像素位置
+    u_cam0 = img_pts_cam0[0, :].reshape(H, W).astype(np.int32)
+    v_cam0 = img_pts_cam0[1, :].reshape(H, W).astype(np.int32)
+    
+    # 创建输出图像
+    output_img = np.zeros_like(img)
+    
+    # 将原图像插值到新的像素位置
+    mask = (u_cam0 >= 0) & (u_cam0 < W) & (v_cam0 >= 0) & (v_cam0 < H)
+    output_img[v_cam0[mask], u_cam0[mask]] = img[v[mask], u[mask]]
+
+    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()
+
+
+
+                        
+if __name__ == '__main__':
+    camera_calibrate_vis()
+    
+
+