pmd-python/src/utils.py

import cv2
import os
from tqdm import tqdm
import numpy as np
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, median_filter
import json
import pickle
import shutil
import pandas as pd
from sklearn.linear_model import LinearRegression
from scipy.interpolate import Rbf
from numpy.linalg import lstsq
from scipy.optimize import curve_fit





def read_and_undis(img_path, mtx, dist):

    img = cv2.imread(img_path)
    dst = cv2.undistort(img, mtx, dist, None, mtx)
    gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
    return gray

def write_point_cloud(file_path, x, y, z):
    # 将 x, y, z 列组合成一个二维数组
    point_cloud = np.column_stack((x, y, z))
    # import pdb; pdb.set_trace()
    # 将数组写入文本文件，使用空格作为分隔符
    np.savetxt(file_path, point_cloud, fmt='%.10f', delimiter=',')
    print(f"Point cloud saved to {file_path}")

def get_baseline_params(filename):
    world_raw = sio.loadmat(filename)
    return world_raw

def binarize(image, threshold=85):
    # Read and binarize the image
    # image = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE)
    _, binary_image = cv2.threshold(image, threshold, 255, cv2.THRESH_BINARY)
    num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(binary_image, connectivity=8)

    # We assume that the largest connected component is the object of interest (the large circle)
    # Identify the label with the largest area, ignoring the background label (label 0)
    largest_label = 1 + np.argmax(stats[1:, cv2.CC_STAT_AREA])

    # Create a mask for the largest connected component
    filtered_image = np.zeros_like(binary_image)
    filtered_image[labels == largest_label] = 255

    return filtered_image

def get_ellipse_points(binary, contours, grid_spacing, ellipse_center, ellipse_axes, angle_rad):
    image_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR)
    grid_points = []
    
    h, w = binary.shape[:2]
    y_indices, x_indices = np.mgrid[0:h:grid_spacing, 0:w:grid_spacing]

    # 将坐标转换为椭圆坐标系
    x_shifted = x_indices - ellipse_center[0]
    y_shifted = y_indices - ellipse_center[1]

    # 旋转坐标系
    cos_angle = np.cos(angle_rad)
    sin_angle = np.sin(angle_rad)
    x_rotated = x_shifted * cos_angle + y_shifted * sin_angle
    y_rotated = -x_shifted * sin_angle + y_shifted * cos_angle

    # 判断每个点是否在椭圆内部或边界上
    ellipse_eq = (x_rotated**2 / ellipse_axes[0]**2) + (y_rotated**2 / ellipse_axes[1]**2)
    inside_ellipse = ellipse_eq < 1

    # 记录并绘制在椭圆内部或边界上的点
    grid_points = []
    contour_points = []

    count = 0
    for i in range(len(contours)):
        for point in contours[i]:
            # import pdb; pdb.set_trace()
            count += 1
            assert point.shape == (1, 2), point.shape
            if count == 400:
                cv2.circle(image_color, point[0], 1, (0, 255, 0), -1)
                cv2.putText(image_color, str(point[0]), (point[0][0], point[0][1]), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 255, 0), 2)
            else:
                cv2.circle(image_color, point[0], 1, (0, 0, 255), -1)

            contour_points.append(point[0].tolist())
    
    return grid_points, contour_points, image_color

def fit_circle(contour_points, grid_spacing):
    z_contour_points = contour_points[:, 2]
    points_float = np.array(contour_points[:, :2], dtype=np.float32)
    # import pdb; pdb.set_trace()
    (center, radius) = cv2.minEnclosingCircle(points_float)

    print(f"Fitted Circle Center: {center}")
    print(f"Fitted Circle Radius: {radius}")

    # Create a grid of points that covers the bounding box of the circle
    x_min = int(center[0] - radius)
    x_max = int(center[0] + radius)
    y_min = int(center[1] - radius)
    y_max = int(center[1] + radius)
    # Generate grid points within the bounding box
    x_vals = np.arange(x_min, x_max, grid_spacing)
    y_vals = np.arange(y_min, y_max, grid_spacing)
    x_grid, y_grid = np.meshgrid(x_vals, y_vals)
    grid_points = np.vstack([x_grid.ravel(), y_grid.ravel()]).T

    # Calculate the squared distances of the grid points from the center
    distances_squared = (grid_points[:, 0] - center[0])**2 + (grid_points[:, 1] - center[1])**2

    # Filter the points to keep only those within the circle
    inside_circle = distances_squared <= radius**2
    grid_points_inside_circle = grid_points[inside_circle]
    z_col = np.full((len(grid_points_inside_circle), 1), z_contour_points[0])

    return np.column_stack((grid_points_inside_circle, z_col))


def fit_sector(contour_points, grid_spacing, erosion_pixels):
    z_contour_points = contour_points[:, 2]
    # 提取 xy 平面上的轮廓点
    contour_xy = contour_points[:, :2]
    # 确保 contour_xy 是 numpy 数组
    contour_xy = np.array(contour_xy, dtype=np.int32)
    
    # 获取轮廓的边界框
    x, y, w, h = cv2.boundingRect(contour_xy)
    # 生成 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 = []
    inside_indices = []
    for idx, point in enumerate(grid_points_xy):
        # 使用cv2.pointPolygonTest判断点是否在轮廓内部
        if cv2.pointPolygonTest(contour_xy, (float(point[0]), float(point[1])), False) >= 0:
            inside_points_xy.append(point)
            inside_indices.append(idx)

    inside_points_xy = np.array(inside_points_xy)

    # 使用过滤后的索引获得网格点，并为每个点设置 z 值
    grid_points_inside_sector = grid_points_xy[inside_indices]
   
    z_col = np.full((len(grid_points_inside_sector), 1), z_contour_points[0])

    
    # 处理负坐标，将所有坐标平移至非负区域
    min_x, min_y = np.min(contour_xy, axis=0) - 10
    offset = np.array([abs(min(min_x, 0)), abs(min(min_y, 0))])
    shifted_contour = contour_xy + offset  # 平移后的轮廓点

    # 获取轮廓的边界框
    x, y, w, h = cv2.boundingRect(shifted_contour)

    # 创建一幅足够大的二值图像来绘制轮廓
    img = np.zeros((y + h + erosion_pixels * 2, x + w + erosion_pixels * 2), dtype=np.uint8)

    # 绘制原始轮廓到二值图像上
    cv2.drawContours(img, [shifted_contour], -1, (255), thickness=cv2.FILLED)

    # 创建腐蚀核并进行腐蚀操作
    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:
        raise ValueError("腐蚀后没有找到轮廓，请检查腐蚀参数。")
    eroded_contour = contours[0] - offset  # 将腐蚀后的轮廓平移回原始坐标系

    # 生成 xy 平面上的网格点
    xx, yy = np.meshgrid(np.arange(x - erosion_pixels, x + w + erosion_pixels, grid_spacing), 
                         np.arange(y - erosion_pixels, y + h + erosion_pixels, grid_spacing))
    grid_points_xy = np.vstack([xx.ravel(), yy.ravel()]).T - offset  # 还原回原坐标系

    # 过滤出介于原轮廓和新轮廓之间的点
    between_points = []
    for point in grid_points_xy:
        # 使用 cv2.pointPolygonTest 判断点是否在原轮廓和腐蚀后的轮廓之间
        in_original_contour = cv2.pointPolygonTest(contour_xy, (float(point[0]), float(point[1])), False) >= 0
        in_eroded_contour = cv2.pointPolygonTest(eroded_contour, (float(point[0]), float(point[1])), False) >= 0
        if in_original_contour and not in_eroded_contour:
            between_points.append(point)

    between_points = np.array(between_points)

    return np.column_stack((grid_points_inside_sector, z_col)), np.column_stack((between_points, np.full((len(between_points), 1), z_contour_points[0])))


def get_meshgrid_contour(binary, save_path, debug=False):

    contours, _ = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    max_index, max_contour = max(enumerate(contours), key=lambda x: cv2.boundingRect(x[1])[2] * cv2.boundingRect(x[1])[3])
    output_image = cv2.cvtColor(binary, 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:
        #print('[pt[0][1],pt[0][0]] = ', [int(pt[0][1]), int(pt[0][0])])
        #print('pt[0].tolist() =', pt[0].tolist())
        #max_contour_lst.append([int(pt[0][1]), int(pt[0][0])])
        max_contour_lst.append(pt[0].tolist())
    #print('max_contour_lst = ',max_contour_lst)
    max_contour_array = np.array(max_contour_lst)

    return max_contour_array


def get_meshgrid(binary, grid_spacing, save_path, debug=False):

    # img_path = 'imgs/temp/white.bmp'
    # image = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE)
    # binary = binarize(image)

    # Detect contours
    contours, _ = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    # 初始化最大范围的变量
    x_min, y_min = float('inf'), float('inf')
    x_max, y_max = 0, 0

    # 计算所有轮廓的最大范围
    for contour in contours:
        x, y, w, h = cv2.boundingRect(contour)
        x_min = min(x_min, x)
        y_min = min(y_min, y)
        x_max = max(x_max, x + w)
        y_max = max(y_max, y + h)
    # 存储采样点的列表
    sampled_points = []

    # 在最大边界框范围内进行采样，步长为 grid_spacing
    for i in range(x_min, x_max, grid_spacing):
        for j in range(y_min, y_max, grid_spacing):
            # 遍历所有轮廓，检查该点是否在任意轮廓内部
            for contour in contours:
                if cv2.pointPolygonTest(contour, (i, j), False) >= 0:
                    sampled_points.append((i, j))
                    break  # 一旦找到在某个轮廓内，就不需要再检查其他轮廓了
    #print('sampled_points ',sampled_points)
    grid_points = np.array((sampled_points))
    grid_points = grid_points[:, ::-1]
    # 将采样点显示在图像上
    output_image = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR)  # 转换为彩色图像用于绘制
    for point in sampled_points:
        cv2.circle(output_image, point, 2, (0, 0, 255), -1)  # 画小圆点

    # 显示结果
    cv2.namedWindow('Sampled Points', 0)
    cv2.imshow("Sampled Points", output_image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    return grid_points

    # # Initialize ellipse
    # ellipse = None

    # # Check and fit an ellipse
    # if contours:
    #     sorted_contours = sorted(contours, key=cv2.contourArea, reverse=True)
    #     # import pdb; pdb.set_trace()
    #     for contour in sorted_contours:
    #         if len(contour) >= 5:  # Need at least 5 points to fit an ellipse
    #             ellipse = cv2.fitEllipse(contour)
    #             break
    #     else:
    #         print("没有足够的轮廓点来拟合椭圆")
    # else:
    #     print("没有检测到足够的轮廓或轮廓点太少")
    # print('ellipse = ', ellipse)
    # # Continue with your grid generation and other tasks if ellipse was successfully fitted
    # if ellipse:
    #     # Convert binary image to BGR for color drawing
    #     grid_points = []
    #     ellipse_center = ellipse[0]
    #     ellipse_axes = (ellipse[1][0] / 2, ellipse[1][1] / 2)  # Dividing axes by 2 to get semi-axes
    #     angle_rad = np.deg2rad(ellipse[2])
    #     # Coordinate of the grid points
    #     grid_points, contour_points, image_color = get_ellipse_points(binary, contours, grid_spacing, ellipse_center, ellipse_axes, angle_rad)   
    # else:
    #     raise ValueError("未能拟合椭圆")
    # cv2.namedWindow('meshgrid', )
    # cv2.imshow('meshgrid', binary)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

    # if debug:
    #     # print('There are {} points in the ellipse'.format(len(grid_points)))
    #     # print('Mean: {}'.format(np.mean(np.array(grid_points))))
    #     print('There are {} points on the ellipse'.format(len(contour_points)))
    #     print('Mean: {}'.format(np.mean(np.array(contour_points))))
    #     cv2.imwrite(os.path.join(save_path, "meshgrid.png"), image_color)
    #     cv2.imshow('meshgrid', image_color)
    #     cv2.waitKey(0)
    #     cv2.destroyAllWindows()
        
    # # grid_points = np.array(grid_points)
    # contour_points = np.array(contour_points)
    # # print('x:', grid_points[:, 0].max()-grid_points[:, 0].min())
    # # print('y:', grid_points[:, 1].max()-grid_points[:, 1].min())
    # return grid_points, contour_points
    # # return np.fliplr(grid_points), np.fliplr(contour_points)

def affine_img2world(grid_points, A, Rc2w, Tc2w, d):
     # 计算交点
    intersection_points_world = []

    for point in grid_points:
        x_img, y_img = point[0], point[1]
        ray_camera_coordinate = np.array([
            (x_img - A[0, 2]) / A[0, 0],
            (y_img - A[1, 2]) / A[1, 1],
            1
        ])
        ray_camera_coordinate /= np.linalg.norm(ray_camera_coordinate)  # 归一化
        ray_world_coordinate = Rc2w @ ray_camera_coordinate
        cameraCenter_world_coordinate = Tc2w

        # 计算世界坐标系中的交点
        t = (-d - cameraCenter_world_coordinate[2]) / ray_world_coordinate[2]
        intersection_point = cameraCenter_world_coordinate.T + t * ray_world_coordinate
        intersection_points_world.append(intersection_point)

    # 输出世界坐标系中的交点
    intersection_points_world = np.array(intersection_points_world)
    intersection_points_world = intersection_points_world.reshape(-1, 3)

    # x_w_data = np.column_stack((grid_points, intersection_points_world[:, 0]))
    # y_w_data = np.column_stack((grid_points, intersection_points_world[:, 1]))
    # z_w_data = np.column_stack((grid_points, intersection_points_world[:, 2]))

    # return [x_w_data, y_w_data, z_w_data]
    return intersection_points_world

def get_world_points(contour_points, world_mat_contents, cam_id, grid_spacing, d, erosion_pixels, debug=False):
    
    A = world_mat_contents['camera_matrix']
    Rw2c = world_mat_contents['rotation_matrix']
    Rc2w = np.linalg.inv(Rw2c)
    Tw2c = world_mat_contents['translation_vector']
    Tc2w = -np.dot(np.linalg.inv(Rw2c), Tw2c)
    world_points_contours = affine_img2world(contour_points, A, Rc2w, Tc2w, d)
    
    world_points_fit,  world_points_boundary = fit_sector(world_points_contours, grid_spacing, erosion_pixels)
    _,  world_points_boundary_3 = fit_sector(world_points_contours, grid_spacing, 3)
    # assert np.abs(world_contour_points[:, 0].max()-world_contour_points[:, 0].min()-(world_contour_points[:, 1].max()-world_contour_points[:, 1].min())) < 1
    # import pdb; pdb.set_trace()
    if debug:
        # print('world x:', world_points_fit[:, 0].max()-world_points_fit[:, 0].min())
        # print('world y:', world_points_fit[:, 1].max()-world_points_fit[:, 1].min())

        #print('world contour x:', world_contour_points[:, 0].max()-world_contour_points[:, 0].min())
        #print('world contour y:', world_contour_points[:, 1].max()-world_contour_points[:, 1].min())

        plt.figure()
        
        plt.scatter(world_points_fit[:, 0], world_points_fit[:, 1],  color='red', alpha=0.5)
        plt.scatter(world_points_boundary[:, 0], world_points_boundary[:, 1], color='green', alpha=0.5)
        plt.title('world points fit')
        plt.xlabel('X Axis')
        plt.ylabel('Y Axis')

        # 调整子图之间的间距
        plt.tight_layout()
        # 显示图像
        plt.show()
    # import pdb; pdb.set_trace()
    # return np.concatenate((world_points_fit, world_contour_points), axis=0)
    return world_points_fit, world_points_boundary, world_points_boundary_3



def get_camera_points(world_points, world_mat_contents, save_path, cam_id, debug=False):
    
    x_w_data, y_w_data, z_w_data = world_points[:, 0], world_points[:, 1], world_points[:, 2]
    A = world_mat_contents['camera_matrix']
    Tw2c = world_mat_contents['translation_vector']
    Rw2c = world_mat_contents['rotation_matrix']
    Rc2w = np.linalg.inv(Rw2c)
    Tc2w = -np.dot(Rc2w, Tw2c)

    # 初始化数据数组
    x_c_array, y_c_array, z_c_array, u_p_array, v_p_array = [], [], [], [], []

    # 确保x_w, y_w, z_w具有相同形状
    assert x_w_data.shape == y_w_data.shape == z_w_data.shape
    #print(x_w_data.shape)
    for i in range(x_w_data.shape[0]):
        x_w, y_w, z_w = x_w_data[i], y_w_data[i], z_w_data[i]

        world_point_now = np.array([x_w, y_w, z_w])
        picture_point_now = A @ (Rw2c @ world_point_now[:, np.newaxis] + Tw2c)
        picture_point_now /= picture_point_now[2]

        u_p = np.round(picture_point_now[0]).item()  # 使用.item()将单元素数组转换为标量
        v_p = np.round(picture_point_now[1]).item()

        # 计算相机坐标
        x_c, y_c, z_c = Tc2w.ravel()
        x_c_array.append(x_c)
        y_c_array.append(y_c)
        z_c_array.append(z_c)
        u_p_array.append(u_p)
        v_p_array.append(v_p)
    
    x_c_array, y_c_array, z_c_array, u_p_array, v_p_array = np.array(x_c_array), np.array(y_c_array), np.array(z_c_array), np.array(u_p_array), np.array(v_p_array)
    camera_points = np.column_stack((x_c_array, y_c_array, z_c_array))

    if debug:
        print('u_p max min:', u_p_array.max(),u_p_array.min())
        print('v_p max min:', v_p_array.max(), v_p_array.min())
        print('u_p:', u_p_array.max()-u_p_array.min())
        print('v_p:', v_p_array.max()-v_p_array.min())

        plt.figure()
        plt.scatter(u_p_array, v_p_array, alpha=0.5)
        plt.title('camera points')
        plt.xlabel('X Axis')
        plt.ylabel('Y Axis')

        # 调整子图之间的间距
        plt.tight_layout()
        #plt.savefig(os.path.join(save_path, str(cam_id) + "_camera_points.png"))

        # 显示图像
        plt.show()

    return camera_points, u_p_array, v_p_array

def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_params, screen_to_world, cfg, save_path, cam_id, debug=False):
    
    # x_phase_unwrapped = x_phase_unwrapped.T
    # y_phase_unwrapped = y_phase_unwrapped.T
    # import pdb; pdb.set_trace()

    screenPixelSize = np.array(cfg['screenPixelSize'])
    Axis_conversion = np.array(cfg['Axis_conversion'][cam_id])
    screenorigin_point = np.array(cfg['screenorigin_point'])

    # 提取旋转矩阵和平移向量
    Rs2w = screen_to_world['screen_to_world_rotation']
    Ts2w = screen_to_world['screen_to_world_translation']

    # 计算 x_phase_unwrapped 中所有非 NaN 值的平均值
    average_x_phase_unwrapped = np.nanmean(x_phase_unwrapped)
    average_y_phase_unwrapped = np.nanmean(y_phase_unwrapped)

    # import pdb; pdb.set_trace()
    # 计算 value_map 并找到最小值
    value_map = np.abs(x_phase_unwrapped - average_x_phase_unwrapped) + np.abs(y_phase_unwrapped - average_y_phase_unwrapped)
    min_index = np.unravel_index(np.nanargmin(value_map), value_map.shape)
    
    # import pdb; pdb.set_trace()
    # 选定一个中心相位值作为绝对相位值
    # abs_phasepoint_picture = np.array(min_index)
    abs_phasepoint_x = x_phase_unwrapped[min_index]
    abs_phasepoint_y = y_phase_unwrapped[min_index]
    abs_phasepoint = np.array([abs_phasepoint_x, abs_phasepoint_y])
    # print('----------------------------------------')
    # print("abs_phasepoint", np.mean(np.isnan(abs_phasepoint)))
    # 根据相位值求解在屏幕上的像素点位置
    abs_phasepoint_screen = (abs_phasepoint / (2 * np.pi)) * (screenPixelSize/ cfg['k'])
    # print("abs_phasepoint_screen", np.mean(np.isnan(abs_phasepoint_screen)))

    # 利用相对位置信息确定绝对相位点的世界坐标系位置
    arrow_abs_mm = np.array([Axis_conversion[0] * cfg['pixelSize_mm'] * (abs_phasepoint_screen[0] - screenorigin_point[0]),
                            Axis_conversion[1] * cfg['pixelSize_mm'] * (abs_phasepoint_screen[1] - screenorigin_point[1]),
                            0])
    abs_phasepoint_world = np.dot(Rs2w, arrow_abs_mm) + Ts2w.T
    
    # print('abs_phasepoint 绝对相位值:')
    # print(abs_phasepoint)
    # print('abs_phasepoint_world 绝对相位值对应的世界坐标系的位置:')
    # print(abs_phasepoint_world)

    # print('----------------------------------------')

    x_data, y_data, z_data = [], [], []

    for idx in range(len(point_data['u_p'])):
        # i, j, u_now = point_data['u_p'][idx]
        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)
      
        phase_x_now = x_phase_unwrapped[v_now, u_now]  # 注意这里的索引要按照Python的行列顺序，与MATLAB相反 raw
        phase_y_now = y_phase_unwrapped[v_now, u_now]

        #phase_x_now = x_phase_unwrapped[u_now, v_now]  
        #phase_y_now = y_phase_unwrapped[u_now, v_now]

        # import pdb; pdb.set_trace()
        derta_phase_x = phase_x_now - abs_phasepoint[0]
        derta_phase_y = phase_y_now - abs_phasepoint[1]
        #assert not np.isnan(derta_phase_x).any(), "derta_phase_x"
        #assert not np.isnan(derta_phase_y).any(), "derta_phase_y"
        derta_mm = (np.array([derta_phase_x, derta_phase_y]) / (2 * np.pi)) * (screenPixelSize / cfg['k']) * cfg['pixelSize_mm'] * Axis_conversion
        #assert not np.isnan(derta_mm).any(), "derta_mm"
        arrow_mm = np.array([derta_mm[0], derta_mm[1], 0]) + arrow_abs_mm
        #assert not np.isnan(arrow_mm).any(), "arrow_mm"
        screen_point_now = Rs2w.dot(arrow_mm) + Ts2w.T
        # import pdb; pdb.set_trace()

        x_data.append([screen_point_now[0]])
        y_data.append([screen_point_now[1]])
        z_data.append([screen_point_now[2]])

    x_data = np.array(x_data)
    y_data = np.array(y_data)
    z_data = np.array(z_data)   
    screen_points = np.column_stack((x_data, y_data, z_data))

    if debug:
        print(" "*100)
        print('*'*100)
        print('screen x:', np.nanmax(x_data), np.nanmin(x_data), np.nanmax(x_data) - np.nanmin(x_data))
        print('screen y:', np.nanmax(y_data), np.nanmin(y_data), np.nanmax(y_data)-np.nanmin(y_data))
        print('screen z:', np.nanmax(z_data), np.nanmin(z_data), np.nanmax(z_data)-np.nanmin(z_data))
        print('*'*100)
        plt.figure()
        plt.scatter(x_data, y_data, alpha=0.5)
        plt.title('screen points')
        plt.xlabel('X Axis')
        plt.ylabel('Y Axis')

        # 调整子图之间的间距
        plt.tight_layout()
        plt.savefig(os.path.join(save_path, str(cam_id) + "_screen_points.png"))
        # 显示图像
        plt.show()
    # import pdb; pdb.set_trace()
    return screen_points

def get_white_mask(white_path, bin_thresh=15, debug=False):  
    gray = cv2.imread(white_path, 0)
   
    mask = np.zeros_like(gray)
    
    _,  thresh_img = cv2.threshold(gray, bin_thresh, 255, cv2.THRESH_BINARY)
    kernel = np.ones((8, 2), 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)
    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)
        cv2.waitKey(0)
    return mask

# def get_world_points_from_mask(binary_image, world_mat_contents, d, save_path, debug=False):

#     # Get the coordinates of non-zero pixels (points inside the mask)
#     mask_points = np.array(np.column_stack(np.where(binary_image > 0)))
#     # Print the number of sampled points
#     num_points = len(mask_points)
#     # print(f"Number of sampled points: {num_points}")
    
#     A = world_mat_contents['camera_matrix']
#     Rw2c = world_mat_contents['rotation_matrix']
#     Rc2w = np.linalg.inv(Rw2c)
#     Tw2c = world_mat_contents['translation_vector']
#     Tc2w = -np.dot(np.linalg.inv(Rw2c), Tw2c)
#     # import pdb; pdb.set_trace()
#     # world_points = affine_img2world(grid_points, A, Rc2w, Tc2w, d)
#     world_points = affine_img2world(mask_points, A, Rc2w, Tc2w, d)

#     if debug:
#         # Plot the original mask and sampled points
#         plt.scatter(world_points[:, 1], world_points[:, 0], color='red', s=1, label="Sampled Points")
#         plt.title(f'Sampled Points from Mask')
#         plt.legend()
#         plt.show()

#     return world_points

def get_world_points_from_mask(grid_points, world_mat_contents, d, save_path, debug=False):

    # Get the coordinates of non-zero pixels (points inside the mask)
    #mask_points = np.array(np.column_stack(np.where(binary_image > 0)))
    # Print the number of sampled points
    # print(f"Number of sampled points: {num_points}")
    
    A = world_mat_contents['camera_matrix']
    Rw2c = world_mat_contents['rotation_matrix']
    Rc2w = np.linalg.inv(Rw2c)
    Tw2c = world_mat_contents['translation_vector']
    Tc2w = -np.dot(np.linalg.inv(Rw2c), Tw2c)
    # import pdb; pdb.set_trace()

    world_points = affine_img2world(grid_points, A, Rc2w, Tc2w, d)

    if debug:
        # Plot the original mask and sampled points
        plt.scatter(world_points[:, 1], world_points[:, 0], color='red', s=1, label="Sampled Points")
        plt.title(f'Sampled Points from Mask')
        plt.legend()
        plt.show()

    return world_points

# 定义旋转矩阵 (例如：绕z轴旋转theta角)
def rotation_matrix_z(theta):
    return np.array([
        [np.cos(theta), -np.sin(theta), 0],
        [np.sin(theta), np.cos(theta), 0],
        [0, 0, 1]
    ])

# 定义平移向量
def translation_vector(tx, ty, tz):
    return np.array([tx, ty, tz])

# 对点云应用旋转和平移
def transform_point_cloud(point_cloud, rotation_matrix, translation_vector):
    # 应用旋转
    rotated_point_cloud = np.dot(point_cloud, rotation_matrix.T)  # 转置矩阵用于点的正确旋转
    # 应用平移
    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 gaussian_surface(X, A, x0, y0, sigma):
    x, y = X
    return A * np.exp(-((x - x0)**2 + (y - y0)**2) / (2 * sigma**2))


def curve_fit_gaussian(points):
    x = points[:, 0]
    y = points[:, 1]
    z = points[:, 2]

    # 使用curve_fit来拟合高斯曲面
    # 初始猜测参数 A, x0, y0, sigma
    initial_guess = [1, np.mean(x), np.mean(y), 1]
    
    # 使用 curve_fit 进行非线性最小二乘拟合
    popt, pcov = curve_fit(gaussian_surface, (x, y), z, p0=initial_guess)
    
    # 拟合的参数
    A, x0, y0, sigma = popt
    print(f"拟合参数: A={A}, x0={x0}, y0={y0}, sigma={sigma}")
    
    # 根据拟合的高斯曲面计算新的 z 值
    z_fitted = gaussian_surface((x, y), A, x0, y0, sigma)
    
    # 生成新的点云
    new_points = np.column_stack((x, y, z_fitted))

    # 可视化原始点云和拟合的高斯曲面
    # fig = plt.figure()
    # ax = fig.add_subplot(111, projection='3d')

    # # 绘制原始点云
    # #ax.scatter(x, y, z, c='r', label='Original Points')

    # # 绘制拟合的高斯曲面点云
    # ax.scatter(x, y, z_fitted, c='b', label='Fitted Gaussian Surface')

    # ax.set_title('3D Point Cloud and Fitted Gaussian Surface')
    # ax.set_xlabel('X')
    # ax.set_ylabel('Y')
    # ax.set_zlabel('Z')
    # ax.legend()

    # plt.show()
    
    return new_points


def curve_fit_2(points):
    x = points[:,0]
    y = points[:,1]
    z = points[:,2]
    A = np.column_stack((x**2, y**2, x*y, x, y, np.ones_like(x)))

    # 使用最小二乘法拟合二次曲面的系数
    coefficients, _, _, _ = lstsq(A, z, rcond=None)

    # 拟合的系数
    a, b, c, d, e, f = coefficients
    print("拟合的系数:", coefficients)

    # 根据拟合的曲面方程计算新的 z 值
    z_fitted = a * x**2 + b * y**2 + c * x * y + d * x + e * y + f

    # 生成新的点云
    new_points = np.column_stack((x, y, z_fitted))


    # # 可视化原始点云和拟合的曲面
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # 绘制原始点云
    #ax.scatter(x, y, z, c='r', label='Original Points')

    # 绘制拟合的曲面点云
    ax.scatter(x, y, z_fitted, c='b', label='Fitted Surface')

    ax.set_title('3D Point Cloud and Fitted Quadratic Surface')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.legend()

    plt.show()
    return new_points

def curve_fit_3(points):
    x = points[:,0]
    y = points[:,1]
    z = points[:,2]
    # 构建三次曲面的设计矩阵
    # 曲面模型：z = ax^3 + by^3 + cx^2y + dxy^2 + ex^2 + fy^2 + gxy + hx + iy + j
    A = np.column_stack((x**3, y**3, x**2*y, x*y**2, x**2, y**2, x*y, x, y, np.ones_like(x)))

    # 使用最小二乘法拟合三次曲面的系数
    coefficients, _, _, _ = lstsq(A, z, rcond=None)

    # 拟合的系数
    a, b, c, d, e, f, g, h, i, j = coefficients
    #print("拟合的系数:", coefficients)

    # 根据拟合的曲面方程计算新的 z 值
    z_fitted = a * x**3 + b * y**3 + c * x**2 * y + d * x * y**2 + e * x**2 + f * y**2 + g * x * y + h * x + i * y + j
    # 生成新的点云
    new_points = np.column_stack((x, y, z_fitted))


    # # # 可视化原始点云和拟合的曲面
    # fig = plt.figure()
    # ax = fig.add_subplot(111, projection='3d')

    # # 绘制原始点云
    # #ax.scatter(x, y, z, c='r', label='Original Points')

    # # 绘制拟合的曲面点云
    # ax.scatter(x, y, z_fitted, c='b', label='Fitted Surface')

    # ax.set_title('3D Point Cloud and Fitted Quadratic Surface')
    # ax.set_xlabel('X')
    # ax.set_ylabel('Y')
    # ax.set_zlabel('Z')
    # ax.legend()

    # plt.show()
    return new_points

def remove_duplicates(points):
    # 使用字典来去重，键为 (x, y)，值为 z
    unique_points = {}
    
    for point in points:
        x, y, z = point
        
        if (x, y) not in unique_points:
            unique_points[(x, y)] = z  # 如果 x, y 坐标未出现过，保留该点
    
    # 将去重后的点云转换为 numpy 数组
    new_points = np.array([[x, y, z] for (x, y), z in unique_points.items()])
    
    return new_points

def post_process(points, debug):
    uniq_points = remove_duplicates(points)
    
    x = uniq_points[:,0]
    y = uniq_points[:,1]
    z = uniq_points[:,2]

    close_point = remove_duplicate_points(points)

    smoothed_point = moving_average_filter_z(close_point, 20)

    fitted_points = curve_fit_gaussian(smoothed_point)
   
    return fitted_points

    
    plane_points = fit_plane_and_adjust(close_point, 10)
    
    z_outliers_removed = remove_outliers(smoothed_z, threshold=30.0)
    z_final = smooth_z_with_std(z_outliers_removed, 5)

    
    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')

        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')
    
        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')

        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)
        
    
        plt.show()

    return z_final
    



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)

    # 将三维点从相机 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()


def rename_gamma():
    img_path = 'D:\\code\\Gamma\\Gamma\\cam3_original_pictures\\'
    i = 0
    for img in os.listdir(img_path):
        print(img)
        new_name = img_path + '3_frame_' + str(i) + '.bmp'
        i += 1
        print(new_name)
        shutil.move(img_path + img, new_name)

# 函数：将点映射到拟合椭圆上的对应点
def map_point_to_ellipse(contour_point, center, axes, angle):
    # 计算轮廓点相对于中心的偏移
    x_diff = contour_point[0][0] - center[0]
    y_diff = contour_point[0][1] - center[1]

    # 将轮廓点旋转至与椭圆一致的角度
    angle_rad = np.deg2rad(angle)
    x_rot = x_diff * np.cos(angle_rad) + y_diff * np.sin(angle_rad)
    y_rot = -x_diff * np.sin(angle_rad) + y_diff * np.cos(angle_rad)

    # 将点缩放到椭圆的长轴和短轴上
    x_ellipse = x_rot / axes[0]
    y_ellipse = y_rot / axes[1]

    # 映射点到椭圆上
    scaling_factor = 1 / np.sqrt(x_ellipse**2 + y_ellipse**2)
    x_ellipse_scaled = x_ellipse * scaling_factor * axes[0]
    y_ellipse_scaled = y_ellipse * scaling_factor * axes[1]

    # 旋转回原来的角度
    x_final = x_ellipse_scaled * np.cos(-angle_rad) + y_ellipse_scaled * np.sin(-angle_rad)
    y_final = -x_ellipse_scaled * np.sin(-angle_rad) + y_ellipse_scaled * np.cos(-angle_rad)

    # 返回映射后的椭圆点
    return (x_final + center[0], y_final + center[1])

def find_best_circle(image, min_thresh, max_thresh):
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)   
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)
    final_ellipse, final_thresh_img, final_contour = None, None, None
    final_error = 100
    final_bin_thresh = 100
    for bin_thresh in range(min_thresh, max_thresh, 5):
       
        _,  thresh_img = cv2.threshold(blurred, bin_thresh, 255, cv2.THRESH_BINARY)
        contours, _ = cv2.findContours(thresh_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        max_contour = max(contours, key=lambda contour: cv2.boundingRect(contour)[2] * cv2.boundingRect(contour)[3])
        ellipse = cv2.fitEllipse(max_contour)
        center = (int(ellipse[0][0]), int(ellipse[0][1]))  
        axes = (int(ellipse[1][0] / 2), int(ellipse[1][1] / 2))  
        angle = ellipse[2] 
        errors = []
        for point in max_contour:
            ellipse_point = map_point_to_ellipse(point, center, axes, angle)
            distance = np.linalg.norm(np.array(point) - np.array(ellipse_point))
       
            errors.append(distance)
        average_error = np.mean(errors)
     
        if average_error < final_error:
            final_error = average_error
            final_ellipse = ellipse 
            final_thresh_img = thresh_img
            final_bin_thresh = bin_thresh
            final_contour = max_contour
    return final_ellipse, final_thresh_img, final_bin_thresh, final_contour



def find_notch(white_path, cam_num):

    image = cv2.imread(white_path)
    if cam_num == 1:
        ellipse, thresh_img, bin_thresh, max_contour_ellipse = find_best_circle(image, 5, 50)
        
        background = np.zeros_like(thresh_img)
        cv2.ellipse(background, ellipse, (255, 255, 255), 1)  
        ellipse_points = np.column_stack(np.where(background == 255))
        (center_x, center_y), (axis_major, axis_minor), angle = ellipse
        
        height, width = thresh_img.shape
        min_gray = 10000
        min_pt = [0,0]  
        for pt in ellipse_points:
            x, y = pt[1], pt[0]
            #cv2.circle(image, (pt[1], pt[0]), 3, (0,255,255), 2)
            x_start = max(0, x - 5)
            x_end = min(width - 1, x + 5)
            y_start = max(0, y - 5)
            y_end = min(height - 1, y + 5)
            neighborhood = thresh_img[y_start:y_end+1, x_start:x_end+1]
            mean_value = np.mean(neighborhood)
            if mean_value < min_gray:
                min_gray = mean_value
                min_pt = (x, y)

        angle_rad = np.arctan2(min_pt[1] - center_y, min_pt[0] - center_x)

        thresh_img = cv2.drawContours(background, [max_contour_ellipse], -1, (255, 255, 255), thickness=cv2.FILLED)
    
        cv2.circle(image, min_pt, 50, (0,255,255), 2)
        #print(min_pt)
        cv2.namedWindow('thresh_img', 0)
        cv2.imshow('thresh_img', thresh_img)
        cv2.namedWindow('contours', 0)
        cv2.imshow('contours', image)
    
        cv2.waitKey(0)
        return angle_rad, thresh_img
    else:
        return 0

def apply_median_filter(z_reconstructed, size=3):
    # 应用中值滤波，size 定义了邻域的大小
    z_filtered = median_filter(z_reconstructed, size=size)
    return z_filtered

def apply_gaussian_filter(z_reconstructed, sigma=1.0):
    # 应用高斯滤波，sigma 控制平滑的程度
    z_filtered = gaussian_filter(z_reconstructed, sigma=sigma)
    return z_filtered

def detect_outliers(z_reconstructed, threshold=3.0):
    mean_z = np.mean(z_reconstructed)
    std_z = np.std(z_reconstructed)
    
    # 计算每个点的 z 值与全局均值的差距
    z_diff = np.abs(z_reconstructed - mean_z)
    
    # 检测超过阈值的点
    outliers = z_diff > (threshold * std_z)
    
    return outliers

def remove_outliers(z_reconstructed, threshold=3.0):
    # 检测异常点
    outliers = detect_outliers(z_reconstructed, threshold=threshold)
    
    # 将异常点替换为邻域的平均值
    z_filtered = np.copy(z_reconstructed)
    z_filtered[outliers] = np.median(z_reconstructed[~outliers])
    
    return z_filtered


def apply_local_average_smoothing(z_reconstructed, window_size=3):
    padded_z = np.pad(z_reconstructed, pad_width=window_size//2, mode='reflect')
    smoothed_z = np.copy(z_reconstructed)

    # 对每个点计算邻域内的平均值
    for i in range(z_reconstructed.shape[0]):
        for j in range(z_reconstructed.shape[1]):
            window = padded_z[i:i+window_size, j:j+window_size]
            smoothed_z[i, j] = np.mean(window)
    
    return smoothed_z





def average_duplicate_points(points):
    """
    处理点云，计算每个 (x, y) 坐标中所有 z 值的均值。
    
    参数:
    points: (n, 3) 形状的 NumPy 数组，表示 (x, y, z) 格式的点云数据
    
    返回:
    result: 去重后的点云数据，每个 (x, y) 对应的 z 值是所有重叠 z 值的均值
    """
    # 将点云数据放入 DataFrame 中，方便处理
    df = pd.DataFrame(points, columns=['x', 'y', 'z'])
    
    # 按照 (x, y) 进行分组，并计算 z 值的均值
    unique_points = df.groupby(['x', 'y'], as_index=False).mean()
    
    return unique_points[['x', 'y', 'z']].to_numpy()

def remove_duplicate_points(points):
    """
    处理点云，保留每个 (x, y) 坐标中与全局 z 值均值最近的一个 z 值。
    
    参数:
    points: (n, 3) 形状的 NumPy 数组，表示 (x, y, z) 格式的点云数据
    
    返回:
    result: 去重后的点云数据，只保留每个 (x, y) 对应的一个 z 值
    """
    # 将点云数据放入 DataFrame 中，方便处理
    df = pd.DataFrame(points, columns=['x', 'y', 'z'])
    
    # 计算全局 z 值的均值
    global_z_mean = df['z'].mean()
    
    # 按照 (x, y) 对每个点进行分组
    def select_closest_z(group):
        # 找到 z 值与全局均值最近的那个
        closest_index = np.abs(group['z'] - global_z_mean).idxmin()
        return group.loc[closest_index]
    
    # 应用分组操作，找到每个 (x, y) 中 z 值与全局均值最近的一个
    unique_points = df.groupby(['x', 'y'], as_index=False).apply(select_closest_z)
    
    # 重置索引并返回结果
    unique_points = unique_points.reset_index(drop=True)
    return unique_points[['x', 'y', 'z']].to_numpy()



def smooth_z_with_std(z_values, threshold=2.0):
    """
    使用均值和标准差去除 z 值中的离散点。
    
    参数:
    z_values: 点云的 z 值数组
    threshold: 超过标准差的倍数，作为离散点的判断标准
    
    返回:
    smoothed_z: 平滑后的 z 值
    """
    # 计算 z 值的均值和标准差
    mean_z = np.mean(z_values)
    std_z = np.std(z_values)
    
    # 创建一个副本，避免直接修改原始数据
    smoothed_z = np.copy(z_values)
    
    # 查找离散点（z 值偏离均值超过 threshold * 标准差的点）
    outliers = np.abs(z_values - mean_z) > threshold * std_z
    
    # 将离散点替换为全局均值或其他平滑方法
    smoothed_z[outliers] = mean_z  # 可以选择其他平滑值，如邻居的均值
    
    return smoothed_z


def fit_plane_and_adjust(points, threshold):
    """
    拟合一个平面，调整距离平面距离大于阈值的点的 z 值，使其在拟合平面上。
    
    参数:
    points: (n, 3) 形状的 NumPy 数组，表示 (x, y, z) 格式的点云数据
    threshold: 距离平面的最大允许偏差，超过此距离的点将被设置为平面上的点
    
    返回:
    result: 处理后的点云数据
    """
    # 将点云数据放入 DataFrame 中，方便处理
    df = pd.DataFrame(points, columns=['x', 'y', 'z'])
    
    # 使用 x 和 y 拟合平面 z = ax + by + c
    X = df[['x', 'y']].values
    Z = df['z'].values
    model = LinearRegression().fit(X, Z)
    
    # 获取拟合平面方程的系数 a, b 和截距 c
    a, b = model.coef_
    c = model.intercept_
    
    # 计算每个点在拟合平面上的 z 值
    z_fit = a * df['x'] + b * df['y'] + c
    
    # 计算每个点到拟合平面的距离
    distances = np.abs(df['z'] - z_fit)
    
    # 将距离超过阈值的点的 z 值设置为平面上的 z 值
    df.loc[distances > threshold, 'z'] = z_fit[distances > threshold]
    
    return df[['x', 'y', 'z']].to_numpy()

def laplacian_smoothing(points, z_values, iterations=10, alpha=0.1):
    """
    使用拉普拉斯平滑来平滑点云 z 值。
    
    参数:
    points: (n, 2) 形状的 NumPy 数组，表示 (x, y) 坐标的点
    z_values: 点云的 z 值数组
    iterations: 平滑的迭代次数
    alpha: 平滑系数，控制更新的幅度
    
    返回:
    smoothed_z: 平滑后的 z 值
    """
    smoothed_z = np.copy(z_values)
    
    for _ in range(iterations):
        for i in range(len(points)):
            # 找到所有的邻居
            distances = np.linalg.norm(points - points[i], axis=1)
            neighbors = distances < 1.5  # 假设距离小于一定值的是邻居
            
            # 计算邻居 z 值的平均值
            neighbor_z = np.mean(smoothed_z[neighbors])
            
            # 更新当前点的 z 值
            smoothed_z[i] = (1 - alpha) * smoothed_z[i] + alpha * neighbor_z
    
    return smoothed_z



def bilinear_interpolation(points, z_values, grid_size=(100, 100)):
    """
    使用双线性插值对点云 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]

    # 使用双线性插值
    grid_z = griddata(points, z_values, (grid_x, grid_y), method='linear')
    
    return grid_x, grid_y, grid_z



def cubic_interpolation(points, z_values, grid_size=(100, 100)):
    """
    使用三次样条插值对点云 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]
    smoothed_z = gaussian_filter(z_values.reshape(grid_size), sigma=1)
    # 使用三次样条插值
    grid_z = griddata(points, smoothed_z, (grid_x, grid_y), method='cubic')
    
    return grid_x, grid_y, grid_z


def moving_average_filter_z(points, window_size=3):
    """
    使用移动平均法对 z 值进行平滑，减少异常值。
    
    参数:
    z_values: 点云的 z 值数组
    window_size: 移动窗口的大小
    
    返回:
    smoothed_z: 平滑后的 z 值
    """
    z_values = points[:,2]
    smoothed_z = np.copy(z_values)
    for i in range(len(z_values)):
        # 找到窗口内的邻居
        start_idx = max(0, i - window_size // 2)
        end_idx = min(len(z_values), i + window_size // 2 + 1)
        smoothed_z[i] = np.mean(z_values[start_idx:end_idx])
    
    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=',')
    
    





if __name__ == '__main__':
    # white_path = 'D:\\data\\four_cam\\1008_storage\\'
    # cam_num = 1
    # #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)