import cv2
print(cv2.__version__) # 4.9.0
import open3d as o3d
print(o3d.__version__) # 0.18.0
import numpy as np
from os import abort 
import sys
sys.path.append(r"./")
from utils import Log
from config import *
import utils
import pandas as pd


def extend_line_3d(pt1, pt2, distance):
    """
    计算 3D 空间两个点的延长线上的一个点
    参数:
    - point1: 第一个点的坐标 (x1, y1, z1)，类型为列表或数组
    - point2: 第二个点的坐标 (x2, y2, z2)，类型为列表或数组
    - distance: 从第二个点延长的距离 (正值表示延长方向，负值表示反向)
    返回:
    - 延长线上的点坐标 (x, y, z)，类型为数组
    """
    # 转换为 NumPy 数组
    point1 = np.array(pt1)
    point2 = np.array(pt2)
    # 计算方向向量
    direction = point2 - point1
    # 计算方向向量的单位向量
    direction_unit = direction / np.linalg.norm(direction)
    # 延长线上的点
    extended_point = point2 + distance * direction_unit
    return extended_point

def filter_cloud(pcd):
    # 获取点云中的点坐标
    points = np.asarray(pcd.points)

    # 定义 x, y, z 的范围
    x_min, x_max = -2.0, 2.0
    y_min, y_max = -2.0, 2.0
    z_min, z_max = 0, global_z_max

    # 创建布尔掩膜
    mask = (
        (points[:, 0] >= x_min) & (points[:, 0] <= x_max) &
        (points[:, 1] >= y_min) & (points[:, 1] <= y_max) &
        (points[:, 2] >= z_min) & (points[:, 2] <= z_max)
    )

    # 应用掩膜以过滤点云
    filtered_points = points[mask]

    # 更新点云对象
    pcd.points = o3d.utility.Vector3dVector(filtered_points)
    return pcd

def sort_marks_aruco_3d(masks):
    new_mask = []
    left_pt_0, left_pt_1, left_pt_2, left_pt_3 = 0,0,0,0
    delete_index = []
    coordinate_temp = [0,1,2,3]
    # max_val = 0
    max_val = -999999.0
    min_val = 999999.0
    for i in range(len(masks)):
        left_pt = masks[i][0]
        val = left_pt[0] + left_pt[1]
        if val > max_val:
            left_pt_3 = i
            max_val = val
        if val < min_val:
            left_pt_0 = i
            min_val = val
    delete_index.append(left_pt_0)
    delete_index.append(left_pt_3)
    coordinate_temp = np.delete(coordinate_temp, delete_index, axis=0)
    if masks[coordinate_temp[0]][0][0] > masks[coordinate_temp[1]][0][0]:
        left_pt_1 = coordinate_temp[0]
        left_pt_2 = coordinate_temp[1]
    else:
        left_pt_2 = coordinate_temp[0]
        left_pt_1 = coordinate_temp[1]
    new_mask.append(masks[left_pt_0])
    new_mask.append(masks[left_pt_1])
    new_mask.append(masks[left_pt_2])
    new_mask.append(masks[left_pt_3])
    return new_mask

def sort_marks_aruco_3d_v2(masks):
    new_mask = []
    left_pt_0, left_pt_1, left_pt_2, left_pt_3 = 0,0,0,0
    delete_index = []
    coordinate_temp = [0,1,2,3]
    max_val = 0
    min_val = 999999
    for i in range(len(masks)):
        left_pt = masks[i][0][0]
        val = left_pt[0] + left_pt[1]
        if val > max_val:
            left_pt_3 = i
            max_val = val
        if val < min_val:
            left_pt_0 = i
            min_val = val
    delete_index.append(left_pt_0)
    delete_index.append(left_pt_3)
    coordinate_temp = np.delete(coordinate_temp, delete_index, axis=0)
    if masks[coordinate_temp[0]][0][0][0] > masks[coordinate_temp[1]][0][0][0]:
        left_pt_1 = coordinate_temp[0]
        left_pt_2 = coordinate_temp[1]
    else:
        left_pt_2 = coordinate_temp[0]
        left_pt_1 = coordinate_temp[1]
    new_mask.append(masks[left_pt_0])
    new_mask.append(masks[left_pt_1])
    new_mask.append(masks[left_pt_2])
    new_mask.append(masks[left_pt_3])
    return new_mask

def cal_camera_marks_3d_conners(file_path):
    marks_3d_conners = []
    frame = cv2.imread(file_path)
    if frame is None:
        abort()

    # 定义使用的字典类型
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_ARUCO_ORIGINAL)
    parameters = cv2.aruco.DetectorParameters()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 检测ArUco标记
    # corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
    aruco_detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
    corners, ids, rejectedImgPoints = aruco_detector.detectMarkers(gray)

    rotation_matrices = []
    if ids is not None:
        # 对角点进行亚像素级别的细化以提高精度
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 50, 0.0003)
        for corner in corners:
            cv2.cornerSubPix(gray, corner, winSize=(3, 3), zeroZone=(-1, -1), criteria=criteria)

        # 定义标记的世界坐标（假设标记位于z=0平面上）
        obj_points = np.array([[-marker_size/2, -marker_size/2, 0], # 左上
                                [ marker_size/2, -marker_size/2, 0], # 右上
                                [ marker_size/2,  marker_size/2, 0], # 右下
                                [-marker_size/2,  marker_size/2, 0]], dtype=np.float32) # 左下
    
        # obj_points = np.array([[-marker_size/2, marker_size/2, 0], # 左下角
        #                         [ marker_size/2, marker_size/2, 0], # 右下角
        #                         [ marker_size/2,  -marker_size/2, 0], # 右上角
        #                         [-marker_size/2,  -marker_size/2, 0]], dtype=np.float32) # 左上角

        # 遍历所有检测到的标记
        Log.info("2D->3D 通过相邻角点计算得到的边长进行对比")
        for i, corner in enumerate(corners):
            Log.error("2d-2d 的边长")
            print(np.linalg.norm(corner[0][0] - corner[0][1]) / 0.7)
            print(np.linalg.norm(corner[0][1] - corner[0][2]) / 0.7)
            print(np.linalg.norm(corner[0][2] - corner[0][3]) / 0.7)
            print(np.linalg.norm(corner[0][3] - corner[0][0]) / 0.7)

            conners_pcd = o3d.geometry.PointCloud()
            # for j in range(4):
            #     conners_pcd.points.append(obj_points[j])

            # 估计每个标记的姿态
            # _, rvec, tvec = cv2.solvePnP(obj_points, corner, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_DLS)

            _, rvec, tvec = cv2.solvePnP(obj_points, corner, camera_matrix, dist_coeffs)

            # 绘制每个标记的轴线
            cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvec, tvec, 0.1)

            # 将旋转向量转换为旋转矩阵（如果需要）
            rotation_matrix, _ = cv2.Rodrigues(rvec)
            rotation_matrices.append(rotation_matrix)

            # 使用cv2.invert()计算矩阵的逆
            retval, rotation_matrix_inv = cv2.invert(rotation_matrix)
            # if retval != 0: print("矩阵的逆为：\n", rotation_matrix_inv)
            # else: print("无法计算矩阵的逆")

            H = np.eye(4)
            H[:3, :3] = rotation_matrix
            H[:3, 3] = tvec.flatten()
            for j in range(4):
                P_world = np.array([obj_points[j][0], obj_points[j][1], obj_points[j][2], 1])
                P_camera_homogeneous = H @ P_world  # 齐次坐标相乘
                conners_pcd.points.append(P_camera_homogeneous[:-1])

            # conners_pcd.rotate(rotation_matrix, (0,0,0))
            # conners_pcd.translate(tvec)
            
            # 在图像上绘制角点编号（可选）
            for j, point in enumerate(corner[0]):
                text = str(conners_pcd.points[j])
                if j == 3: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 255, 255), 10, cv2.LINE_AA)
                elif j == 2: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 255, 0), 10, cv2.LINE_AA)
                elif j == 1: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (255, 255, 255), 10, cv2.LINE_AA)
                else: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 0, 255), 10, cv2.LINE_AA)


            # 将点云的点转换为 NumPy 数组
            points = np.asarray(conners_pcd.points)
            reversed_points = points[::-1]
            conners_pcd.points = o3d.utility.Vector3dVector(reversed_points)

            # 计算角点和边长
            e1p1 = extend_line_3d(conners_pcd.points[2], conners_pcd.points[1], b2)
            e1p2 = extend_line_3d(conners_pcd.points[3], conners_pcd.points[0], b2)
            left2 = extend_line_3d(e1p2, e1p1 , b1)
            top = extend_line_3d(e1p1, e1p2, board_size - b1 - marker_size)

            e2p1 = extend_line_3d(conners_pcd.points[1], conners_pcd.points[0], board_size - b1 - marker_size)
            e2p2 = extend_line_3d(conners_pcd.points[2], conners_pcd.points[3], board_size - b1 - marker_size)
            top2 = extend_line_3d(e2p2, e2p1, b2)
            right = extend_line_3d(e2p1, e2p2, board_size - b2 - marker_size)

            e3p1 = extend_line_3d(conners_pcd.points[0], conners_pcd.points[3], board_size  - b2 - marker_size)
            e3p2 = extend_line_3d(conners_pcd.points[1], conners_pcd.points[2], board_size  - b2 - marker_size)
            right2 = extend_line_3d(e3p2, e3p1, board_size - b1 - marker_size)
            bottom = extend_line_3d(e3p1, e3p2, b1)

            e4p1 = extend_line_3d(conners_pcd.points[3], conners_pcd.points[2], b2)
            e4p2 = extend_line_3d(conners_pcd.points[0], conners_pcd.points[1], b2)
            bottom2 = extend_line_3d(e4p2, e4p1, board_size - b2 - marker_size)
            left = extend_line_3d(e4p1, e4p2, b2)

            print(
                np.linalg.norm(left - left2),
                np.linalg.norm(top - top2),
                np.linalg.norm(right - right2),
                np.linalg.norm(bottom - bottom2)
            )
            print(
                np.linalg.norm(top - left),
                np.linalg.norm(right - top),
                np.linalg.norm(bottom - right),
                np.linalg.norm(left - bottom),
            )
            marks_3d_conners.append([left, top, right, bottom])
    
    marks_3d_conners = sort_marks_aruco_3d(marks_3d_conners)
    # 按照左、上、右、底顺序返回
    return frame, marks_3d_conners

def cal_camera_marks_3d_conners_v2(file_path):
    marks_3d_conners = []
    frame = cv2.imread(file_path)
    if frame is None:
        abort()

    # 定义使用的字典类型
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_ARUCO_ORIGINAL)
    parameters = cv2.aruco.DetectorParameters()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 检测ArUco标记
    # corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
    aruco_detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
    corners, ids, rejectedImgPoints = aruco_detector.detectMarkers(gray)

    rotation_matrices = []
    if ids is not None:
        # 对角点进行亚像素级别的细化以提高精度
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 50, 0.0003)
        for corner in corners:
            cv2.cornerSubPix(gray, corner, winSize=(3, 3), zeroZone=(-1, -1), criteria=criteria)

        # 定义标记的世界坐标（假设标记位于z=0平面上）
        obj_points = np.array([[-marker_size/2, -marker_size/2, 0], # 左上
                                [ marker_size/2, -marker_size/2, 0], # 右上
                                [ marker_size/2,  marker_size/2, 0], # 右下
                                [-marker_size/2,  marker_size/2, 0]], dtype=np.float32) # 左下
    
        # obj_points = np.array([[-marker_size/2, marker_size/2, 0], # 左下角
        #                         [ marker_size/2, marker_size/2, 0], # 右下角
        #                         [ marker_size/2,  -marker_size/2, 0], # 右上角
        #                         [-marker_size/2,  -marker_size/2, 0]], dtype=np.float32) # 左上角

        # 遍历所有检测到的标记
        Log.error("2D->2D 通过相邻角点计算得到的边长进行对比")
        # for i, cornner in enumerate(corners):
        print(np.linalg.norm(corners[0][0][0] - corners[0][0][1]))
        print(np.linalg.norm(corners[0][0][1] - corners[0][0][2]))
        print(np.linalg.norm(corners[0][0][2] - corners[0][0][3]))
        print(np.linalg.norm(corners[0][0][3] - corners[0][0][0]))
        for i, corner in enumerate(corners):
            conners_pcd = o3d.geometry.PointCloud()
            # for j in range(4):
            #     conners_pcd.points.append(obj_points[j])

            # 估计每个标记的姿态
            # _, rvec, tvec = cv2.solvePnP(obj_points, corner, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_DLS)

            _, rvec, tvec = cv2.solvePnP(obj_points, corner, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_EPNP)

            # 绘制每个标记的轴线
            cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvec, tvec, 0.1)

            # 将旋转向量转换为旋转矩阵（如果需要）
            rotation_matrix, _ = cv2.Rodrigues(rvec)
            rotation_matrices.append(rotation_matrix)

            # 使用cv2.invert()计算矩阵的逆
            retval, rotation_matrix_inv = cv2.invert(rotation_matrix)
            # if retval != 0: print("矩阵的逆为：\n", rotation_matrix_inv)
            # else: print("无法计算矩阵的逆")

            H = np.eye(4)
            H[:3, :3] = rotation_matrix
            H[:3, 3] = tvec.flatten()
            for j in range(4):
                P_world = np.array([obj_points[j][0], obj_points[j][1], obj_points[j][2], 1])
                P_camera_homogeneous = H @ P_world  # 齐次坐标相乘
                conners_pcd.points.append(P_camera_homogeneous[:-1])

            # conners_pcd.rotate(rotation_matrix, (0,0,0))
            # conners_pcd.translate(tvec)
            
            # 在图像上绘制角点编号（可选）
            for j, point in enumerate(corner[0]):
                text = str(conners_pcd.points[j])
                if j == 3: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 255, 255), 10, cv2.LINE_AA)
                elif j == 2: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 255, 0), 10, cv2.LINE_AA)
                elif j == 1: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (255, 255, 255), 10, cv2.LINE_AA)
                else: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 0, 255), 10, cv2.LINE_AA)

            # 将点云的点转换为 NumPy 数组
            points = np.asarray(conners_pcd.points)
            reversed_points = points[::-1]
            conners_pcd.points = o3d.utility.Vector3dVector(reversed_points)

            left = conners_pcd.points[1]
            top = conners_pcd.points[0]
            right = conners_pcd.points[3]
            bottom = conners_pcd.points[2]
            # left = conners_pcd.points[3]
            # top = conners_pcd.points[0]
            # right = conners_pcd.points[1]
            # bottom = conners_pcd.points[2]

            print(
                np.linalg.norm(top - left),
                np.linalg.norm(right - top),
                np.linalg.norm(bottom - right),
                np.linalg.norm(left - bottom),
            )
            marks_3d_conners.append([left, top, right, bottom])
    
    marks_3d_conners = sort_marks_aruco_3d(marks_3d_conners)
    # 按照左、上、右、底顺序返回
    return frame, marks_3d_conners


def cal_lidar_marks_3d_conners(frame, r_file_path): 
    # file_path = os.path.dirname(__file__) + "\\samples\\1.png"

    frame, marks_3d_conners = [],[]
    file_path = os.path.dirname(__file__) + "\\samples\\output.ply"
    src_cloud_load = o3d.io.read_point_cloud(file_path)

    reflectivitys = utils.parse_custom_attribute(file_path, "reflectivity")
    # file_path = os.path.dirname(__file__) + "\\samples\\output_data.ply"
    # src_cloud_data = o3d.io.read_point_cloud(file_path)

    # 点云旋转
    Rx = src_cloud_load.get_rotation_matrix_from_xyz((roll, 0, 0))
    Ry = src_cloud_load.get_rotation_matrix_from_xyz((0, pitch, 0))
    Rz = src_cloud_load.get_rotation_matrix_from_xyz((0, 0, yaw))
    src_cloud_load.rotate(Rx, (1,0,0))
    src_cloud_load.rotate(Ry, (0,1,0))
    src_cloud_load.rotate(Rz, (0,0,1))
    src_cloud_load.translate(T_vector)
    # o3d.visualization.draw_geometries([src_cloud_load], mesh_show_wireframe=True)

    # 点云过滤
    ## 通过距离和反射率过滤后剩下得点云数据
    filte_src_cloud_load = []
    ## 点云的反射率数据
    filte_sreflectivitys = []
    src_cloud_load_array = np.asarray(src_cloud_load.points)
    index = 0
    for point_3d in src_cloud_load_array:
        if -2.0 < point_3d[0] < 2.0 and -2.0 < point_3d[1] < 2.0 and point_3d[2] < global_z_max: 
            filte_src_cloud_load.append((point_3d[0], point_3d[1], point_3d[2]))
            filte_sreflectivitys.append([reflectivitys[index]])
        index += 1

    # 点云投影
    ## 点云投影后的2D图
    im_project = np.zeros((global_h, global_w, 3), dtype=np.uint8)
    ## 点云2D和3D对应的图
    im_2d_to_3d = np.zeros((global_h, global_w, 3), dtype=np.float64)
    rvec = np.array([np.array([0.]), np.array([0.]), np.array([0.])])
    tvec = np.float64([0.0, 0.0, 0.0])
    pts_2d, _ = cv2.projectPoints(np.asarray(filte_src_cloud_load), rvec, tvec, camera_matrix, dist_coeffs)

    list_pts_2d = pts_2d.tolist()
    list_pts_2d = [[[int(item) for item in subsublist] for subsublist in sublist] for sublist in list_pts_2d]
    list_pts_2d_all_mask = []
    for i, pt_2d in enumerate(list_pts_2d):
        if 0 <= pt_2d[0][0] < global_w and 0 <= pt_2d[0][1] < global_h:
            # 只挑选出反射率小于30的点
            if filte_sreflectivitys[i][0] < 30:
                # 添加2D图像的像素值
                im_project[pt_2d[0][1], pt_2d[0][0]] = (255,255,255)
                # 添加2D-3D映射图像的数值
                im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]] = filte_src_cloud_load[i]
                # 添加2D坐标数值，这里要保持和原始数据一致的信息，所以顺序是 pt_2d[0][0], pt_2d[0][1]
                list_pts_2d_all_mask.append([pt_2d[0][0], pt_2d[0][1]])

    # 计算图像的最小值和最大值
    min_val = np.min(im_project)
    max_val = np.max(im_project)
    print(min_val)
    print(max_val)

    # 进行膨胀操作
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))

    # 计算联通区域
    im_project_delate = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
    im_project_delate = cv2.cvtColor(im_project_delate, cv2.COLOR_RGB2GRAY)
    _, im_project_delate_binary = cv2.threshold(im_project_delate, 128, 255, cv2.THRESH_BINARY)

    # 查找轮廓
    # 计算连通域
    num_labels, labels, stats, centroids = \
        cv2.connectedComponentsWithStats(im_project_delate_binary, connectivity=8)

    # 忽略背景组件（标签0）
    sizes = stats[1:, -1]  # 获取所有连通组件的面积（忽略背景）
    rectangles = stats[1:, :4]  # 获取所有连通组件的边界框（忽略背景）

    # 根据面积大小排序索引，并选择面积最大的前4个
    sorted_indexes_by_size = sorted(range(sizes.shape[0]), key=lambda k: sizes[k], reverse=True)[:4]

    print("面积最大的4个连通组件索引:")
    print(sorted_indexes_by_size)

    # 创建一个新的与labels大小相同的数组，用于保存面积最大的4个连通组件
    filtered_labels = np.zeros_like(labels)

    for i in sorted_indexes_by_size:
        filtered_labels[labels == i + 1] = i + 1  # 注意这里要加1，因为忽略了背景组件

    # 给每个连通区域分配一种颜色
    colors = np.random.randint(0, 255, size=(len(sorted_indexes_by_size), 3), dtype=np.uint8)

    mask4_conners = []
    for i, idx in enumerate(sorted_indexes_by_size):
        colored_filtered_labels = np.zeros((filtered_labels.shape[0], filtered_labels.shape[1], 1), dtype=np.uint8)
        color = colors[i]
        mask = (filtered_labels == idx + 1)
        colored_filtered_labels[mask] = 255
        non_zero = cv2.findNonZero(colored_filtered_labels)
        rect = cv2.minAreaRect(non_zero)
        box = cv2.boxPoints(rect)
        box = np.int0(box)
        conners = utils.sort_conners_2d(list(box))
        # 4个角点拍寻 left top right bottom
        mask4_conners.append(conners)

    # mask 板子排序 左上 右上 左下 右下
    mask4_conners = sort_marks_aruco_3d(mask4_conners)
    ## for test mask 板子排序 左上 右上 左下 右下
    # for i in range(4):
    #     conners = mask4_conners[i]
    #     cv2.drawContours(im_project, [np.asarray(conners)], 0, (255), 10)
    #     im_tmp = cv2.resize(im_project, (1000, 750))
    #     cv2.imshow("1", im_tmp)
    #     cv2.waitKey(0)

    # 初始化每个标记板的2d坐标数组和3d点云数据数组
    pts_2d_mask_list = []
    im_2d_to_3d_mask_list = []
    for i in range(4):
        pts_2d_mask = []
        im_2d_to_3d_mask = np.zeros((global_h, global_w, 3), dtype=np.float64)
        pts_2d_mask_list.append(pts_2d_mask)
        im_2d_to_3d_mask_list.append(im_2d_to_3d_mask)

    # 按没个标记版分别归纳2d坐标数据和3d点云数据
    ## 遍历过滤后的2d点坐标数据
    for pt_index in list_pts_2d_all_mask:
        for i in range(4):
            conners = mask4_conners[i]
            if cv2.pointPolygonTest(np.asarray(conners), (pt_index[0],pt_index[1]), False) >= 0:
                # 这里要保持2D坐标的顺序，所以是pt_index[0],pt_index[1]
                pts_2d_mask_list[i].append([pt_index[0], pt_index[1]])
                # im_2d_to_3d_mask_list[i].append(im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]])
                im_2d_to_3d_mask_list[i][pt_index[1], pt_index[0]] = im_2d_to_3d[pt_index[1], pt_index[0]]
                break
    
    # 遍历4个标记板，计算每隔标记板4个角点的3D坐标
    for m in range(4):
        pts_2d_mask = pts_2d_mask_list[m]
        im_2d_to_3d_mask = im_2d_to_3d_mask_list[m]
        H = utils.cal_H(im_2d_to_3d_mask, pts_2d_mask, pts_2d_mask)

        # 通过单应矩阵计算角点 3D 单位坐标下的数值
        H_inv = np.linalg.inv(H)
        conners_pcd = o3d.geometry.PointCloud()
        for i in range(4):
            conner = conners[i]
            x, y = conner[0], conner[1]
            pts_hom = np.array([x, y, 1])
            pts_3d = np.dot(H_inv, pts_hom.T)
            pts_3d = pts_3d/pts_3d[2]
            conners_pcd.points.append((pts_3d[0],pts_3d[1],pts_3d[2]))
        
        # 生成当前标定板的点云数据
        mark_pcd = o3d.geometry.PointCloud()
        for i, pt_2d in enumerate(pts_2d_mask):
            mark_pcd.points.append([im_2d_to_3d[pt_2d[1], pt_2d[0]][0], 
                                    im_2d_to_3d[pt_2d[1], pt_2d[0]][1], 
                                    im_2d_to_3d[pt_2d[1], pt_2d[0]][2]])
        # 计算标定板点云拟合平面的参数，这里会受到反射率、噪声等的影响
        plane_model, inlier_cloud, inlier_pcd_correct = utils.correct_mark_cloud(mark_pcd)
        # 重新计算3D角点的空间未知
        conners_pcd = utils.reproject_cloud(plane_model, conners_pcd)

    Log.info("2D标记角点，得到得边长为：")
    for i in range(4):
        print(np.linalg.norm(conners_pcd.points[i] - conners_pcd.points[(i+1)%4]))

   
    return frame, marks_3d_conners


def cal_lidar_marks_3d_conner_v2(r_file_path): 
    marks_3d_conners = []
    file_path = r_file_path
    src_cloud_load = o3d.io.read_point_cloud(file_path)

    reflectivitys = utils.parse_custom_attribute(file_path, "reflectivity")

    # 点云旋转
    Rx = src_cloud_load.get_rotation_matrix_from_xyz((roll, 0, 0))
    Ry = src_cloud_load.get_rotation_matrix_from_xyz((0, pitch, 0))
    Rz = src_cloud_load.get_rotation_matrix_from_xyz((0, 0, yaw))
    src_cloud_load.rotate(Rx, (1,0,0))
    src_cloud_load.rotate(Ry, (0,1,0))
    src_cloud_load.rotate(Rz, (0,0,1))
    src_cloud_load.translate(T_vector)
    # o3d.visualization.draw_geometries([src_cloud_load], mesh_show_wireframe=True)

    # 点云过滤
    ## 通过距离和反射率过滤后剩下得点云数据
    filte_src_cloud_load = []
    ## 点云的反射率数据
    filte_sreflectivitys = []
    src_cloud_load_array = np.asarray(src_cloud_load.points)
    index = 0
    for point_3d in src_cloud_load_array:
        if -2.0 < point_3d[0] < 2.0 and -2.0 < point_3d[1] < 2.0 and point_3d[2] < global_z_max: 
            filte_src_cloud_load.append((point_3d[0], point_3d[1], point_3d[2]))
            filte_sreflectivitys.append([reflectivitys[index]])
        index += 1

    # 点云投影
    ## 点云投影后的2D图
    im_project = np.zeros((global_h, global_w, 3), dtype=np.uint8)
    ## 点云2D和3D对应的图
    im_2d_to_3d = np.zeros((global_h, global_w, 3), dtype=np.float64)
    rvec = np.array([np.array([0.]), np.array([0.]), np.array([0.])])
    tvec = np.float64([0.0, 0.0, 0.0])
    pts_2d, _ = cv2.projectPoints(np.asarray(filte_src_cloud_load), rvec, tvec, camera_matrix, dist_coeffs)

    list_pts_2d = pts_2d.tolist()
    list_pts_2d = [[[int(item) for item in subsublist] for subsublist in sublist] for sublist in list_pts_2d]
    list_pts_2d_all_mask = []
    for i, pt_2d in enumerate(list_pts_2d):
        if 0 <= pt_2d[0][0] < global_w and 0 <= pt_2d[0][1] < global_h:
            # 只挑选出反射率小于30的点
            if filte_sreflectivitys[i][0] < 20:
                # 添加2D图像的像素值
                im_project[pt_2d[0][1], pt_2d[0][0]] = (255,255,255)
                # 添加2D-3D映射图像的数值
                im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]] = filte_src_cloud_load[i]
                # 添加2D坐标数值，这里要保持和原始数据一致的信息，所以顺序是 pt_2d[0][0], pt_2d[0][1]
                list_pts_2d_all_mask.append([pt_2d[0][0], pt_2d[0][1]])

    # 计算图像的最小值和最大值
    min_val = np.min(im_project)
    max_val = np.max(im_project)
    print(min_val)
    print(max_val)

    # 进行膨胀操作
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (17, 17))

    # 计算联通区域
    im_project_delate = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
    im_project_delate_color = im_project_delate.copy()
    im_project_delate = cv2.cvtColor(im_project_delate, cv2.COLOR_RGB2GRAY)
    _, im_project_delate_binary = cv2.threshold(im_project_delate, 1, 255, cv2.THRESH_BINARY)

    # 查找轮廓
    # 计算连通域
    num_labels, labels, stats, centroids = \
        cv2.connectedComponentsWithStats(im_project_delate_binary, connectivity=8)

    # 忽略背景组件（标签0）
    sizes = stats[1:, -1]  # 获取所有连通组件的面积（忽略背景）
    rectangles = stats[1:, :4]  # 获取所有连通组件的边界框（忽略背景）

    # 根据面积大小排序索引，并选择面积最大的前4个
    sorted_indexes_by_size = sorted(range(sizes.shape[0]), key=lambda k: sizes[k], reverse=True)[:4]

    print("面积最大的4个连通组件索引:")
    print(sorted_indexes_by_size)

    # 创建一个新的与labels大小相同的数组，用于保存面积最大的4个连通组件
    filtered_labels = np.zeros_like(labels)

    for i in sorted_indexes_by_size:
        filtered_labels[labels == i + 1] = i + 1  # 注意这里要加1，因为忽略了背景组件

    # 给每个连通区域分配一种颜色
    colors = np.random.randint(0, 255, size=(len(sorted_indexes_by_size), 3), dtype=np.uint8)

    mask4_conners = []
    for i, idx in enumerate(sorted_indexes_by_size):
        colored_filtered_labels = np.zeros((filtered_labels.shape[0], filtered_labels.shape[1], 1), dtype=np.uint8)
        color = colors[i]
        mask = (filtered_labels == idx + 1)
        colored_filtered_labels[mask] = 255
        # 使用该区域与原图相与
        im_project_gray = cv2.cvtColor(im_project, cv2.COLOR_BGR2GRAY)
        colored_filtered_labels = cv2.bitwise_and(colored_filtered_labels, im_project_gray)
        non_zero = cv2.findNonZero(colored_filtered_labels)

        # conners = utils.select_and_sort_conners_2d(non_zero)

        rect = cv2.minAreaRect(non_zero)
        box = cv2.boxPoints(rect)
        box = np.int0(box)
        conners = utils.sort_conners_2d(list(box))
        # 4个角点拍寻 left top right bottom
        mask4_conners.append(conners)

    # mask 板子排序 左上 右上 左下 右下
    mask4_conners = sort_marks_aruco_3d(mask4_conners)
    ## for test mask 板子排序 左上 右上 左下 右下
    for i in range(4):
        conners = mask4_conners[i]
        cv2.drawContours(im_project_delate_color, [np.asarray(conners)], 0, (0, 255, 0), 20)
    im_tmp = cv2.resize(im_project_delate_color, (1000, 750))
    cv2.imshow("1", im_tmp)
    cv2.waitKey(0)

    Log.error("3D-2D")
    print(np.linalg.norm(mask4_conners[0][0] - mask4_conners[0][1]))
    print(np.linalg.norm(mask4_conners[0][1] - mask4_conners[0][2]))
    print(np.linalg.norm(mask4_conners[0][2] - mask4_conners[0][3]))
    print(np.linalg.norm(mask4_conners[0][3] - mask4_conners[0][0]))
    
    obj_points = np.array([[-marker_size/2, -marker_size/2, 0], # 左上
                                [ marker_size/2, -marker_size/2, 0], # 右上
                                [ marker_size/2,  marker_size/2, 0], # 右下
                                [-marker_size/2,  marker_size/2, 0]], dtype=np.float32) # 左下

    # obj_points = np.array([[-marker_size/2, marker_size/2, 0], # 左下角
    #                         [ marker_size/2, marker_size/2, 0], # 右下角
    #                         [ marker_size/2,  -marker_size/2, 0], # 右上角
    #                         [-marker_size/2,  -marker_size/2, 0]], dtype=np.float32) # 左上角

    # 遍历所有检测到的标记
    rotation_matrices = []
    Log.info("2D->3D 通过相邻角点计算得到的边长进行对比")
    for i, corner in enumerate(mask4_conners):
        conners_pcd = o3d.geometry.PointCloud()
        # for j in range(4):
        #     conners_pcd.points.append(obj_points[j])
        # image_points = np.array([
        #     [corner[0][0], corner[0][1]],
        #     [corner[1][0], corner[1][1]],
        #     [corner[2][0], corner[2][1]],
        #     [corner[3][0], corner[3][1]]
        # ], dtype=np.float32)

        # 0123 -> 2301
        # 与objects3D坐标对齐
        image_points = np.array([
            [corner[2][0], corner[2][1]],
            [corner[3][0], corner[3][1]],
            [corner[0][0], corner[0][1]],
            [corner[1][0], corner[1][1]],
        ], dtype=np.float32)
        # 估计每个标记的姿态
        # _, rvec, tvec = cv2.solvePnP(obj_points, corner, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_DLS)
        _, rvec, tvec = cv2.solvePnP(obj_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_EPNP)

        frame = im_project
        # 绘制每个标记的轴线
        # cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvec, tvec, 0.1)

        # 将旋转向量转换为旋转矩阵（如果需要）
        rotation_matrix, _ = cv2.Rodrigues(rvec)
        rotation_matrices.append(rotation_matrix)

        # 使用cv2.invert()计算矩阵的逆
        retval, rotation_matrix_inv = cv2.invert(rotation_matrix)
        # if retval != 0: print("矩阵的逆为：\n", rotation_matrix_inv)
        # else: print("无法计算矩阵的逆")

        H = np.eye(4)
        H[:3, :3] = rotation_matrix
        H[:3, 3] = tvec.flatten()
        for j in range(4):
            P_world = np.array([obj_points[j][0], obj_points[j][1], obj_points[j][2], 1])
            P_camera_homogeneous = H @ P_world  # 齐次坐标相乘
            conners_pcd.points.append(P_camera_homogeneous[:-1])

        # conners_pcd.rotate(rotation_matrix, (0,0,0))
        # conners_pcd.translate(tvec)
        
        # 在图像上绘制角点编号（可选）
        for j, point in enumerate(image_points):
            text = str(conners_pcd.points[j])
            if j == 3: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 255, 255), 10, cv2.LINE_AA)
            elif j == 2: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 255, 0), 10, cv2.LINE_AA)
            elif j == 1: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (255, 255, 255), 10, cv2.LINE_AA)
            else: cv2.putText(frame, text, tuple(point.astype(int)), cv2.FONT_HERSHEY_SIMPLEX, 2.5, (0, 0, 255), 10, cv2.LINE_AA)

        # 将点云的点转换为 NumPy 数组
        points = np.asarray(conners_pcd.points)
        reversed_points = points[::-1]
        conners_pcd.points = o3d.utility.Vector3dVector(reversed_points)

        left = conners_pcd.points[3]
        top = conners_pcd.points[0]
        right = conners_pcd.points[1]
        bottom = conners_pcd.points[2]
        conners = [left, top, right, bottom]
        conners = utils.sort_conners_2d(conners)

        print(
            np.linalg.norm(top - left),
            np.linalg.norm(right - top),
            np.linalg.norm(bottom - right),
            np.linalg.norm(left - bottom),
        )
        # marks_3d_conners.append([left, top, right, bottom])
        marks_3d_conners.append(conners)
    
    marks_3d_conners = sort_marks_aruco_3d(marks_3d_conners)
    # 按照左、上、右、底顺序返回
    return frame, marks_3d_conners

import struct
def get_low_8bits(float_num):
    # 把浮点数转换成一个4字节的二进制表示
    bytes_rep = struct.pack('>d', float_num)
    # 解析这个二进制表示为一个整数
    int_rep = int.from_bytes(bytes_rep, 'big')
    # 使用位运算取出低8位
    low_8bits = int_rep & 0xFF
    return low_8bits

def check_ply_file(filename):
    with open(filename, 'r') as file:
        lines = file.readlines()
    
    # 打印文件头部分
    print("File header:")
    for line in lines[:lines.index('end_header\n') + 1]:
        print(line.strip())
    
    # 打印前几个数据行
    print("\nSample data rows:")
    data_start_index = lines.index('end_header\n') + 1
    for i, line in enumerate(lines[data_start_index:data_start_index + 5], start=data_start_index):
        print(f"Line {i}: {line.strip()}")

if __name__ == '__main__':
    ile_path = os.path.dirname(__file__) + "\\samples\\output.ply"
    cal_lidar_marks_3d_conner_v2(ile_path)


    # o3d.visualization.draw_geometries([src_cloud_load], mesh_show_wireframe=True)

    #################################
    # FILE_PATH = "./samples/calibration/1/output.png"
    # frame = cv2.imread(FILE_PATH)
    # if frame is None:
    #     exit()
    # frame, marks = cal_camera_marks_3d_conners_v2(frame)

    # # frame, marks = cal_camera_marks_3d_conners(FILE_PATH)
    # Log.info("2D->3D marks conners value")
    # print(marks)
    # frame = cv2.resize(frame, (1000, 750))
    # cv2.imshow('frame', frame)
    # cv2.waitKey(0) 

    # cv2.destroyAllWindows()