pmd-python/unit_test.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
from aprilgrid import Detector
from src.utils import *
from scipy.optimize import least_squares
import random
from scipy.io import savemat, loadmat
from src.pcl_postproc import smooth_pcl, align2ref
from src.calibration import *
import math
from src.calibration import calibrate_world, calibrate_screen_chessboard, calibrate_screen_aprilgrid, map_screen_to_world
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import find_peaks
from skimage.measure import find_contours
import numpy as np
import matplotlib.pyplot as plt
from scipy.ndimage import gaussian_filter
from skimage.restoration import unwrap_phase
from aprilgrid import Detector
from src.calibration.calibrate import generate_3d_points, show_detect_result




def pixel_to_world(points_2d, K, R, T, depth):
    """
    将像素坐标转换为世界坐标系中的 3D 坐标。
    
    Parameters:
    points_2d: np.ndarray
        图像中的2D像素坐标 (N, 2)。
    K: np.ndarray
        相机内参矩阵 (3x3)。
    R: np.ndarray
        相机旋转矩阵 (3x3)。
    T: np.ndarray
        相机平移向量 (3, 1)。
    depth: float or np.ndarray
        点的深度（单位：毫米或其他实际单位）。
        
    Returns:
    points_3d_world: np.ndarray
        世界坐标系中的 3D 点 (N, 3)。
    """
    # 将像素坐标转换为齐次坐标 (u, v, 1)
    image_points_3d = np.hstack((points_2d, np.ones((points_2d.shape[0], 1))))
    norm_points = np.linalg.inv(K).dot(image_points_3d.reshape(-1, 3).T).T
    # 逆投影到3D空间
    object_points = []
    for point in norm_points:
    # 将归一化坐标转换为3D点
        point_3d = np.zeros((3,), dtype=np.float32)
        point_3d[:2] = point[:2] / point[2]  # 归一化坐标转换为3D坐标
        point_3d[2] = 1

        # 使用旋转和平移向量进行逆投影
        # 首先需要将旋转向量转换为旋转矩阵
        #R, _ = cv2.Rodrigues(R.reshape(3, 3))
        object_point = np.dot(R.T, point_3d - T.reshape(3, 1)).flatten()

        object_points.append(object_point)

  
    points_3d_world = np.array(object_points)

    print("Object Points:")
    print(points_3d_world)

    return points_3d_world


def project_to_world(points_2d, K, R, T, dist_coeffs, depth=1.0):
    """
    将相机检测到的2D点转换为世界坐标系下的3D点。
    
    Parameters:
    points_2d: np.ndarray
        图像中的2D点 (N, 2)。
    K: np.ndarray
        相机内参矩阵。
    R: np.ndarray
        相机旋转矩阵 (3x3)。
    T: np.ndarray
        相机平移向量 (3, 1)。
    dist_coeffs: np.ndarray
        相机的畸变系数。
    depth: float or np.ndarray
        点的深度（可以是标量或与点的数量匹配的数组）。
        
    Returns:
    points_3d_world: np.ndarray
        世界坐标系下的3D点 (N, 3)。
    """

    # 去畸变并将2D点转换为相机归一化坐标
    #points_undistorted = cv2.undistortPoints(points_2d, K, dist_coeffs)

    #print('points_undistorted = ', points_undistorted)

    # 将2D点扩展为3D点，相机归一化坐标 (x, y, 1)
    points_3d_camera = np.hstack((points_2d.squeeze(), np.ones((points_2d.shape[0], 1))))

    # 乘以深度，得到实际的相机坐标系中的 3D 点
    points_3d_camera *= depth

    # 将相机坐标系中的点转换为世界坐标系中的点
    points_3d_world = np.dot(R.T, (points_3d_camera.T - T)).T

    return points_3d_world


def plot_3d_points(points_3d, label, color, ax):
    """
    绘制3D点云。
    
    Parameters:
    points_3d: np.ndarray
        3D世界坐标系中的点 (N, 3)。
    label: str
        相机的标签名称。
    color: str
        点的颜色。
    ax: Axes3D
        3D图形对象。
    """
   
    ax.scatter(points_3d[:, 0], points_3d[:, 1], points_3d[:, 2], label=label, color=color)


# 定义投影函数
def project_points(points_3d, rvec, tvec, K, dist_coeffs):
    """
    将 3D 世界坐标点投影到 2D 图像平面。
    
    Parameters:
    points_3d: numpy.ndarray
        3D 点的坐标 (N, 3)。
    rvec: numpy.ndarray
        旋转向量 (3,)。
    tvec: numpy.ndarray
        平移向量 (3,)。
    K: numpy.ndarray
        相机内参矩阵 (3, 3)。
    dist_coeffs: numpy.ndarray
        畸变系数。
    
    Returns:
    image_points: numpy.ndarray
        投影到图像平面的 2D 点 (N, 2)。
    """
    image_points, _ = cv2.projectPoints(points_3d, rvec, tvec, K, dist_coeffs)
    return np.squeeze(image_points)

# def residuals(params, n_cameras, n_points, camera_indices, point_indices, points_2d, K_matrices, dist_coeffs):
#     """
#     计算捆绑调整中的残差，即重投影误差。
    
#     Parameters:
#     params: np.ndarray
#         优化的参数（包括所有相机的外参和所有3D点的坐标）。
#     n_cameras: int
#         相机的数量。
#     n_points: int
#         3D 点的数量。
#     camera_indices: np.ndarray
#         每个观测对应的相机索引。
#     point_indices: np.ndarray
#         每个观测对应的 3D 点索引。
#     points_2d: np.ndarray
#         实际观测到的 2D 图像点。
#     K_matrices: list of np.ndarray
#         每个相机的内参矩阵。
#     dist_coeffs: list of np.ndarray
#         每个相机的畸变系数。
    
#     Returns:
#     residuals: np.ndarray
#         重投影误差。
#     """
#     # 提取旋转和平移向量
#     rvecs = params[:n_cameras * 3].reshape((n_cameras, 3))  # 旋转向量
#     tvecs = params[n_cameras * 3:n_cameras * 6].reshape((n_cameras, 3))  # 平移向量
    
#     # 提取3D点
#     points_3d = params[n_cameras * 6:].reshape((n_points, 3))  # 3D 点
    
#     residuals = []

    
#     # 计算每个观测的重投影误差
#     for i in range(len(camera_indices)):
#         cam_idx = camera_indices[i]  # 当前观测的相机
#         point_idx = point_indices[i]  # 当前观测的 3D 点
#         K = K_matrices[cam_idx]  # 相机的内参矩阵
#         dist = dist_coeffs[cam_idx]  # 相机的畸变系数
        
#         # 投影3D点到相机平面，使用OpenCV的projectPoints函数
#         projected_points, _ = cv2.projectPoints(
#             points_3d[point_idx].reshape(1, -1),  # 当前的3D点
#             rvecs[cam_idx].reshape(-1, 1),        # 当前相机的旋转向量 (Rodrigues形式)
#             tvecs[cam_idx].reshape(-1, 1),        # 当前相机的平移向量
#             K,                                    # 内参矩阵
#             dist                                  # 畸变系数
#         )
        
#         # 将 projected_points 变成 (N, 2) 的形状
#         projected_points = projected_points.squeeze()

#         # 计算残差（重投影误差）
#         residuals.append(projected_points - points_2d[i])

#     return np.concatenate(residuals)



def calculate_normal(points):
    # points 是一个 (4, 3) 的数组，表示 4 个 3D 点
    vec1 = points[1] - points[0]
    vec2 = points[2] - points[0]
    normal = np.cross(vec1, vec2)  # 计算法向量
    return normal / np.linalg.norm(normal)  # 单位化


def point_to_plane_distance(point, plane_normal, plane_point):
    return np.dot(plane_normal, point - plane_point)



def residuals(params, n_cameras, n_points, camera_indices, point_indices, points_2d, K_matrices, dist_coeffs, plane_constraints):
    rvecs = params[:n_cameras * 3].reshape((n_cameras, 3))
    tvecs = params[n_cameras * 3:n_cameras * 6].reshape((n_cameras, 3))
    points_3d = params[n_cameras * 6:].reshape((n_points, 3))

    residuals = []

    # 计算重投影误差
    for i in range(len(camera_indices)):
        cam_idx = camera_indices[i]
        point_idx = point_indices[i]
        K = K_matrices[cam_idx]
        dist = dist_coeffs[cam_idx]

        projected_points, _ = cv2.projectPoints(
            points_3d[point_idx].reshape(1, -1),
            rvecs[cam_idx].reshape(3, 1),
            tvecs[cam_idx].reshape(3, 1),
            K, dist
        )
        projected_points = projected_points.squeeze()
        residual = projected_points - points_2d[i]
        residuals.extend(residual.tolist())  # 将重投影误差的两个分量添加到 residuals

    # 计算平面约束残差
    for constraint in plane_constraints:
        points_idx = constraint
        plane_points = points_3d[points_idx[:3]]  # 取前三个点计算平面
        plane_normal = calculate_normal(plane_points)
        plane_point = plane_points[0]

        for idx in points_idx:
            distance = point_to_plane_distance(points_3d[idx], plane_normal, plane_point)
            residuals.append(distance)  # 添加标量距离值

    return np.array(residuals)

def extract_params_from_result(result, n_cameras, n_points):
    """
    从优化结果中提取旋转向量、平移向量和3D点数据。
    
    Parameters:
    result: OptimizeResult
        scipy.optimize.least_squares 返回的结果。
    n_cameras: int
        相机的数量。
    n_points: int
        3D 点的数量。
        
    Returns:
    rvecs: list of np.ndarray
        优化后的旋转向量。
    tvecs: list of np.ndarray
        优化后的平移向量。
    points_3d: np.ndarray
        优化后的3D点坐标。
    """
    optimized_params = result.x
    
    # 提取旋转和平移向量
    rvecs = optimized_params[:n_cameras * 3].reshape((n_cameras, 3))  # 旋转向量
    tvecs = optimized_params[n_cameras * 3:n_cameras * 6].reshape((n_cameras, 3))  # 平移向量
    
    # 提取3D点
    #points_3d = optimized_params[n_cameras * 6:].reshape((n_points, 3))  # 3D点
    
    return rvecs, tvecs


def compare_params(optimized_rvecs, optimized_tvecs, original_cam_params):
    """
    对比优化后的相机外参和原始标定结果。
    
    Parameters:
    optimized_rvecs: np.ndarray
        优化后的旋转向量。
    optimized_tvecs: np.ndarray
        优化后的平移向量。
    original_cam_params: list of dict
        原始标定结果（包含旋转矩阵和平移向量）。
        
    Returns:
    None
    """
    for cam_idx, original_params in enumerate(original_cam_params):
        # 原始旋转矩阵和转换为旋转向量
        original_rvec = cv2.Rodrigues(original_params['rotation_matrix'])[0].flatten()
        original_tvec = original_params['translation_vector'].flatten()

        # 优化后的旋转向量和平移向量
        optimized_rvec = optimized_rvecs[cam_idx]
        optimized_tvec = optimized_tvecs[cam_idx]

        # 计算旋转向量和平移向量的差异
        rotation_diff = np.linalg.norm(original_rvec - optimized_rvec)
        translation_diff = np.linalg.norm(original_tvec - optimized_tvec)

        print(f"相机 {cam_idx} 差异：")
        print(f"旋转向量差异: {rotation_diff}")
        print(f"平移向量差异: {translation_diff}")



def calculate_reprojection_error(rvecs, tvecs, points_3d, points_2d, K_matrices, dist_coeffs):
    """
    计算重投影误差。
    
    Parameters:
    rvecs: np.ndarray
        相机的旋转向量。
    tvecs: np.ndarray
        相机的平移向量。
    points_3d: np.ndarray
        3D点坐标。
    points_2d: np.ndarray
        2D图像中的对应点。
    K_matrices: list of np.ndarray
        相机内参矩阵。
    dist_coeffs: list of np.ndarray
        畸变系数。
        
    Returns:
    float
        平均重投影误差。
    """
    total_error = 0
    total_points = 0

    for cam_idx in range(len(rvecs)):
        projected_points, _ = cv2.projectPoints(points_3d, rvecs[cam_idx], tvecs[cam_idx], K_matrices[cam_idx], dist_coeffs[cam_idx])
        error = np.linalg.norm(points_2d[cam_idx] - projected_points.squeeze(), axis=1).sum()
        total_error += error
        total_points += len(points_2d[cam_idx])

    return total_error / total_points



def bundle_adjustment(all_object_points, all_corners, cam_params, plane_constraints):
    """
    使用捆绑调整优化相机参数和3D点坐标。
    
    Parameters:
    all_object_points: list of list of np.ndarray
        每个相机中每个图像中的 3D 物体点。
    all_corners: list of list of np.ndarray
        每个相机中的多个图像中的 2D 图像点（每台相机拍摄了多张图片）。
    cam_params: list of dict
        每个相机的内参和外参。
    
    Returns:
    result: scipy.optimize.OptimizeResult
        优化后的参数结果。
    """
    n_cameras = len(cam_params)  # 相机的数量

    # 展平所有 3D 物体点
    all_object_points_flat = []
    for object_points_cam in all_object_points:
        all_object_points_flat.extend(object_points_cam)  # 将每个相机的所有 3D 物体点展平

    all_object_points_flat = np.concatenate(all_object_points_flat)
    n_points = all_object_points_flat.shape[0]

    # 收集相机内参矩阵和畸变系数
    K_matrices = [params['camera_matrix'] for params in cam_params]
    dist_coeffs = [params['distortion_coefficients'] for params in cam_params]

    # 创建相机索引和点索引，用于优化
    camera_indices = []
    point_indices = []
    points_2d = []

    # 记录全局 3D 点的偏移量，用于正确生成 point_indices
    point_offset = 0

    # 遍历每台相机的所有图像
    for cam_idx, (object_points_cam, corners_cam) in enumerate(zip(all_object_points, all_corners)):
        for img_idx, points_2d_list in enumerate(corners_cam):  # 遍历每个相机的每张图像中的2D点
            n_points_in_image = points_2d_list.shape[0]

            # 给每个2D点分配当前相机的相机索引（相机编号 cam_idx）
            camera_indices.extend([cam_idx] * n_points_in_image)
            
            # 给每个2D点分配全局3D点索引
            point_indices.extend(range(point_offset, point_offset + n_points_in_image))
            
            # 增加偏移量
            point_offset += n_points_in_image

            # 收集2D图像点
            points_2d.extend(points_2d_list)

    camera_indices = np.array(camera_indices)
    point_indices = np.array(point_indices)
    points_2d = np.array(points_2d)


    # 初始化外参（旋转和平移向量）和 3D 点
    rvecs = [cv2.Rodrigues(params['rotation_matrix'])[0].flatten() for params in cam_params]
    tvecs = [params['translation_vector'].flatten() for params in cam_params]

    # 将外参和3D点展平为一维数组
    initial_params = np.hstack([
        np.concatenate(rvecs),  # 所有相机的旋转向量
        np.concatenate(tvecs),  # 所有相机的平移向量
        all_object_points_flat.ravel()  # 所有3D点展平
    ])

    # 执行捆绑调整，使用 'trf' 或 'dogbox' 方法
    # result = least_squares(
    #     residuals, initial_params, verbose=2, x_scale='jac', ftol=1e-4, method='trf',  # 使用 'trf' 方法
    #     args=(n_cameras, n_points, camera_indices, point_indices, points_2d, K_matrices, dist_coeffs)
    # )
    result = least_squares(
    residuals, initial_params, verbose=2, x_scale='jac', ftol=1e-4, method='trf',
    args=(n_cameras, n_points, camera_indices, point_indices, points_2d, K_matrices, dist_coeffs, plane_constraints)
)

    # 优化前的重投影误差
    original_rvecs = [cv2.Rodrigues(params['rotation_matrix'])[0] for params in cam_params]
    original_tvecs = [params['translation_vector'] for params in cam_params]

    # original_error = calculate_reprojection_error(
    #     original_rvecs, original_tvecs, original_points_3d, original_points_2d, K_matrices, dist_coeffs
    # )

    # # 优化后的重投影误差
    # optimized_error = calculate_reprojection_error(
    #     optimized_rvecs, optimized_tvecs, points_3d, points_2d, K_matrices, dist_coeffs
    # )

    #print(f"优化前的重投影误差: {original_error}")
    #print(f"优化后的重投影误差: {optimized_error}")

    return result


def visualize_projection(image, points_2d, color='r', title='Projected Points'):
    """
    在图像上绘制投影点。
    
    Parameters:
    image: numpy.ndarray
        图像数据。
    points_2d: numpy.ndarray
        要绘制的 2D 点。
    color: str
        绘制点的颜色。
    title: str
        图像标题。
    """
    plt.figure(figsize=(8, 6))
    plt.imshow(image, cmap='gray')
    plt.scatter(points_2d[:, 0], points_2d[:, 1], c=color, s=50, label='Projected Points')
    plt.legend()
    plt.title(title)
    plt.show()


def compute_reprojection_error(projected_points, observed_points):
    """
    计算投影点与实际观测点之间的误差。
    
    Parameters:
    projected_points: numpy.ndarray
        投影得到的2D点，形状为 (N, 2)。
    observed_points: numpy.ndarray
        实际观测到的2D点，形状为 (N, 2)。
    
    Returns:
    float
        平均重投影误差。
    """
    error = np.linalg.norm(projected_points - observed_points, axis=1)
    return np.mean(error)

# 将 3D 点转换到相机坐标系
def transform_points_to_camera_frame(points, R, T):
    return (R @ points.T + T).T


def detect_april_tag_points(detector, img_file, object_point_single):
    """
    检测单张图像中的 AprilTags,并返回图像中的 2D 角点和相应的 3D 物体点。
    
    Parameters:
    detector: AprilTag 检测器对象
    img_file: 图像文件路径
    object_point_single: 标定板的 3D 物体点
    
    Returns:
    all_corners: np.ndarray
        图像中的 2D 角点 (N, 2)
    all_object_points: np.ndarray
        对应的 3D 物体点 (N, 3)
    """
    #print('Processing image file:', img_file)

    # 读取并转换为灰度图像
    src = cv2.imread(img_file)
    gray = cv2.imread(img_file, 0)
    if gray is None:
        print(f"Failed to load image: {img_file}")
        return np.array([]), np.array([])

    # 检测 AprilTags
    detections = detector.detect(gray)
    all_corners = []
    all_object_points = []
    tag_id_lst = []

    #show_detect_result(src, detections)

    if detections:
        for detection in detections:
            corners = detection.corners  # 检测到的角点 (4, 2)
            tag_id = detection.tag_id    # 标签 ID
            tag_id_lst.append(int(tag_id))


            # 将检测到的角点存储为 np.float32 格式
            all_corners.extend(corners.astype(np.float32))

            # 根据标签 ID 提取相应的 3D 坐标
            if tag_id < len(object_point_single):
                all_object_points.extend(object_point_single[tag_id])
            else:
                print(f"Warning: Tag ID {tag_id} is out of range.")

    if all_object_points and all_corners:
        return np.array(all_corners, dtype=np.float32).reshape(-1, 2), \
               np.array(all_object_points, dtype=np.float32).reshape(-1, 3), \
               tag_id_lst, detections
    else:
        print("No valid detections in this image.")
        return np.array([]), np.array([]), [], []



def single_camera_calibrate_vis():
    config_path = 'D:/code/pmdrecons-python-xtt/config/cfg_3freq_wafer.json'
    para_path = 'D:\\code\\pmdrecons-python-xtt\\config\\world_params.mat'
    world_params = loadmat(para_path)
    camera_matrix = np.array(world_params['camera_matrix'])
    dist_coeffs = np.array(world_params['distortion_coefficients'])
    img_dir = 'D:\\data\\one_cam\\pmd_benchmarks\\set4-20240906\\set4-20240906\\calibration\\'

    img_paths = glob.glob(os.path.join(img_dir, 'world_calibration/*.bmp'))

    debug = 1

    cfg = json.load(open(config_path, 'r'))
    chessboard_size = cfg['world_chessboard_size']
    square_size = cfg['world_square_size']

    # 准备棋盘格的世界坐标
    objp = np.zeros((chessboard_size[0]*chessboard_size[1], 3), np.float32)
    objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
    objp *= square_size

    # 存储每个棋盘格在相机坐标系中的位置
    chessboard_positions = []

    for fname in img_paths:
        print(fname)
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # 寻找棋盘格角点
        ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

        if ret:
            # 提高角点精度
            criteria = (cv2.TermCriteria_EPS + cv2.TermCriteria_MAX_ITER, 30, 0.001)
            corners_subpix = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)

            # 计算棋盘格的姿态（相对于相机）
            success, rvec, tvec = cv2.solvePnP(objp, corners_subpix, camera_matrix, dist_coeffs)
            if success:
                # 计算旋转矩阵
                R, _ = cv2.Rodrigues(rvec)

                # 将棋盘格角点转换到相机坐标系
                # objp_in_camera = R * objp^T + tvec
                objp_in_camera = (R @ objp.T + tvec).T  # 转置以获得 N x 3 的矩阵

                # 存储转换后的棋盘格角点
                chessboard_positions.append(objp_in_camera)
            else:
                print("姿态估计失败：", fname)
        else:
            print("无法找到棋盘格角点：", fname)

    cv2.destroyAllWindows()

    # 绘制多个棋盘格在相机坐标系中的位置
    if len(chessboard_positions) > 0:
        # 创建3D绘图
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')

        # 绘制每个棋盘格
        for idx, chessboard in enumerate(chessboard_positions):
            x = chessboard[:, 0]
            y = chessboard[:, 1]
            z = chessboard[:, 2]
            ax.scatter(x, y, z, label=f'Chessboard {idx+1}')
            plt.pause(3)
            

        # 绘制相机位置（原点）
        ax.scatter(0, 0, 0, c='r', marker='^', s=100, label='Camera Position')

        # 设置标签和标题
        ax.set_xlabel('X axis')
        ax.set_ylabel('Y axis')
        ax.set_zlabel('Z axis')
        ax.set_title('Multiple Chessboard Positions Relative to Fixed Camera')

        # 设置等比例显示
        all_x = np.hstack([chessboard[:, 0] for chessboard in chessboard_positions])
        all_y = np.hstack([chessboard[:, 1] for chessboard in chessboard_positions])
        all_z = np.hstack([chessboard[:, 2] for chessboard in chessboard_positions])
        ax.set_box_aspect([np.ptp(a) for a in [all_x, all_y, all_z]])

        # 显示图例
        ax.legend()
        plt.show()
    else:
        print("未能计算出有效的棋盘格位置。")

    return


def draw(img, corner, imgpts):
    corner = tuple(corner.ravel().astype(int))
    imgpts = imgpts.astype(int)

    # 绘制坐标轴
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 0, 255), 5)  # X轴（红色）
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 255, 0), 5)  # Y轴（绿色）
    img = cv2.line(img, corner, tuple(imgpts[3].ravel()), (255, 0, 0), 5)  # Z轴（蓝色）

    return img



def verify_coplanarity(points_3d):
    # 使用SVD拟合平面
    centroid = np.mean(points_3d, axis=0)
    centered_points = points_3d - centroid
    U, S, Vt = np.linalg.svd(centered_points)
    normal_vector = Vt[-1, :]
    # 计算每个点到平面的距离
    distances = np.abs(centered_points @ normal_vector)
    mean_distance = np.mean(distances)
    max_distance = np.max(distances)
    print(f"Mean distance to plane: {mean_distance}")
    print(f"Max distance to plane: {max_distance}")
    # 设置一个阈值判断是否共面
    threshold = 1e-3  # 根据需求调整
    if max_distance < threshold:
        print("All points are approximately coplanar.")
    else:
        print("Points are not coplanar.")



def fit_plane(points):
    """拟合一个平面并返回法向量和质心."""
    centroid = np.mean(points, axis=0)  # 计算质心
    centered_points = points - centroid  # 将点移到质心

    # 使用 SVD 分解拟合平面法向量
    _, _, vh = np.linalg.svd(centered_points)
    normal_vector = vh[-1]  # 法向量是 SVD 的最后一列

    return normal_vector, centroid


def rotation_matrix_from_vectors(v1, v2):
    """计算将向量 v1 旋转到 v2 的旋转矩阵."""
    v1 = v1 / np.linalg.norm(v1)
    v2 = v2 / np.linalg.norm(v2)
    axis = np.cross(v1, v2)  # 旋转轴
    angle = np.arccos(np.dot(v1, v2))  # 旋转角度

    # 构建旋转矩阵（Rodrigues 公式）
    K = np.array([
        [0, -axis[2], axis[1]],
        [axis[2], 0, -axis[0]],
        [-axis[1], axis[0], 0]
    ])
    rotation_matrix = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K)
    return rotation_matrix


def align_points_to_horizontal(points):
    """将给定的 3D 点集旋转到水平面."""
    normal_vector, centroid = fit_plane(points)  # 拟合平面
    print('normal_vector = ', normal_vector)
    rotation_to_horizontal = rotation_matrix_from_vectors(normal_vector, np.array([0, 0, 1]))
    #print('rotation_to_horizontal = ', rotation_to_horizontal)

    # 将所有点旋转到水平面，并返回结果
    centered_points = points - centroid  # 平移到质心
    rotated_points = (rotation_to_horizontal @ centered_points.T).T + centroid  # 旋转并平移回去

    return rotated_points, rotation_to_horizontal



def camera_calibrate_vis():
    # 加载配置和相机参数
    config_path = 'D:\\code\\pmd-python\\config\\cfg_3freq_wafer.json'
    detector = Detector('t36h11')
    cfg = json.load(open(config_path, 'r'))
    with open(os.path.join('D:\\code\\pmd-python\\config\\', 'cam_params1023.pkl'), 'rb') as pkl_file:
        cam_params = pickle.load(pkl_file)
    
    # 生成 3D 物体点
    world_chessboard_size, world_square_size, world_square_spacing = [10, 6], 35.2, 10.56
    object_point_single = generate_3d_points(world_chessboard_size, world_square_size, world_square_spacing)  
    
    # 保存所有相机的观测数据
    all_points_3d_world = []
    all_image_points = []
    all_camera_centers = []
    K_matrices = []
    dist_coeffs = []
    all_corners = []
    all_object_points = []
    optimized_cam_params = []
    all_tag_lst = []
    all_detections = []

    # 定义四个相机的标定图像路径
    calibration_images_path = [
        'D:\\data\\four_cam\\calibrate\\single_test\\cam0\\',
        'D:\\data\\four_cam\\calibrate\\single_test\\cam1\\',
        'D:\\data\\four_cam\\calibrate\\single_test\\cam2\\',
        'D:\\data\\four_cam\\calibrate\\single_test\\cam3\\'
    ]


    # 遍历每个相机
    for cam_id, params in enumerate(cam_params):
        K_matrices.append(params['camera_matrix'])
        dist_coeffs.append(params['distortion_coefficients'])
        
        camera_corners = []  # 用于当前相机的2D图像点
        camera_object_points = []  # 用于当前相机的3D物体点
       
        # 对每个相机的标定图像提取2D角点
        for img_file in os.listdir(calibration_images_path[cam_id]):
            # 读取图像并检测AprilTags，获取2D角点和对应的3D物体点
            image_points, object_points, tag_id_lst, detections = detect_april_tag_points(
                detector, calibration_images_path[cam_id] + img_file, object_point_single
            )
            # print('image_points  = ', image_points)
            # print('object_points =', object_points)
            print('tag_id_lst = ', tag_id_lst)
            all_tag_lst.append(tag_id_lst)
            all_corners.append(image_points)
            all_object_points.append(object_points)
            all_detections.append(detections)
            undistorted_points = cv2.undistortPoints(image_points,  params['camera_matrix'], params['distortion_coefficients'])
            success, rotation_vector, translation_vector = cv2.solvePnP(object_points, image_points, params['camera_matrix'], 
                                                                       params['distortion_coefficients'],
                                                                       params['rotation_matrix'],
                                                                       params['translation_vector']
                                                                       )
            rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
            # 假设的深度值 (s)
            # 你需要根据场景给出一个合理的深度值
            # 相机的中心
            
            camera_center = -rotation_matrix.T @ translation_vector
            #print('camera_center = ', camera_center)
            depth = camera_center[-1]
            print('undistorted_points shape = ', undistorted_points.shape)
            print('image_points shape = ', image_points.shape)
            print('depth = ', depth)

            # 将 2D 图像点转换为齐次坐标
            #print(image_points.shape)
            image_points_homogeneous = np.hstack([image_points, np.ones((image_points.shape[0], 1), dtype=np.float32)])
            # 批量计算相机坐标系中的 3D 点 (N x 3 矩阵)
            camera_coordinates = (np.linalg.inv(params['camera_matrix']) @ image_points_homogeneous.T).T * depth

            # 将相机坐标转换为世界坐标 (N x 3 矩阵)
            world_coordinates = (np.linalg.inv(rotation_matrix) @ (camera_coordinates - translation_vector.T).T).T
            all_points_3d_world.append(world_coordinates)

    print('len 3d = ', len(all_points_3d_world))

    # adjusted_cam_params = []

    # for i, params in enumerate(cam_params):
    #     world_coordinates = all_points_3d_world[i]  # 获取该相机的 3D 点
    #     #print('world_coordinates = ', world_coordinates)

    #     # 将点集旋转到水平面，并获取旋转矩阵
    #     #aligned_points, rotation_to_horizontal = align_points_to_horizontal(world_coordinates)
    #     aligned_points, rotation_to_horizontal = align2ref(world_coordinates)
        
    #     print('aligned_points = ', rotation_matrix)
    #     print('rotation_to_horizontal = ', rotation_to_horizontal)

    #     # 更新相机的外参（旋转矩阵）
    #     params['rotation_matrix'] = rotation_to_horizontal @ params['rotation_matrix']
    #     adjusted_cam_params.append(params)

    #     # 更新校正后的 3D 点
    #     all_points_3d_world[i] = aligned_points
    
    # fig = plt.figure()
    # ax = fig.add_subplot(111, projection='3d')

    # for i, points in enumerate(all_points_3d_world):
    #     ax.scatter(points[:, 0], points[:, 1], points[:, 2], label=f'Cam {i}')

    # ax.set_xlabel('X')
    # ax.set_ylabel('Y')
    # ax.set_zlabel('Z')
    # plt.legend()
    # plt.show()

    # # 3. 保存调整后的相机参数，以备后续使用
    # with open(os.path.join('D:\\code\\pmd-python\\config\\', 'cam_params1023.pkl'), 'wb') as f:
    #     pickle.dump(adjusted_cam_params, f)
    

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    colors = ['r', 'g', 'b', 'c']  # 颜色列表

    for i in range(len(all_points_3d_world)):
        
        plot_3d_points(all_points_3d_world[i], label=f'Cam {i}', color=[random.random() for _ in range(3)], ax=ax)
        # 显示图例和更新当前图像
        ax.legend(loc='upper right')
        plt.pause(2)  # 暂停 1 秒，逐个显示效果

    
    # for cam_idx, (params, corners) in enumerate(zip(cam_params, all_corners)):
    #     #print('all_points_3d_world[cam_idx] = ', all_points_3d_world[cam_idx])
    #     plot_3d_points(all_points_3d_world[cam_idx], label=f'Cam {cam_idx}', color=colors[cam_idx], ax=ax)

    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
   # ax.set_zlim([0, 500])
    plt.legend()
    plt.show()

    #cam_params[3]['rotation_matrix'] = np.eye(3)
    #cam_params[3]['translation_vector'] = np.zeros((3, 1))

    # stereo_calibration_data_0_3 = stereo_calibrate(all_detections, 0, 3, object_point_single, cam_params)
    # stereo_calibration_data_1_3 = stereo_calibrate(all_detections, 1, 3, object_point_single, cam_params)
    # stereo_calibration_data_2_3 = stereo_calibrate(all_detections, 2, 3, object_point_single, cam_params)

    # stereo_calib_data_list = []
    # stereo_calib_data_list.append(stereo_calibration_data_0_3)
    # stereo_calib_data_list.append(stereo_calibration_data_1_3)
    # stereo_calib_data_list.append(stereo_calibration_data_2_3)


    #     # 设置相机 3 的外参不变
    # R_3 = cam_params[3]['rotation_matrix']
    # T_3 = cam_params[3]['translation_vector']

    # for i in [0, 1, 2]:
    #     # 获取相机 i 的初始外参
    #     R_i = cam_params[i]['rotation_matrix']
    #     T_i = cam_params[i]['translation_vector']
        
    #     # 计算初始相对位姿
    #     R_i3_init = R_3 @ R_i.T
    #     T_i3_init = -R_3 @ R_i.T @ T_i + T_3
        
    #     # 获取双目标定结果
    #     stereo_calib_data = stereo_calib_data_list[i]
    #     R_i3_stereo = stereo_calib_data['rotation_matrix']
    #     T_i3_stereo = stereo_calib_data['translation_vector']
        
    #     # 计算位姿差异
    #     delta_R = R_i3_stereo @ R_i3_init.T
    #     delta_T = T_i3_stereo - delta_R @ T_i3_init
        
    #     # 更新相机外参
    #     R_i_new = delta_R @ R_i
    #     T_i_new = delta_R @ T_i + delta_T
        
    #     # 更新 cam_params
    #     cam_params[i]['rotation_matrix'] = R_i_new
    #     cam_params[i]['translation_vector'] = T_i_new
    

    # with open('D:\\code\\pmd-python\\config\\stereo_calib2.pkl', 'wb') as pkl_file:
    #        pickle.dump(cam_params, pkl_file) 

    
def stereo_calibrate(all_detections, cam_id1, cam_id2, object_point_single, cam_params):

    detections1 = all_detections[cam_id1]
    detections2 = all_detections[cam_id2]
   
    detections_dict1 = {d.tag_id: d for d in detections1}
    detections_dict2 = {d.tag_id: d for d in detections2}

    # 用于保存所有图像的检测到的角点和 ID
    objpoints = []  # 3D 点
    imgpoints1 = []  # 相机1的 2D 点
    imgpoints2 = []  # 相机2的 2D 点
    image_size = None

    # 找到两个相机共同检测到的标签 ID
    common_tags = set(detections_dict1.keys()) & set(detections_dict2.keys())
    print('common_tags = ', common_tags)
    if common_tags:
        image_points1 = []
        image_points2 = []
        object_points = []

        for tag_id in common_tags:
            # 获取相机1的角点
            corners1 = detections_dict1[tag_id].corners.astype(np.float32)
            # 获取相机2的角点
            corners2 = detections_dict2[tag_id].corners.astype(np.float32)

            # 获取对应的 3D 点
            if tag_id < len(object_point_single):
                obj_pts = object_point_single[tag_id]
            else:
                print(f"Warning: Tag ID {tag_id} is out of range.")
                continue

            object_points.append(obj_pts)
            image_points1.append(corners1)
            image_points2.append(corners2)

        if object_points and image_points1 and image_points2:
            objpoints.append(np.array(object_points, dtype=np.float32).reshape(-1, 3))
            imgpoints1.append(np.array(image_points1, dtype=np.float32).reshape(-1, 2))
            imgpoints2.append(np.array(image_points2, dtype=np.float32).reshape(-1, 2))
        else:
            print("No valid common detections in this image pair.")
    else:
        print("No common tags detected in this image pair.")
    
    K1 = cam_params[cam_id1]['camera_matrix']
    dist1 = cam_params[cam_id1]['distortion_coefficients']
    K2 = cam_params[cam_id2]['camera_matrix']
    dist2 = cam_params[cam_id2]['distortion_coefficients']

    # 立体标定
    flags = 0
    flags |= cv2.CALIB_FIX_INTRINSIC  # 固定单目内参

    criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)

    ret, _, _, _, _, R, T, E, F = cv2.stereoCalibrate(
        objpoints,       # 3D 点
        imgpoints1,      # 相机1的 2D 点
        imgpoints2,      # 相机2的 2D 点
        K1,              # 相机1的内参
        dist1,           # 相机1的畸变系数
        K2,              # 相机2的内参
        dist2,           # 相机2的畸变系数
        image_size,      # 图像尺寸
        criteria=criteria,
        flags=flags
    )

    print(f"Stereo Calibration between Camera {cam_id1} and Camera {cam_id2} completed.")
    print(f"Rotation Matrix R:\n{R}")
    print(f"Translation Vector T:\n{T}")

    # 将结果保存到字典中
    stereo_calibration_data = {
        'camera_matrix1': K1,
        'distortion_coefficients1': dist1,
        'camera_matrix2': K2,
        'distortion_coefficients2': dist2,
        'rotation_matrix': R,
        'translation_vector': T,
        'essential_matrix': E,
        'fundamental_matrix': F,
        'reprojection_error': ret
    }

    # 返回标定结果
    print('stereo_calibration_data = ', stereo_calibration_data)

    return stereo_calibration_data


# ######################## bundle_adjustment
#     # 遍历每个相机
#     for cam_id, params in enumerate(cam_params):
#         K_matrices.append(params['camera_matrix'])
#         dist_coeffs.append(params['distortion_coefficients'])
        
#         camera_corners = []  # 用于当前相机的2D图像点
#         camera_object_points = []  # 用于当前相机的3D物体点

#         # 对每个相机的标定图像提取2D角点
#         for img_file in os.listdir(calibration_images_path[cam_id]):
#             # 读取图像并检测AprilTags，获取2D角点和对应的3D物体点
#             image_points, object_points, tag_lst = detect_april_tag_points(
#                 detector, calibration_images_path[cam_id] + img_file, object_point_single
#             )

#             # 将检测到的2D图像点和3D物体点保存到当前相机的列表
#             if image_points is not None and object_points is not None:
#                 camera_corners.append(image_points)
#                 camera_object_points.append(object_points)
        
#         # 保存当前相机的所有图像点到全局列表
#         all_corners.append(camera_corners)
#         all_object_points.append(camera_object_points)

#     # 调用捆绑调整函数，优化所有相机的内参、外参和3D点
#     plane_constraints = [
#     [0, 1, 2, 3],  # 第一个平面的点索引
#     [4, 5, 6, 7]   # 第二个平面的点索引
#     ]   
#     result = bundle_adjustment(all_object_points, all_corners, cam_params, plane_constraints)
    
#     print(result)
#     optimized_rvecs, optimized_tvecs = extract_params_from_result(result, 4, len(all_object_points))
#     original_rvecs = [cv2.Rodrigues(params['rotation_matrix'])[0] for params in cam_params]
#     original_tvecs = [params['translation_vector'] for params in cam_params]

#     print('optimized_rvecs = ',optimized_rvecs)
    
#     for i in range(4):
#         optimized_rotation_matrices = cv2.Rodrigues(optimized_rvecs[i])[0]
#         #print(original_tvecs[i], optimized_tvecs[i].reshape(-1, 1))
#         calibration_data = {
#         'camera_matrix': cam_params[i]['camera_matrix'],
#         'distortion_coefficients': cam_params[i]['distortion_coefficients'],
#         'rotation_matrix': optimized_rotation_matrices,
#         'translation_vector': optimized_tvecs[i].reshape(-1, 1)
#         }
        
#         optimized_cam_params.append(calibration_data)

#     with open('D:\\code\\pmd-python\\config\\optimized_param1021.pkl', 'wb') as pkl_file:
        
#         pickle.dump(optimized_cam_params, pkl_file)
    
#     # 打印优化后的结果
#     print("Bundle Adjustment 完成!")


    # #绘制相机
    # 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])

    # plt.show()

def plt_gaussion():

    # 1. Create a Gaussian surface
    X = np.linspace(-3, 3, 100)
    Y = np.linspace(-3, 3, 100)
    X, Y = np.meshgrid(X, Y)
    Z = np.exp(-(X**2 + Y**2))

    # 2. Compute gradients
    Zx, Zy = np.gradient(Z)

    # 3. Plotting the Gaussian surface
    fig = plt.figure(figsize=(14, 6))

    # Plot 1: Gaussian Surface
    ax1 = fig.add_subplot(121, projection='3d')
    ax1.plot_surface(X, Y, Z, cmap='viridis', edgecolor='none')
    ax1.set_title('Gaussian Surface')
    ax1.set_xlabel('X')
    ax1.set_ylabel('Y')
    ax1.set_zlabel('Z')

    # Plot 2: Gradient Field Visualization
    ax2 = fig.add_subplot(122)
    ax2.quiver(X, Y, Zx, Zy, color='r')
    ax2.set_title('Gradient Field of Gaussian Surface')
    ax2.set_xlabel('X')
    ax2.set_ylabel('Y')

    plt.tight_layout()
    plt.show()



def test_aprilgrid():
    
    dll_path = "C:\\Users\\zhang\\AppData\\Local\\Programs\\Python\\Python312\\Lib\\site-packages\\pupil_apriltags\\lib\\apriltag.dll"
    if not os.path.exists(dll_path):
        raise FileNotFoundError(f"{dll_path} 不存在")
   

    ctypes.WinDLL(dll_path, winmode=0)
    detector = apriltag.Detector(
        families="tag36h11",  # 确保与标签类型一致
        nthreads=4,           # 多线程加速
        quad_decimate=0.5,    # 降采样因子，调小提高检测精度
        quad_sigma=0.0,       # 高斯模糊，处理噪声图像
        refine_edges=True     # 边缘细化
    )
    #detector = Detector('t36h11')

    
    #os.add_dll_directory(dll_path)
    
    #img_file = 'D:\\data\\one_cam\\calibration-1029\\screen\\Image_20241029170810995.bmp'
    #img_file = 'C:\\Users\\zhang\\Desktop\\Image_20241029190420495.bmp'
    
    img_dir = 'D:\\data\\1030\\'
    for img_name in os.listdir(img_dir):
        img_file = os.path.join(img_dir, img_name)

        # 从文件读取并解码图像
        image_data = np.fromfile(img_file, dtype=np.uint8)
        img = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
        
        image_size = None

        if image_size is None:
            image_size = (img.shape[1], img.shape[0])  # 保存图像尺寸
        
        # 转换为灰度图像
        img = cv2.flip(img, 0)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # 检测 AprilTags
        detections = detector.detect(gray)
        #print(detections)
        # for detection in detections:
        #     corners = detection.corners
        #     tag_id = detection.tag_id
        #     if tag_id == 0 or tag_id == 1: 
        #         print(tag_id, corners)
        if 1:
        # from calibration.get_camera_params import show_detect_result
            show_detect_result_new(img, detections)
            #show_detect_result(img, detections)





def stack_apriltag(rows, cols):
    # 设置图片路径和目标大图的输出路径
    input_folder = 'C:\\Users\\zhang\\Desktop\\apriltag'  # 替换为你的图片文件夹路径
    output_image_path = 'D:\\data\\0output_image.png'
    output_image_path1 = 'D:\\data\\0output_image_' + str(rows) + 'x' + str(cols) + '.png'

    # 获取并按顺序生成每张图片的路径
    images = [
        os.path.join(input_folder, f'tag36h11_{i}.png')
        for i in range(rows * cols)
    ]

    # 检查是否找到足够数量的图片
    missing_files = [img for img in images if not os.path.exists(img)]
    if missing_files:
        raise FileNotFoundError(f"以下图片未找到: {missing_files}")

    # 打开第一张图片来确定每张小图的大小
    with Image.open(images[0]) as img:
        width, height = img.size

    # 创建大图，大小为 (cols * width) x (rows * height)
    output_image = Image.new('RGB', (cols * width, rows * height))

    # 拼接图片到大图中
    for index, image_path in enumerate(images):
        with Image.open(image_path) as img:
            row = index // cols
            col = index % cols
            output_image.paste(img, (col * width, row * height))

    # 保存拼接好的大图
    output_image.save(output_image_path)
    print(f"拼接完成！大图已保存为 {output_image_path}")

    # 计算等比例缩放后的尺寸，使其尽可能适应 1920x1080，但不改变比例
    aspect_ratio = output_image.width / output_image.height
    target_width, target_height = 1920, 1080

    if aspect_ratio > (target_width / target_height):
        # 图像更宽，按照宽度进行缩放
        new_width = target_width
        new_height = int(target_width / aspect_ratio)
    else:
        # 图像更高，按照高度进行缩放
        new_height = target_height
        new_width = int(target_height * aspect_ratio)

    resized_image = output_image.resize((new_width, new_height), Image.LANCZOS)

    # 创建 1920x1080 的黑色背景图
    final_image = Image.new('RGB', (target_width, target_height), (0, 0, 0))

    # 计算粘贴位置，使图像居中
    paste_x = (target_width - new_width) // 2
    paste_y = (target_height - new_height) // 2
    final_image.paste(resized_image, (paste_x, paste_y))

    # 保存最终的大图
    final_image.save(output_image_path1)
    print(f"缩放并保存完成！小图已保存为 {output_image_path1}")


def stack_apriltag2(rows, cols):
    # 设置图片路径和目标大图的输出路径
    input_folder = 'C:\\Users\\zhang\\Desktop\\apriltag'  
    output_image_path = 'D:\\data\\2output_image_pad.png'
    output_image_path1 = 'D:\\data\\2output_image_pad_' + str(rows) + 'x' + str(cols) + '.bmp'
    #target_width, target_height = 1920, 1080
    target_width, target_height = 2360, 1640

    # 获取并按顺序生成每张图片的路径
    images = [
        os.path.join(input_folder, f'tag36h11_{i}.png')
        for i in range(rows * cols)
    ]

    # 检查是否找到足够数量的图片
    missing_files = [img for img in images if not os.path.exists(img)]
    if missing_files:
        raise FileNotFoundError(f"以下图片未找到: {missing_files}")

    # 打开第一张图片来确定每张小图的大小
    first_image = cv2.imread(images[0])
    height, width, _ = first_image.shape

    # 创建大图，大小为 (cols * width) x (rows * height)
    output_image = np.zeros((rows * height, cols * width, 3), dtype=np.uint8)

    # 拼接图片到大图中
    for index, image_path in enumerate(images):
        img = cv2.imread(image_path)
        img = cv2.flip(img, 0)  # 垂直翻转图片
        row = index // cols
        col = index % cols
        output_image[row*height:(row+1)*height, col*width:(col+1)*width] = img

    # 保存拼接好的大图
    cv2.imwrite(output_image_path, output_image)
    print(f"拼接完成！大图已保存为 {output_image_path}")

    # 计算等比例缩放后的尺寸，使其尽可能适应 1920x1080，但不改变比例
    
    aspect_ratio = output_image.shape[1] / output_image.shape[0]
    if aspect_ratio > (target_width / target_height):
        # 图像更宽，按照宽度进行缩放
        new_width = target_width
        new_height = int(target_width / aspect_ratio)
    else:
        # 图像更高，按照高度进行缩放
        new_height = target_height
        new_width = int(target_height * aspect_ratio)

    resized_image = cv2.resize(output_image, (new_width, new_height), interpolation=cv2.INTER_LANCZOS4)

    # 创建 1920x1080 的黑色背景图
    final_image = np.zeros((target_height, target_width, 3), dtype=np.uint8)  # 注意这里大小的变化

    # 计算粘贴位置，使图像居中
    paste_x = (target_width - new_width) // 2
    paste_y = (target_height - new_height) // 2
    final_image[paste_y:paste_y+new_height, paste_x:paste_x+new_width] = resized_image

    # 保存最终的大图
    cv2.imwrite(output_image_path1, final_image)
    print(f"缩放并保存完成！小图已保存为 {output_image_path1}")




def stack_apriltag1(rows, cols):
    # 设置图片路径和目标大图的输出路径
    input_folder = 'C:\\Users\\zhang\\Desktop\\apriltag'  # 替换为你的图片文件夹路径
    output_image_path = 'D:\\data\\output_image.png'
    output_image_path1 = 'D:\\data\\output_image_' + str(rows) + 'x' + str(cols) + '.bmp'

    # 获取并按顺序生成每张图片的路径
    images = [
        os.path.join(input_folder, f'tag36h11_{i}.png')
        for i in range(rows * cols)
    ]

    # 检查是否找到足够数量的图片
    missing_files = [img for img in images if not os.path.exists(img)]
    if missing_files:
        raise FileNotFoundError(f"以下图片未找到: {missing_files}")

    # 打开第一张图片来确定每张小图的大小
    first_image = cv2.imread(images[0])
    height, width, _ = first_image.shape

    # 创建大图，大小为 (cols * width) x (rows * height)
    output_image = np.zeros((rows * height, cols * width, 3), dtype=np.uint8)

    # 拼接图片到大图中
    for index, image_path in enumerate(images):
        img = cv2.imread(image_path)
        img = cv2.flip(img, 1)  # 水平翻转图片
        row = index // cols
        col = index % cols
        output_image[(rows-row-1)*height:(rows-row)*height, (cols-col-1)*width:(cols-col)*width] = img

    # 保存拼接好的大图
    cv2.imwrite(output_image_path, output_image)
    print(f"拼接完成！大图已保存为 {output_image_path}")

    # 计算等比例缩放后的尺寸，使其尽可能适应 1920x1080，但不改变比例
    target_width, target_height = 1920, 1080
    aspect_ratio = output_image.shape[1] / output_image.shape[0]
    if aspect_ratio > (target_width / target_height):
        # 图像更宽，按照宽度进行缩放
        new_width = target_width
        new_height = int(target_width / aspect_ratio)
    else:
        # 图像更高，按照高度进行缩放
        new_height = target_height
        new_width = int(target_height * aspect_ratio)

    resized_image = cv2.resize(output_image, (new_width, new_height), interpolation=cv2.INTER_LANCZOS4)

    # 创建 1920x1080 的黑色背景图
    final_image = np.zeros((target_height, target_width, 3), dtype=np.uint8)  # 注意这里大小的变化

    # 计算粘贴位置，使图像居中
    paste_x = (target_width - new_width) // 2
    paste_y = (target_height - new_height) // 2
    final_image[paste_y:paste_y+new_height, paste_x:paste_x+new_width] = resized_image

    # 保存最终的大图
    cv2.imwrite(output_image_path1, final_image)
    print(f"缩放并保存完成！小图已保存为 {output_image_path1}")


def cal_pad_pixel():
    # Given values
    diagonal_inch = 10.86  # diagonal length in inches
    width_pixels = 2360
    height_pixels = 1640

    # Calculate the aspect ratio constant k using the Pythagorean theorem
    k = diagonal_inch / math.sqrt(width_pixels**2 + height_pixels**2)

    # Calculate width and height in inches
    width_inch = k * width_pixels
    height_inch = k * height_pixels

    # Convert the width and height to millimeters (1 inch = 25.4 mm)
    width_mm = width_inch * 25.4
    height_mm = height_inch * 25.4

    # Calculate the size of each pixel in millimeters
    pixel_width_mm = width_mm / width_pixels
    pixel_height_mm = height_mm / height_pixels

    print(pixel_width_mm, pixel_height_mm)


def cvt_image():
    input_dir = 'D:\\code\\code of PMD\\code of PMD\\hud1\\24\\'
    output_dir = 'D:\\code\\code of PMD\\code of PMD\\hud1\\8\\'
     
    for img in os.listdir(input_dir):
        image = cv2.imread(input_dir + img, 0)
        height, width = image.shape
    
        # 计算中心四分之一区域的边界
        x1 = width // 4
        x2 = 3 * width // 4
        y1 = height // 4
        y2 = 3 * height // 4
        # 再次减半以保留中心八分之一
        x1 = x1 + (x2 - x1) // 4
        x2 = x2 - (x2 - x1) // 4
        y1 = y1 + (y2 - y1) // 4
        y2 = y2 - (y2 - y1) // 4
        # x1 = int(width/2) - 200
        # x2 = int(width/2) + 200
        # y1 = 280
      
        # 创建一个黑色图像
        black_image = np.zeros_like(image)
    
        # 将中心四分之一区域复制到黑色图像上
        black_image[y1:y2, x1:x2] = image[y1:y2, x1:x2]
        cv2.imwrite(output_dir + img, image)
        cv2.namedWindow('src', 0)
        cv2.imshow('src', image)
        cv2.waitKey(10) 


def pixel_to_world(pixel_coords, depth, K, R, t):
    """
    将像素坐标转换为世界坐标
    :param pixel_coords: 像素坐标 (u, v)
    :param depth: 深度值
    :param K: 内参矩阵 (3x3)
    :param R: 旋转矩阵 (3x3)
    :param t: 平移向量 (3x1)
    :return: 世界坐标 (X, Y, Z)
    """
    # 将像素坐标 (u, v) 转为齐次坐标 (u, v, 1)
    uv1 = np.array([pixel_coords[0], pixel_coords[1], 1])
    
    # 反投影到相机坐标系
    K_inv = np.linalg.inv(K)
    cam_coords = depth * (K_inv @ uv1)
    
    # 将相机坐标转换到世界坐标
    R_inv = np.linalg.inv(R)
    world_coords = R_inv @ (cam_coords - t)
    
    return world_coords


def calculate_distance(pixel1, pixel2, depth1, depth2, K, R, t):
    """
    计算两像素点在世界坐标系中的距离
    :param pixel1: 第一个像素点坐标 (u1, v1)
    :param pixel2: 第二个像素点坐标 (u2, v2)
    :param depth1: 第一个点的深度值
    :param depth2: 第二个点的深度值
    :param K: 内参矩阵
    :param R: 外参旋转矩阵
    :param t: 外参平移向量
    :return: 两点之间的实际距离
    """
    # 转换两点到世界坐标
    world_point1 = pixel_to_world(pixel1, depth1, K, R, t)
    world_point2 = pixel_to_world(pixel2, depth2, K, R, t)
    print(world_point1, world_point2)
    
    # 计算欧几里得距离
    distance = np.linalg.norm(world_point1 - world_point2)
    return distance


def pixel_to_world_on_plane(u, v, K, r_vector, t, h):
    """
    将像素点 (u, v) 转换到世界坐标系中，假设物体位于 z=h 的平面上
    :param u: 像素点横坐标
    :param v: 像素点纵坐标
    :param K: 相机内参矩阵 (3x3)
    :param R: 外参旋转矩阵 (3x3)
    :param t: 外参平移向量 (3x1)
    :param h: 平面高度（z=h）
    :return: 世界坐标 (X_w, Y_w, h) 和深度值（点到相机光心的距离）
    """
    # 计算归一化相机坐标 (3x1)
    R = cv2.Rodrigues(np.array(r_vector).reshape(3,1))[0]
    K_inv = np.linalg.inv(K)
    pixel_coords = np.array([u, v, 1])  # 3x1
    normalized_coords = K_inv @ pixel_coords  # (3,)

    # 调整为列向量 (3, 1)
    normalized_coords = normalized_coords[:, np.newaxis]

    # 相机光心在世界坐标系中的位置 (3x1)
    camera_center = -np.linalg.inv(R) @ t  # (3, 1)

    #print('camera_center = ', camera_center)

    # 光线方向 (3, 1)
    ray_dir = np.linalg.inv(R) @ normalized_coords  # (3, 1)

    # 求解射线与 z=h 平面的交点 (标量 lambda)
    lambda_ = (h - camera_center[2]) / ray_dir[2]

    # 世界坐标 (3x1)
    world_coords = camera_center + lambda_ * ray_dir
    #print('world_coords = ', world_coords)
    #world_coords[2] = h  # 强制设定 z=h

    # 返回为一维数组 (3,) 或者列向量 (3, 1)
    return world_coords.flatten()


def get_world_point(K, R, T, contour_points, d):
    A = K
    Rw2c = R
    Rc2w = np.linalg.inv(Rw2c)
    Tw2c = T
    Tc2w = -np.dot(np.linalg.inv(Rw2c), Tw2c)
    world_points_contours = affine_img2world(contour_points, A, Rc2w, Tc2w, d)
    return world_points_contours


def camera2world_vis():
    # cam_param_path = 'D:\\code\\pmd-python\\config\\cam_params_1125.pkl'
    img_path = 'D:\\data\\one_cam\\1226image\\calibration\\cam\\Image_20241226135705692.bmp'
    #img_path = 'D:\\data\\one_cam\\pad-test-1125\\test3\\calibration\\screen\\Image_20241128190133634.bmp'
    # with open(cam_param_path, 'rb') as pkl_file:
    #     cam_params = pickle.load(pkl_file)
    matlab_calib_json_path = ('D:\\code\\code of PMD\\code of PMD\\cameraParams_1_cam0103.json')
    #matlab_calib_json_path = ('D:\\code\\code of PMD\\code of PMD\\cameraParams_1227cam3.json')
    camera_pkl_path = 'D:\\code\\pmd-python\\config\\camera_params_1227cam3.pkl'
    screen_pkl_path = 'D:\\code\\pmd-python\\config\\screen_params_1226_1.pkl'
    
    screen = 0
    circle = 0
    
    if screen:
        with open(camera_pkl_path, 'rb') as pkl_file:
            cam_params = pickle.load(pkl_file)
        with open(matlab_calib_json_path, 'r', encoding='utf-8') as json_file:
            screen_params = json.load(json_file)
    
    if not screen:
        with open(matlab_calib_json_path, 'r', encoding='utf-8') as json_file:
            cam_params = json.load(json_file)
        #x_scale, y_scale = 0.98829, 0.988425
        #x_scale, y_scale = 0.987255, 0.987355
        #x_scale, y_scale = 0.98699, 0.98670
        x_scale, y_scale = 0.993665, 0.9934
        x_scale, y_scale = 1.0023, 0.998
        x_scale, y_scale = 1, 1 #cam0

        cam_params['camera_matrix'][0][0]= cam_params['camera_matrix'][0][0] * x_scale
        cam_params['camera_matrix'][1][1]= cam_params['camera_matrix'][1][1] * y_scale
        dist_rdist = cam_params['dist_rdist']
        dist_tdist = cam_params['dist_tdist']
        r_vector = cam_params['rotation_vectors'][-1]
        K = np.array(cam_params['camera_matrix'])
        R = cv2.Rodrigues(np.array(r_vector).reshape(3,1))[0]
        T = np.array(cam_params['translation_vectors'][-1]).reshape(3,1)
        print('dist_rdist = ', dist_rdist)
        print('dist_tdist = ', dist_tdist)
        print(dist_rdist[0])
        dist_coeffs = np.array([dist_rdist[0] , dist_rdist[1] , dist_tdist[0], dist_tdist[1], 0]) 
       
        print('K = ',K)
        print('rotation_matrix = ', R)
        print('r_vector = ', r_vector)
        print('t = ', T)
        print('dist_rdist = ', dist_rdist)
        print('dist_tdist = ', dist_tdist)

        with open(camera_pkl_path, 'wb') as pkl_file:
            cam_params_pkl = {
                'camera_matrix': K,
                'distortion_coefficients': dist_coeffs,
                'rotation_matrix': R,
                'rotation_vector': r_vector,
                'translation_vector': T,
            }
            print('cam_params = ', cam_params_pkl)
            cam_data = []
            cam_data.append(cam_params_pkl)
            pickle.dump(cam_data, pkl_file)
    
    if screen:
        with open(screen_pkl_path, 'wb') as pkl_file:
            r_vector = screen_params['rotation_vectors'][-1]
            screen_distortion = cam_params[0]['distortion_coefficients']
            K = cam_params[0]['camera_matrix'] #same matrix for same camera
            R = cv2.Rodrigues(np.array(r_vector).reshape(3,1))[0]
            T = np.array(screen_params['translation_vectors'][-1]).reshape(3,1)
            dist_coeffs = cam_params[0]['distortion_coefficients']

            print('K = ', K)
            print('R = ', R)
            print('T = ', T)
            
            T[2] = T[2] + 9
            print('T = ', T)

            screen_params = {
                'screen_matrix': K,
                'screen_distortion': screen_distortion,
                'screen_rotation_matrix': R,
                'screen_rotation_vector': r_vector,
                'screen_translation_vector': T
            }
           
            pickle.dump(screen_params, pkl_file)

    
    img = cv2.imread(img_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    chessboard_size = [8, 8]
    img_size = img.shape
    print('raw size = ',img_size)

    h, w = img.shape[:2]
    new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(K, dist_coeffs, (w, h), 1, (w, h))
    undistorted_image = cv2.undistort(gray, K, dist_coeffs, None, new_camera_matrix)
    undistorted_show = cv2.undistort(img, K, dist_coeffs, None, new_camera_matrix)

    print('new_camera_matrix = ', new_camera_matrix)
    print('K = ', K)

    if circle:
        circle_w, circle_h = 8, 6
        chessboard_size = [8, 6]
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        params = cv2.SimpleBlobDetector_Params()
        params.maxArea = 10e4
        params.minArea = 10
        params.minDistBetweenBlobs = 5
        blobDetector = cv2.SimpleBlobDetector_create(params)
        ret, corners = cv2.findCirclesGrid(undistorted_image, (circle_w, circle_h), cv2.CALIB_CB_SYMMETRIC_GRID, blobDetector, None)
        print('ret = ', ret)
        if ret:
            corners_refined = cv2.cornerSubPix(undistorted_image, corners, (circle_w, circle_h), (-1, -1), criteria)
            cv2.drawChessboardCorners(undistorted_show, (circle_w, circle_h), corners_refined, ret)

            cv2.namedWindow('Detected Circles', 0)
            cv2.imshow("Detected Circles", undistorted_show) 
            cv2.waitKey(0) 
            cv2.destroyAllWindows()

        # json_path = 'D:\\code\\code of PMD\\code of PMD\\circle_imagePoints.json'
        # json_data = json.load(open(json_path, 'r'))
        # imagePoints = np.array(json_data['imagePoints'])
        # last_image_points = imagePoints[:, :, -1]
        # print(last_image_points)
        # for pt in last_image_points:
        #     cv2.circle(img, (int(pt[0]), int(pt[1])),  2, (255,0,255), 2)
       
        # corners_refined = last_image_points
        
        # cv2.namedWindow('matlab', 0)
        # cv2.imshow('matlab', img)
        # #cv2.imwrite('vis_chessboard.png', undistorted_show)
        # cv2.waitKey(0) 
       
    else:
        # x, y, w, h = roi
        # print(x, y, w, h)
        # undistorted_image = undistorted_image[y:y+h, x:x+w]
        # print(undistorted_image.shape)
        
        # 寻找棋盘格角点
        #ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
        flag = cv2.CALIB_CB_EXHAUSTIVE | cv2.CALIB_CB_ACCURACY
        ret, corners = cv2.findChessboardCorners(undistorted_image, chessboard_size, flag)
        print('ret = ', ret)

        # 如果找到足够的角点，添加到列表中
        if ret:
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
            corners_refined = cv2.cornerSubPix(undistorted_image, corners, (11, 11), (-1, -1), criteria)
            img = cv2.drawChessboardCorners(undistorted_image, chessboard_size, corners_refined, ret)
    world_coords = [
        np.array(pixel_to_world_on_plane(u, v, K, r_vector, T, 0)) for u, v in corners_refined.squeeze()
    ] 
   
    corners_refined = corners_refined.squeeze()
    # print('corners_refined = ', corners_refined.shape)

    # 计算水平和垂直方向相邻角点的距离
    horizontal_distances = []
    vertical_distances = []
    cols, rows = chessboard_size

    font_size = 0.9
    font_thickness = 1

    for i in range(chessboard_size[1]):  # 遍历每一行
        for j in range(chessboard_size[0] - 1):  # 水平相邻点
            idx1 = i * chessboard_size[0] + j
            idx2 = idx1 + 1
            distance = np.linalg.norm(world_coords[idx1] - world_coords[idx2])
            horizontal_distances.append(distance)
            x, y =int(corners_refined[idx1][0]),  int(corners_refined[idx1][1])
            distance_text = f"{distance:.4f}"
            cv2.putText(img, distance_text, (x - 30, y ), cv2.FONT_HERSHEY_SIMPLEX, font_size, (0, 255, 0), font_thickness)

    for i in range(chessboard_size[1] - 1):  # 垂直相邻点
        for j in range(chessboard_size[0]):  # 遍历每一列
            idx1 = i * chessboard_size[0] + j
            idx2 = idx1 + chessboard_size[0]
            distance = np.linalg.norm(world_coords[idx1] - world_coords[idx2])
            vertical_distances.append(distance)
            x, y = int(corners_refined[idx1][0]), int(corners_refined[idx1][1])
            distance_text = f"{distance:.4f}"
            cv2.putText(img, distance_text, (x, y - 30), cv2.FONT_HERSHEY_SIMPLEX, font_size, (255, 0, 255), font_thickness)

    print("水平方向相邻角点距离:", np.mean(horizontal_distances))
    print("垂直方向相邻角点距离:", np.mean(vertical_distances))
    
    cv2.namedWindow('src', 0)
    cv2.imshow('src', img)
    cv2.imwrite('vis_chessboard.png', img)
    cv2.waitKey(0) 



# 每四个元素为一组计算相邻点的xy的欧式距离，包括第四个点和第一个点
def calculate_distances(coords):
    distances = []
    for i in range(0, len(coords), 4):
        group = coords[i:i+4]
        if len(group) < 4:
            break
        d1 = np.linalg.norm(group[0][:2] - group[1][:2])
        d2 = np.linalg.norm(group[1][:2] - group[2][:2])
        d3 = np.linalg.norm(group[2][:2] - group[3][:2])
        d4 = np.linalg.norm(group[3][:2] - group[0][:2])
        distances.append([d1, d2, d3, d4])
    return distances


def camera2world_aprilgrid_vis():
    # cam_param_path = 'D:\\code\\pmd-python\\config\\cam_params_1125.pkl'
    img_path = 'D:\\data\\four_cam\\calibrate\\calibrate-1016\\cam3\\Image_20241016171139414.bmp'
    #img_path = 'D:\\data\\one_cam\\pad-test-1125\\test3\\calibration\\screen\\Image_20241128190133634.bmp'
    # with open(cam_param_path, 'rb') as pkl_file:
    #     cam_params = pickle.load(pkl_file)
    # matlab_calib_json_path = ('D:\\code\\code of PMD\\code of PMD\\camera_calibration2-cam-1129.json')
    # matlab_calib_json_path = ('D:\\code\\code of PMD\\code of PMD\\cameraParams_cam_screen_1216_4-1.json')
    camera_pkl_path = 'D:\\code\\pmd-python\\config\\cam_params_4.pkl'
    camera_pkl_path = 'D:\\code\\pmd-python\\config\\camera_params_1216_4_1.pkl'
    screen_pkl_path = None
    
    screen = 0
    circle = 0
    chessboard = 0
    aprilgrid = True
    
    # if screen:
    #     with open(camera_pkl_path, 'rb') as pkl_file:
    #         cam_params = pickle.load(pkl_file)
    #     with open(matlab_calib_json_path, 'r', encoding='utf-8') as json_file:
    #         screen_params = json.load(json_file)
    
    if not screen:
        with open(camera_pkl_path, 'rb') as pkl_file:
            cam_params = pickle.load(pkl_file)

        print('cam_params = ', cam_params)
        #return 0
       
        x_scale, y_scale = 0.995291, 0.995291  #cam0
        x_scale, y_scale = 0.993975, 0.993975  #cam1 
        x_scale, y_scale = 0.993085, 0.993085  #cam2
        x_scale, y_scale = 0.994463, 0.994463  #cam3

        print('cam_params = ', cam_params)

        cam_params['camera_matrix'][0][0]= cam_params['camera_matrix'][0][0] * x_scale
        cam_params['camera_matrix'][1][1]= cam_params['camera_matrix'][1][1] * y_scale
       
        K = np.array(cam_params['camera_matrix'])
        R = np.array(cam_params['rotation_matrix'])
        T = np.array(cam_params['translation_vector'])
        dist_coeffs = np.array(cam_params['distortion_coefficients'])
        print('K = ',K)
        print('rotation_matrix = ', R)
        print('t = ', T)
        print('dist_coeffs = ', dist_coeffs)

    
    img = cv2.imread(img_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    chessboard_size = [6, 8]
    img_size = img.shape
    print('raw size = ',img_size)

    h, w = img.shape[:2]
    new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(K, dist_coeffs, (w, h), 1, (w, h))
    undistorted_image = cv2.undistort(gray, K, dist_coeffs, None, new_camera_matrix)
    undistorted_show = cv2.undistort(img, K, dist_coeffs, None, new_camera_matrix)

    if circle:
        circle_w, circle_h = 6, 8
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        params = cv2.SimpleBlobDetector_Params()
        params.maxArea = 10e4
        params.minArea = 10
        params.minDistBetweenBlobs = 5
        blobDetector = cv2.SimpleBlobDetector_create(params)
        ret, corners = cv2.findCirclesGrid(undistorted_image, (circle_w, circle_h), cv2.CALIB_CB_SYMMETRIC_GRID, blobDetector, None)
        if ret:
            corners_refined = cv2.cornerSubPix(undistorted_image, corners, (circle_w, circle_h), (-1, -1), criteria)
            cv2.drawChessboardCorners(undistorted_show, (circle_w, circle_h), corners_refined, ret)

            cv2.namedWindow('Detected Circles', 0)
            cv2.imshow("Detected Circles", undistorted_show) 
            cv2.waitKey(0) 
            cv2.destroyAllWindows()

        # json_path = 'D:\\code\\code of PMD\\code of PMD\\circle_imagePoints.json'
        # json_data = json.load(open(json_path, 'r'))
        # imagePoints = np.array(json_data['imagePoints'])
        # last_image_points = imagePoints[:, :, -1]
        # print(last_image_points)
        # for pt in last_image_points:
        #     cv2.circle(img, (int(pt[0]), int(pt[1])),  2, (255,0,255), 2)
       
        # corners_refined = last_image_points
        
        # cv2.namedWindow('matlab', 0)
        # cv2.imshow('matlab', img)
        # #cv2.imwrite('vis_chessboard.png', undistorted_show)
        # cv2.waitKey(0) 
       
    elif chessboard:
        # x, y, w, h = roi
        # print(x, y, w, h)
        # undistorted_image = undistorted_image[y:y+h, x:x+w]
        # print(undistorted_image.shape)
        
        # 寻找棋盘格角点
        #ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
        flag = cv2.CALIB_CB_EXHAUSTIVE | cv2.CALIB_CB_ACCURACY
        ret, corners = cv2.findChessboardCorners(undistorted_image, chessboard_size, flag)
        print('ret = ', ret)

        # 如果找到足够的角点，添加到列表中
        if ret:
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
            corners_refined = cv2.cornerSubPix(undistorted_image, corners, (11, 11), (-1, -1), criteria)
            img = cv2.drawChessboardCorners(undistorted_image, chessboard_size, corners_refined, ret)
    elif aprilgrid:
        detector = Detector('t36h11')
        detections = detector.detect(gray)
        corners_refined = []

        # 遍历每个检测到的标签
        for detection in detections:
            corners = detection.corners  # 检测到的角点 (4, 2)
            tag_id = detection.tag_id    # 标签 ID
            #print(tag_id, corners)
            corners_refined.append(corners)

    corners_refined = np.array(corners_refined).reshape(len(corners_refined) * 4, 2)
    print('corners_refined = ', corners_refined.shape)
    world_coords = [
        np.array(pixel_to_world_on_plane(u, v, K, R, T, 0)) for u, v in corners_refined
    ] 
   
    corners_refined = corners_refined.squeeze()
    for pt in corners_refined:
        cv2.circle(img, (int(pt[0]), int(pt[1])), 3, (255,0,255), 2)

    distances = calculate_distances(world_coords)
    print(np.mean(distances))
    # cv2.namedWindow('src', 0)
    # cv2.imshow('src', img)
    
    # cv2.waitKey(0) 

    # print('corners_refined = ', corners_refined.shape)

    # 计算水平和垂直方向相邻角点的距离
    # horizontal_distances = []
    # vertical_distances = []
    # cols, rows = chessboard_size

    # font_size = 1.9
    # font_thickness = 2

    # for i in range(chessboard_size[1]):  # 遍历每一行
    #     for j in range(chessboard_size[0] - 1):  # 水平相邻点
    #         idx1 = i * chessboard_size[0] + j
    #         idx2 = idx1 + 1
    #         distance = np.linalg.norm(world_coords[idx1] - world_coords[idx2])
    #         horizontal_distances.append(distance)
    #         x, y =int(corners_refined[idx1][0]),  int(corners_refined[idx1][1])
    #         distance_text = f"{distance:.4f}"
    #         #cv2.putText(img, distance_text, (x - 30, y ), cv2.FONT_HERSHEY_SIMPLEX, font_size, (0, 255, 0), font_thickness)

    # for i in range(chessboard_size[1] - 1):  # 垂直相邻点
    #     for j in range(chessboard_size[0]):  # 遍历每一列
    #         idx1 = i * chessboard_size[0] + j
    #         idx2 = idx1 + chessboard_size[0]
    #         distance = np.linalg.norm(world_coords[idx1] - world_coords[idx2])
    #         vertical_distances.append(distance)
    #         x, y = int(corners_refined[idx1][0]), int(corners_refined[idx1][1])
    #         distance_text = f"{distance:.4f}"
    #         #cv2.putText(img, distance_text, (x, y - 30), cv2.FONT_HERSHEY_SIMPLEX, font_size, (255, 0, 255), font_thickness)

    # print("水平方向相邻角点距离:", np.mean(horizontal_distances))
    # print("垂直方向相邻角点距离:", np.mean(vertical_distances))
    
   



def generate_fringe_patterns(width, height, shifts, frequency=1, orientation='horizontal'):
    patterns = []
    for i in range(shifts):
        shift = (i * 2 * np.pi) / shifts
        if orientation == 'horizontal':
            x = np.linspace(0, 2 * np.pi * frequency, width) + shift
            pattern = np.sin(x)
            pattern = np.tile(pattern, (height, 1))
            # cv2.namedWindow('hpattern', 0)
            # cv2.imshow('hpattern', pattern)
            # cv2.waitKey(0) 
        elif orientation == 'vertical':
            y = np.linspace(0, 2 * np.pi * frequency, height) + shift
            pattern = np.sin(y)
            pattern = np.tile(pattern.reshape(-1, 1), (1, width))
            # cv2.namedWindow('vpattern', 0)
            # cv2.imshow('vpattern', pattern)
            # cv2.waitKey(0) 
        patterns.append(pattern)
    return patterns

def calculate_phase(images):
    shifts = len(images)
    sin_sum = np.zeros_like(images[0])
    cos_sum = np.zeros_like(images[0])
    
    for i in range(shifts):
        sin_sum += images[i] * np.sin(2 * np.pi * i / shifts)
        cos_sum += images[i] * np.cos(2 * np.pi * i / shifts)
    
    phase = np.arctan2(sin_sum, cos_sum)
    
    unwrapped_phase = unwrap_phase(phase)
    return unwrapped_phase

def extract_cut_lines(phase_map, threshold):
    
    horizontal_lines = np.argwhere(np.abs(np.diff(np.sign(phase_map - threshold), axis=1)) > 0)
    vertical_lines = np.argwhere(np.abs(np.diff(np.sign(phase_map - threshold), axis=0)) > 0)    
    
    return horizontal_lines, vertical_lines

def find_intersections(horizontal_lines, vertical_lines, tolerance=2):
    intersections = []
    vertical_set = set(map(tuple, vertical_lines))
    
    for h_line in horizontal_lines:
        y, x = h_line
        for dy in range(-tolerance, tolerance + 1):
            for dx in range(-tolerance, tolerance + 1):
                if (y + dy, x + dx) in vertical_set:
                    intersections.append([y, x])

                    break
                    
    return np.array(intersections)

def visualize_results(phase_map, intersections):
    plt.imshow(phase_map, cmap='jet')
    plt.scatter( intersections[:, 0], color='red', s=5, label='Intersections')
    #plt.scatter(intersections[:, 1], intersections[:, 0], color='red', s=5, label='Intersections')
    plt.colorbar()
    plt.legend()
    plt.title("Phase Cut Lines Intersections")
    plt.show()


def extract_features_from_patterns(patterns, threshold=np.pi/2):
    phase_map = calculate_phase(patterns)

    cv2.namedWindow('vpattern', 0)
    cv2.imshow('vpattern', phase_map)
    cv2.waitKey(0) 
          
    horizontal_lines, vertical_lines = extract_cut_lines(phase_map, threshold)
    intersections = find_intersections(horizontal_lines, vertical_lines)
    print('intersections = ', intersections)
    visualize_results(phase_map, intersections)
    return intersections

def draw_calibration_pattern(image_size, circle_radius, center_distance, rows, cols, img_name):
    # 创建一个全白的图像
    image = np.ones((image_size[1], image_size[0]), dtype=np.uint8) * 255
    circle_distance = center_distance - 2 * circle_radius

    # 计算圆心坐标
    start_x = (image_size[0] - (circle_radius * 2 + circle_distance) * (cols - 1)) // 2
    start_y = (image_size[1] - (circle_radius * 2 + circle_distance) * (rows - 1)) // 2

    # 绘制每个圆
    for i in range(rows):
        for j in range(cols):
            center_x = start_x + j * (circle_radius * 2 + circle_distance)
            center_y = start_y + i * (circle_radius * 2 + circle_distance)
            # 用黑色填充圆形
            cv2.circle(image, (center_x, center_y), circle_radius, (0, 0, 0), -1)

    # 显示图像
    cv2.namedWindow('Calibration Pattern', 0)
    cv2.imshow('Calibration Pattern', image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

    # 可选：保存为文件
    cv2.imwrite(img_name, image)


def paste_img():
    src_path = 'D:\\code\\pmd-python\\tools\\patterns3_honor_800_400\\'
    dst_path = 'D:\\code\\pmd-python\\tools\\patterns3_honor_800_400_paste\\'
    os.makedirs(dst_path, exist_ok = True)
    large_w, large_h = 1920, 1200
    for img in os.listdir(src_path):
        small_img = cv2.imread(src_path + img)  
        small_h, small_w, _ = small_img.shape  

        # 创建一张大图片（例如白色背景，3通道）
        large_img = np.ones((large_h, large_w, 3), dtype=np.uint8) * 255  # 白色背景

        # 计算小图片放置在大图片中心的起始位置
        start_row = (large_h - small_h) // 2
        start_col = (large_w - small_w) // 2
        print('start_row , start_col = ',start_row, start_col)

        # 将小图片贴到大图片中心
        large_img[start_row:start_row + small_h, start_col:start_col + small_w] = small_img

        # 保存结果
        #cv2.imwrite(dst_path + img, large_img)

def pkl_vis():
    cam_para_path = 'D:\\code\\pmd-python\\config\\cam_params_4.pkl'
    with open(cam_para_path, 'rb') as pkl_file:
        cam_params = pickle.load(pkl_file)
        print('cam_params = ', cam_params)



def set_cam_params():
    cam_params_path = 'D:\\code\\pmd-python\\config\\cam_params_4.pkl'

    new_cam0_path = 'D:\\code\\pmd-python\\config\\camera_params_1227cam0.pkl'
    new_cam1_path = 'D:\\code\\pmd-python\\config\\camera_params_1227cam1.pkl'
    new_cam2_path = 'D:\\code\\pmd-python\\config\\camera_params_1227cam2.pkl'
    new_cam3_path = 'D:\\code\\pmd-python\\config\\camera_params_1227cam3.pkl'

    with open(new_cam0_path, 'rb') as pkl_file:
        cam_params0 = pickle.load(pkl_file)
    with open(new_cam1_path, 'rb') as pkl_file:
        cam_params1 = pickle.load(pkl_file)
    with open(new_cam2_path, 'rb') as pkl_file:
        cam_params2 = pickle.load(pkl_file)
    with open(new_cam3_path, 'rb') as pkl_file:
        cam_params3 = pickle.load(pkl_file)
    
    with open(cam_params_path, 'rb') as pkl_file:
        cam_params = pickle.load(pkl_file)
        
        cam_params[0]['camera_matrix'] = cam_params0[0]['camera_matrix']
        cam_params[1]['camera_matrix'] = cam_params1[0]['camera_matrix']
        cam_params[2]['camera_matrix'] = cam_params2[0]['camera_matrix']
        cam_params[3]['camera_matrix'] = cam_params3[0]['camera_matrix']
    
    with open('D:\\code\\pmd-python\\config\\cam_params_4_refined.pkl', 'wb') as pkl_file:
            
        pickle.dump(cam_params, pkl_file)


def refine_cam_params():
    img1_path = 'D:\\data\\one_cam\\1226image\\calibration\\cam\\Image_20241226135705692.bmp'
    img2_path = 'D:\\data\\one_cam\\betone1230\\20250103151342402-pingjing\\0_frame_24.bmp'
    matlab_calib_json_path = 'D:\\code\\code of PMD\\code of PMD\\cameraParams_1_cam0103.json'

        


if __name__ == '__main__':

    #draw_calibration_pattern(image_size=(1920, 1080), circle_radius=40, center_distance=100, rows=6, cols=8, img_name='calibration_pattern-1226-1920-1080-40-100.png')
    #paste_img()
    #camera2world_vis()
    #camera2world_aprilgrid_vis()
    #pkl_vis()
   # set_cam_params()
    refine_cam_params()