# -- coding: utf-8 --
from ctypes import *
import cv2 # 4.9.0
print(cv2.__version__) # 4.9.0
import numpy as np
print(np.__version__) # 1.24.4
import open3d as o3d
print(o3d.__version__) # 0.18.0
import math
import os
import time
# from skimage import feature
import aurco_3d_detect as aurco_3d_detect  
from os import abort 
from utils import Log
from config import *

# 点云整体移动 kk
# 边缘点散射后需要重新投影到平面
# 顶点算法再优化

# 流程
# 3D点云整理修正，kk参数(该参数与物体反射率相关，通过MAP映射，后期不断更新MAP内容)
# 3D点云空间过滤
# 3D点云提取标定板 seg_mark_cloud
    # 生成 im_2d_to_3d  （采用相机内参）
    # 生成 im_project   （采用相机内参）
    # 通过 im_project 识别标定板2D区域 obj_roi
    # 将 im_2d_to_3d 中的3D点云数据按照 obj_roi 进行归集，形成单独的标定板的3D数据文件
# 优化标定板3D数据 correct_mark_cloud
    # 3D 数据平滑、游离点\噪点去除
    # 使用 RANSAC 平面拟合，计算平面模型 [A,B,C,D]、计算内点集合 inlier_cloud 、计算外点集合 outlier_cloud
    # 最小二乘计算 inlier_cloud 拟合的平面
    # 通过平面模型 [A,B,C,D] 将内外点重投至该平面，计算 inlier_pcd_correct
    # 将 inlier_pcd_correct 和 inlier_cloud 合并至 inlier_pcd_correct
# 3D标定板角点提取 select_conners_from_mark_cloud_2d
    # 通过 select_conners_from_mark_cloud_3d 对 inlier_pcd_correct 自动选取 3D 角点 （3D角点自动提取算法）
    # 将 inlier_pcd_correct 投影至 2D
        # 生成 im_2d_to_3d_n (n:1-4)（采用相机内参）
        # 生成 im_project_n (n:1-4) （采用相机内参）
        # 在2D图中人工标记角点
        # 通过2D点、H、[A,B,C,D] 得到3D角点 c_lidar[16]
        # 【check】通过3D角点 c_lidar[16] 计算得出的标定板边长 l 和实际值的误差（绝对误差、4条边的均方差）
# 2D标定板角点提取
    # 定位 ARUCO 标记，计算外参
    # 计算标定板4个角点的3D值 c_camera[16]
    # 【check】通过3D角点 c_camera[16] 计算得出的标定板边长 l 和实际值的误差（绝对误差、4条边的均方差）
# SVD
    # 使用SVD c_lidar[16] 和 c_camera[16] 计算相机和雷达的旋转矩阵 RT
    # 循环采用不同数据通过SVD进行上述计算，采用最大似然优化 RT
    # 【check】c_lidar[16] 通过 RT 恢复到的位置和 c_camera[16] 比较（绝对误差、4条边的均方差）
    # 【check】3D点云通过重投影到2D图像的角点和标定板图像角点的误差估计（人工）
# 推理验证
    # 2D识别标定板
    # 2D标定板角点提取
    # 通过2D点、H、[A,B,C,D] 得到3D角点，c_lidar_camera[16]
    # 【check】c_lidar_camera[16]、c_lidar[16] 的误差（绝对误差、4条边的均方差）
    # 【check】c_lidar_camera[16]、c_camera[16] 、c_lidar[16]计算得出的边长和实际值的误差（绝对误差、4条边的均方差）
# 自动化参数优化
    # 实现自动推理验证流程
    # 生成参数和结果对照记录
    # 自动优化参数
# 预埋件实测

def _eulerAnglesToRotationMatrix(theta) :
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
    R = np.dot(R_z, np.dot( R_y, R_x ))
    return R

def cal_project_matrix():
    # src_size是原始图像的大小
    src_size = (w, h)
    
    # 计算新的摄像机矩阵
    new_camera_matrix = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, src_size, alpha=0)
    Log.info("2D 投影矩阵计算结果为：")
    print(new_camera_matrix)
    # print(new_camera_matrix)
    return new_camera_matrix


# 定义获取点云深度最大最小值
def get_pcd_depth_range(pts_3d):
    # 初始化最小值和最大值为第一个点的x坐标
    min_x = 0
    max_x = 0

    # 遍历所有点，更新最小值和最大值
    for point_3d in pts_3d.points:
        x = point_3d[0]
        if x < min_x:
            min_x = x
        elif x > max_x:
            max_x = x

    return max_x, min_x

def undistort(distorted_img):
    # 矫正畸变
    undistorted_img = cv2.undistort(distorted_img, camera_matrix, dist_coeffs, None, camera_matrix)
    # cv2.imwrite("distorted_img.png", undistorted_img)
    # undistorted_img = cv2.imread("distorted_img.png")
    # cv2.imshow("1", undistorted_img)
    # cv2.waitKey(0)
    return undistorted_img

def clear_folder(folder_path):
    for file_name in os.listdir(folder_path):
        file_path = os.path.join(folder_path, file_name)
        if os.path.isfile(file_path):
            os.remove(file_path)
        else:
            clear_folder(file_path)
            os.rmdir(file_path)


def sort_marks_pcd_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
    min_val = 999999
    for i in range(4):
        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 detect_marks_roi(im_project, show=False):
    masks = []

    offset = 40
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (30, 30))
    im_tmp = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
    im_tmp = cv2.GaussianBlur(im_tmp, (29, 29), 0)

    # im_tmp = cv2.resize(im_tmp, (900, 700))
    # cv2.imshow(".",im_tmp)
    # cv2.waitKey(0)

    # 查找联通区域
    gray = cv2.cvtColor(im_tmp, cv2.COLOR_BGR2GRAY)
    _, binary = cv2.threshold(gray, 128, 255, cv2.THRESH_BINARY)
    # num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(binary, connectivity=8, ltype=cv2.CV_32S)


    contours, _ = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    colors = [ (255, 0, 0) , (255, 255, 0) , (0, 0, 255) , (255, 0, 255) ]
    color_idx = 0

    Log.info("3D->2D 投影连通区域的像素点数量：")
    for contour in contours:
        rect_conner_points = []
        size = cv2.contourArea(contour)
        top_idx = 0
        right_idx = 0
        bottom_idx = 0
        left_idx = 0
        x, y, w, h = cv2.boundingRect(contour)
        for idx in range(len(contour)):
            if y == contour[idx][0][1]: top_idx = idx
            if x == contour[idx][0][0]: left_idx = idx
            if y + h - 1 == contour[idx][0][1]: bottom_idx = idx
            if x + w - 1 == contour[idx][0][0]: right_idx = idx
        contour[left_idx][0][0] -= offset
        contour[top_idx][0][1] -= offset
        contour[right_idx][0][0] += offset
        contour[bottom_idx][0][1] += offset
        if contour[left_idx][0][0] < 0 or contour[top_idx][0][1] < 0 or \
            contour[right_idx][0][0] > global_w or contour[bottom_idx][0][1] > global_h: 
            continue
        if size > rmin:
            print(size)
        if size > rmin and size < rmax:
            contour[left_idx][0][0] -= offset
            contour[top_idx][0][1] -= offset
            contour[right_idx][0][0] += offset
            contour[bottom_idx][0][1] += offset
            cv2.circle(im_project, (contour[top_idx][0][0], contour[top_idx][0][1]), 25,  colors[color_idx] , -1)
            cv2.circle(im_project, (contour[right_idx][0][0], contour[right_idx][0][1]), 25,  colors[color_idx], -1)
            cv2.circle(im_project, (contour[bottom_idx][0][0], contour[bottom_idx][0][1]), 25,  colors[color_idx], -1)
            cv2.circle(im_project, (contour[left_idx][0][0], contour[left_idx][0][1]), 25,  colors[color_idx], -1)
            rect_conner_points.append(contour[left_idx][0])
            rect_conner_points.append(contour[top_idx][0])
            rect_conner_points.append(contour[right_idx][0])
            rect_conner_points.append(contour[bottom_idx][0])
            masks.append(rect_conner_points)
            color_idx += 1
    

    if len(masks) != MARK_COUNT:
        Log.error("3D 重投影未找到4个mark")
        show = True
        if show:
            im_project = cv2.resize(im_project, (900, 700))
            cv2.imshow("im", im_project)
            cv2.waitKey(0)
        return []
    
    # 按照 左上1，右上2，左下3，右下4的顺序重新排序
    masks = sort_marks_pcd_3d(masks)
    
    # for test
    if show:
        im_project = cv2.resize(im_project, (900, 700))
        cv2.imshow("im", im_project)
        cv2.waitKey(0)
    return masks


def project_cloud(pcd_cloud):
    im_project = np.zeros((global_h, global_w, 3), dtype=np.uint8)
    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.array(pcd_cloud.points), rvec, tvec, camera_matrix, dist_coeffs)
    color = (255,255,255)
    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]
    points_np = np.asarray(pcd_cloud.points)
    for i, pt_2d in enumerate(list_pts_2d):
        # x, y = pt_2d.ravel()
        # if x == float('nan') or y == float('nan'): continue
        # x, y = int(x), int(y)
        # x, y = pt_2d[0][0], pt_2d[0][1]
        if 0 <= pt_2d[0][0] < global_w and 0 <= pt_2d[0][1] < global_h:
            im_project[pt_2d[0][1], pt_2d[0][0]] = color
            # im_2d_to_3d[y, x][0] = pcd_cloud.points[i][0]
            # im_2d_to_3d[y, x][1] = pcd_cloud.points[i][1]
            # im_2d_to_3d[y, x][2] = pcd_cloud.points[i][2]
            im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]] = points_np[i]
    return im_project, im_2d_to_3d, pts_2d, list_pts_2d

def seg_mark_cloud(ply_file_path, kk, show=False):
    marks_pcd = []

    src_cloud_load = o3d.io.read_point_cloud(ply_file_path)
    if src_cloud_load is None or len(src_cloud_load.points) == 0 :
        abort()

    start_time = time.time()

    # 点云旋转
    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))
    # t_matrix = np.identity(4)
    # t_matrix[:3, 3] = T_vector
    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)
    
    # 点云筛选
    project_inlier_cloud = o3d.geometry.PointCloud()
    for point_3d in np.asarray(src_cloud_load.points):
        if -2.0 < point_3d[0] < 2.0 and -2.0 < point_3d[1] < 2.0 and point_3d[2] < 3.5: # 筛选x,y,z坐标 
            _r = math.sqrt(pow(point_3d[0], 2) + pow(point_3d[1], 2) + pow(point_3d[2], 2))
            if _r == 0: continue
            if point_3d[0] > 0: point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
            else:               point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
            if point_3d[1] > 0: point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
            else:               point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
            point_3d[2] = point_3d[2] + point_3d[2] / _r * kk
            # point_3d[2] -= 0.0
            project_inlier_cloud.points.append((point_3d[0], point_3d[1], point_3d[2]))

    # 投影3D点云
    im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(project_inlier_cloud)

    # 自动识别ROI角点
    mark_rois = detect_marks_roi(im_project, show)
    if len(mark_rois) != MARK_COUNT:
        return marks_pcd

    # 保存4个点云的数据到文件
    # clear_folder("./detect_cloud")
    clear_folder(conf_temp_cloud_path)
    # 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]
    for i in range(len(mark_rois)):
        if len(mark_rois[i]) < 4: continue
        one_mark_pcd = o3d.geometry.PointCloud()
        mark_rois_tmp = np.asarray(mark_rois[i])
        for _, pt_2d in enumerate(list_pts_2d):
            # x, y = pt_2d.ravel()
            # x, y = int(x), int(y)
            # x, y = pt_2d[0][0], pt_2d[0][1]
            if cv2.pointPolygonTest(mark_rois_tmp, (pt_2d[0][0],pt_2d[0][1]), False) >= 0:
                one_mark_pcd.points.append((im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][0], 
                                            im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][1], 
                                            im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][2]))
        # o3d.io.write_point_cloud("./detect_cloud/" + str(i) + ".ply", tmp_cloud)
        o3d.io.write_point_cloud(conf_temp_cloud_path + str(i) + ".ply", one_mark_pcd)
        if show:
            o3d.visualization.draw_geometries([one_mark_pcd], mesh_show_wireframe=True)
        marks_pcd.append(one_mark_pcd)
    
    if show:
        im_project = cv2.resize(im_project, (int(934), int(700)), interpolation=cv2.INTER_LINEAR)
        cv2.imshow("im_project", im_project)
        cv2.waitKey(0)

    end_time = time.time()
    execution_time = end_time - start_time
    Log.info("3D 分割点云数据耗时：")
    print(execution_time * 1000)
    return marks_pcd

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

    # 定义 x, y, z 的范围
    x_min, x_max = -1.8, 1.8
    y_min, y_max = -1.5, 1.5
    z_min, z_max = 0.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 seg_mark_cloud_simple(ply_file_path, kk, show=False):
    marks_pcd = []

    src_cloud_load = o3d.io.read_point_cloud(ply_file_path)
    if src_cloud_load is None or len(src_cloud_load.points) == 0 :
        abort()

    start_time = time.time()

    # 点云旋转
    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))
    # t_matrix = np.identity(4)
    # t_matrix[:3, 3] = T_vector
    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)
    
    # 点云筛选
    project_inlier_cloud = o3d.geometry.PointCloud()
    if kk != 0.0:
        for point_3d in np.asarray(src_cloud_load.points):
            if -2.0 < point_3d[0] < 2.0 and -2.0 < point_3d[1] < 2.0 and point_3d[2] < global_z_max: # 筛选x,y,z坐标 
                _r = math.sqrt(pow(point_3d[0], 2) + pow(point_3d[1], 2) + pow(point_3d[2], 2))
                if _r == 0: continue
                if point_3d[0] > 0: point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
                else:               point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
                if point_3d[1] > 0: point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
                else:               point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
                point_3d[2] = point_3d[2] + point_3d[2] / _r * kk
                # point_3d[2] -= 0.0
                project_inlier_cloud.points.append((point_3d[0], point_3d[1], point_3d[2]))
    else:
        project_inlier_cloud = filter_cloud(src_cloud_load)

    # 投影3D点云
    im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(project_inlier_cloud)

    # 自动识别ROI角点
    mark_rois = detect_marks_roi(im_project, show)
    if len(mark_rois) != MARK_COUNT:
        return marks_pcd

    # 保存4个点云的数据到文件
    # clear_folder("./detect_cloud")
    # clear_folder(conf_temp_cloud_path)
    # 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]
    Log.info("3D->2D project points")
    for i in range(len(mark_rois)):
        if len(mark_rois[i]) < 4: continue
        one_mark_pcd = o3d.geometry.PointCloud()
        mark_rois_tmp = np.asarray(mark_rois[i])
        select_itmes = 0
        for _, pt_2d in enumerate(list_pts_2d):
            # x, y = pt_2d.ravel()
            # x, y = int(x), int(y)
            # x, y = pt_2d[0][0], pt_2d[0][1]
            if cv2.pointPolygonTest(mark_rois_tmp, (pt_2d[0][0],pt_2d[0][1]), False) >= 0:
                # one_mark_pcd.points.append((im_2d_to_3d[y, x][0], im_2d_to_3d[y, x][1], im_2d_to_3d[y, x][2]))
                one_mark_pcd.points.append((im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][0], 
                                            im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][1], 
                                            im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][2]))
                select_itmes += 1
        print(select_itmes)
        # o3d.io.write_point_cloud("./detect_cloud/" + str(i) + ".ply", tmp_cloud)

        # 不写入文件，节省时间
        # o3d.io.write_point_cloud(conf_temp_cloud_path + str(i) + ".ply", one_mark_pcd)
        # if show:
        #     o3d.visualization.draw_geometries([one_mark_pcd], mesh_show_wireframe=True)

        marks_pcd.append(one_mark_pcd)
    
    if show:
        im_project = cv2.resize(im_project, (int(934), int(700)), interpolation=cv2.INTER_LINEAR)
        cv2.imshow("im_project", im_project)
        cv2.waitKey(0)

    end_time = time.time()
    execution_time = end_time - start_time
    Log.info("3D 分割点云数据耗时：")
    print(execution_time * 1000)
    return marks_pcd

# distance_threshold (float):
# 平面内点的最大距离阈值。这是指一个点到估计平面的距离，如果距离小于或等于这个阈值，则认为该点属于此平面。
# ransac_n (int):
# 每次迭代中用于估计模型的点数。对于平面分割，通常设置为 3，因为三个非共线点可以唯一确定一个平面。
# num_iterations (int):
# RANSAC 算法的迭代次数。更多的迭代次数会增加找到最优解的概率，但也增加了计算时间。
def correct_mark_cloud(mark_pcd):
    plane_model, inliers = mark_pcd.segment_plane(distance_threshold=0.0155,
                                            ransac_n=3,
                                            num_iterations=10000)
    inlier_pcd = mark_pcd.select_by_index(inliers)
    inlier_pcd = reproject_cloud(plane_model, inlier_pcd)  #利用投影得到的图像
    inlier_pcd = inlier_pcd.paint_uniform_color([1.0, 0, 0])
    outlier_cloud = mark_pcd.select_by_index(inliers, invert=True)

    # 暂时没有用到
    # radius = 0.05
    # inlier_pcd = smooth_point_cloud(inlier_pcd, radius) #利用拟合得到的图像

    inlier_pcd_correct = reproject_cloud(plane_model, mark_pcd)  #利用投影得到的图像
    inlier_pcd_correct = inlier_pcd_correct.paint_uniform_color([0.0, 1.0, 0])
    # o3d.visualization.draw_geometries([ inlier_pcd, inlier_pcd_correct ])
    # inlier_pcd_correct = inlier_pcd # ???
    return plane_model, inlier_pcd, inlier_pcd_correct


# 3D标定板角点提取 select_conners_from_mark_cloud_2d
    # 通过 select_conners_from_mark_cloud_3d 对 inlier_pcd_correct 自动选取 3D 角点 （3D角点自动提取算法）
    # 将 inlier_pcd_correct 投影至 2D
        # 生成 im_2d_to_3d_n (n:1-4)（采用相机内参）
        # 生成 im_project_n (n:1-4) （采用相机内参）
        # 在2D图中人工标记角点
        # 通过2D点、H、[A,B,C,D] 得到3D角点 c_lidar[16]
        # 【check】通过3D角点 c_lidar[16] 计算得出的标定板边长 l 和实际值的误差（绝对误差、4条边的均方差）
def cal_H(im_2d_to_3d, pts_2d, list_pts_2d):
    pts_3d = []
    # 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]
    for i, pt_2d in enumerate(list_pts_2d):
        # x, y = pt_2d.ravel()
        # x, y = int(x), int(y)
        # x, y = pt_2d[0][0], pt_2d[0][1]
        pts_3d.append([im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][0], 
                       im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][1], 
                       im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][2]])
    ransacReprojThreshold = 3.0	
    # 查找单应矩阵
    # 修改第三个参数 1.0 -> 5.0
    # 利用基于RANSAC的鲁棒算法选择最优的四组配对点，再计算转换矩阵H(3*3)并返回,以便于反向投影错误率达到最小
    # ransacReprojThreshold 设置为 1.0
    # H, mask = cv2.findHomography(np.array(src_points), np.array(dst_points), cv2.RANSAC, ransacReprojThreshold, maxIters=1500, confidence=0.998) ## 修改 1.0 -> 5.0
    pts_hom = []
    for i in range(len(pts_2d)):
        pt = pts_2d[i]
        x,y = pt[0][0], pt[0][1]
        pts_hom.append([x*1.0, y*1.0, 1.0])
    H, _ = cv2.findHomography(np.array(pts_3d), np.array(pts_hom), cv2.RANSAC, ransacReprojThreshold) ## 修改 1.0 -> 5.0
    Log.info("2D-3D 单应性矩阵计算结果为：")
    print(H)
    return H

def sort_conners_2d(conners):
    sort_conner = []
    left, top, right, bottom = 0, 0, 0, 0
    # maxx,maxy = 0,0
    maxx,maxy = -100000,-100000
    minx,miny = 100000,100000
    for i in range(len(conners)):
        conner = conners[i]
        if conner[0] < minx: 
            minx = conner[0]
            left = i
        if conner[0] > maxx: 
            maxx = conner[0]
            right = i
        if conner[1] < miny: 
            miny = conner[1]
            top = i
        if conner[1] > maxy: 
            maxy = conner[1]
            bottom = i
    sort_conner.append(conners[left])
    sort_conner.append(conners[top])
    sort_conner.append(conners[right])
    sort_conner.append(conners[bottom])
    return sort_conner

def remove_convex_hull_points(contour):
    hull = cv2.convexHull(contour, returnPoints=True)
    hull_indices = set(hull.flatten())
    non_hull_points = [point for i, point in enumerate(contour) if i not in hull_indices]
    return np.array(non_hull_points)

def smooth_convex_hull(contour, epsilon=0.1):
    # 计算凸包
    hull = cv2.convexHull(contour)
    # 使用 approxPolyDP 减少顶点数量，从而达到平滑效果
    epsilon_value = epsilon * cv2.arcLength(hull, True)
    smoothed_hull = cv2.approxPolyDP(hull, epsilon_value, True)
    
    return np.array(smoothed_hull)

from scipy.interpolate import splprep, splev
def spline_fit(points, s=0, k=2):
    # 确保有足够的数据点
    if len(points) <= k:
        return None
    
    tck, u = splprep(points.T, s=s, k=k)
    u_new = np.linspace(u.min(), u.max(), 1000)
    x_new, y_new = splev(u_new, tck)
    return np.vstack((x_new, y_new)).T

def smooth_convex_hull_with_spline(contour, s=0):
    hull = cv2.convexHull(contour)
    points = hull.reshape(-1, 2)
    smoothed_points = spline_fit(points, s=s)
    if smoothed_points is None:
        return None
    return smoothed_points.astype(np.int32)

def find_largest_contour(contours):
    if not contours:
        return None
    
    # 计算所有轮廓的面积
    areas = [cv2.contourArea(cnt) for cnt in contours]
    
    # 找到面积最大的轮廓索引
    max_area_index = np.argmax(areas)
    
    # 返回面积最大的轮廓
    return contours[max_area_index]

from shapely.geometry import Polygon,MultiPolygon  # 需要安装 shapely 库来处理多边形相交
def rect_to_polygon(rect):
    box = cv2.boxPoints(rect)
    return Polygon(box)

def polygon_intersection(poly1, poly2):
    inter_poly = poly1.intersection(poly2)
    if not inter_poly.is_empty:
        return inter_poly
    else:
        return None

def fill_intersection_white(image, rect1, rect2):
    """
    在给定图像中，用白色填充两个旋转矩形的相交区域。
    
    参数:
        image (numpy.ndarray): 输入图像
        rect1 (tuple): 第一个旋转矩形，格式为 (center, size, angle)
        rect2 (tuple): 第二个旋转矩形，格式为 (center, size, angle)
    """
    # 将旋转矩形转换为多边形
    poly1 = rect_to_polygon(rect1)
    poly2 = rect_to_polygon(rect2)

    # 计算相交区域
    intersection = polygon_intersection(poly1, poly2)
    
    if intersection is not None:
        # 创建掩膜并填充白色
        mask = np.zeros_like(image)
        
        # 如果相交区域是多边形
        if isinstance(intersection, Polygon):
            inter_points = np.array(intersection.exterior.coords, dtype=np.int32)
            cv2.fillPoly(mask, [inter_points], (255, 255, 255))
        elif isinstance(intersection, MultiPolygon):
            for geom in intersection.geoms:
                inter_points = np.array(geom.exterior.coords, dtype=np.int32)
                cv2.fillPoly(mask, [inter_points], (255, 255, 255))

        # 应用掩膜到原图
        image[mask == 255] = 255


def select_conners_from_mark_cloud_2d_old(mark_pcd, plane_model):
    # 将 inlier_pcd_correct 投影至 2D
    im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(mark_pcd)
    im_project_gray = cv2.cvtColor(im_project, cv2.COLOR_BGR2GRAY)
    non_zero = cv2.findNonZero(im_project_gray)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (20, 20))
    im_project = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
    
    for point in non_zero:
        im_project[point[0][1], point[0][0]] = (255,255,255)
    rect = cv2.minAreaRect(non_zero)
    box = cv2.boxPoints(rect)
    box = np.int0(box)
    # for test
    # cv2.drawContours(im_project, [box], 0, (255), 10)
    # im_project = cv2.resize(im_project, (1600, 1200))
    # cv2.imshow("_im", im_project)
    # cv2.waitKey(0)

    # 计算 mark cloud 点云的单应矩阵
    H = cal_H(im_2d_to_3d, pts_2d, list_pts_2d)

    # 获取高精确的角点 2D 位置
    conners = sort_conners_2d(list(box))

    # 通过单应矩阵计算角点 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]))
    conners_pcd = 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 conners_pcd

def select_conners_from_mark_cloud_2d(mark_pcd, plane_model):
    # 将 inlier_pcd_correct 投影至 2D
    im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(mark_pcd)
    im_project_gray = cv2.cvtColor(im_project, cv2.COLOR_BGR2GRAY)
    non_zero = cv2.findNonZero(im_project_gray)

    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (20, 20))
    im_project = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)

    for point in non_zero:
        im_project[point[0][1], point[0][0]] = (255,255,255)
    rect = cv2.minAreaRect(non_zero)
    box = cv2.boxPoints(rect)
    box = np.int0(box)

    #================================================
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (40, 40))
    im_project_gray = cv2.morphologyEx(im_project_gray, cv2.MORPH_DILATE, kernel)
    kernel = cv2.getStructuringElement(cv2.MORPH_CROSS, (40, 40))
    im_project_gray = cv2.morphologyEx(im_project_gray, cv2.MORPH_CLOSE, kernel)

    # im_project_gray = cv2.resize(im_project_gray, (1600, 1200))
    # cv2.imshow("im_project_gray", im_project_gray)
    # cv2.waitKey(0)
    _, binary_img = cv2.threshold(im_project_gray, 0, 255, cv2.THRESH_BINARY)
    contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # 对每个轮廓进行处理
    # processed_contours = [remove_convex_hull_points(cnt) for cnt in contours]
    processed_contours = [smooth_convex_hull(cnt) for cnt in contours]
    contour = find_largest_contour(processed_contours)
    rect2 = cv2.minAreaRect(contour)
    box2 = cv2.boxPoints(rect2)
    box2 = np.int0(box2)
    # box = box2

    temp_img = np.zeros((im_project_gray.shape[0], im_project_gray.shape[1], 1), dtype=np.uint8)
    fill_intersection_white(temp_img, rect, rect2)
    non_zero = cv2.findNonZero(temp_img)
    rect3 = cv2.minAreaRect(non_zero)
    box3 = cv2.boxPoints(rect3)
    box3 = np.int0(box3)

    box = box2

    # processed_contours = []
    # for cnt in contours:
    #     smoothed_hull = smooth_convex_hull_with_spline(cnt, s=0.0)
    #     if smoothed_hull is None: continue
    #     # cv2.polylines(drawing, [smoothed_hull], isClosed=True, color=(0, 255, 0), thickness=2)
    #     processed_contours.append(smoothed_hull)
    # drawing = np.zeros_like(im_project)
    # cv2.drawContours(drawing, processed_contours, -1, color=255, thickness=1)
    # cv2.imshow("Processed Contours", drawing)
    # cv2.waitKey(0)
    #
    # for test
    # cv2.drawContours(im_project, [box2], 0, (255), 15)
    # # cv2.drawContours(im_project, processed_contours, -1, color=(0,0,255), thickness=10)
    # cv2.drawContours(im_project, [box3], 0, (0,255,0), 20)
    # for point in non_zero:
    #     im_project[point[0][1], point[0][0]] = (255,255,255)
    # im_project = cv2.resize(im_project, (1400, 1000))
    # cv2.imshow("_im", im_project)
    # cv2.waitKey(0)
    #================================================

    # 计算 mark cloud 点云的单应矩阵
    H = cal_H(im_2d_to_3d, pts_2d, list_pts_2d)

    # 获取高精确的角点 2D 位置
    conners = sort_conners_2d(list(box))

    Log.error("3d-2d 的边长")
    print(np.linalg.norm(conners[0] - conners[1]))
    print(np.linalg.norm(conners[1] - conners[2]))
    print(np.linalg.norm(conners[2] - conners[3]))
    print(np.linalg.norm(conners[3] - conners[0]))

    # 通过单应矩阵计算角点 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]))
    conners_pcd = 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 conners_pcd

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 correct_mask_conner(inlier_pcd_correct, pcd_conners_2d):
    oriented_bounding_box = inlier_pcd_correct.get_axis_aligned_bounding_box()
    center_point = oriented_bounding_box.get_center()
    # 0.3536
    real_len = 0.3535534
    pcd_correct_conners = o3d.geometry.PointCloud()
    for i in range(4):
        error_len = np.linalg.norm(pcd_conners_2d.points[i] - center_point) - real_len
        real_conner = extend_line_3d(center_point, pcd_conners_2d.points[i], -error_len)
        pcd_correct_conners.points.append(real_conner)
        # print("中心点距离:" + str(np.linalg.norm(pcd_conners_2d.points[i] - center_point) ))
        # print(real_conner)
    return pcd_correct_conners, center_point

def fit_plane_least_squares(points):
    # 计算中心点
    centroid = np.mean(points, axis=0)
 
    # 计算协方差矩阵
    cov_matrix = (points - centroid).T.dot(points - centroid)
 
    # 计算协方差矩阵的特征值和特征向量
    eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix)
 
    # 最小特征值对应的特征向量就是平面的法向量
    normal = eigenvectors[:, np.argmin(eigenvalues)]
    return centroid, normal


def smooth_point_cloud(pcd, radius):
    # Create a new PointCloud object
    smoothed_pcd = o3d.geometry.PointCloud()
    smoothed_points = []
 
    # Build KDTree for the input point cloud
    kdtree = o3d.geometry.KDTreeFlann(pcd)
 
    # For each point, perform least squares plane fitting
    for point in np.asarray(pcd.points):
        # Find points within the radius
        [k, idx, _] = kdtree.search_radius_vector_3d(point, radius)
        if k < 3:
            # If there are not enough points, skip
            continue
 
        # Extract neighboring points
        neighbors = np.asarray(pcd.points)[idx, :]
 
        # Use least squares to fit a plane
        centroid, normal = fit_plane_least_squares(neighbors)
 
        # Project the point onto the fitted plane
        point_to_centroid = point - centroid
        distance = point_to_centroid.dot(normal)
        smoothed_point = point - distance * normal
        smoothed_points.append(smoothed_point)
 
    # Set the smoothed points
    smoothed_pcd.points = o3d.utility.Vector3dVector(smoothed_points)
    return smoothed_pcd


def reproject_cloud(plane_model, cloud_load):
    reproject_pcd = o3d.geometry.PointCloud()
    A,B,C,D = plane_model
    for pts_3d in cloud_load.points:
        # 求线与平面的交点
        t =  -D / (A * pts_3d[0] + B * pts_3d[1] + C * pts_3d[2])
        # pts_3d = np.array([t * pts_3d[0], t * pts_3d[1], t * pts_3d[2]])
        reproject_pcd.points.append((t * pts_3d[0], t * pts_3d[1], t * pts_3d[2]))
    return reproject_pcd


def o2t(o_pcd):
    t_pcd = o3d.t.geometry.PointCloud()
    t_pcd.point.positions = o3d.core.Tensor(np.asarray(o_pcd.points))
    t_pcd.point.colors = o3d.core.Tensor(np.asarray(o_pcd.colors))
    o_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
    t_pcd.point.normals = o3d.core.Tensor(np.asarray(o_pcd.normals))
    return t_pcd


def t2o(t_pcd):
    o_pcd = o3d.geometry.PointCloud()
    o_pcd.points = o3d.utility.Vector3dVector(t_pcd.point.positions.numpy())
    # o_pcd.colors = o3d.utility.Vector3dVector(t_pcd.point.colors.numpy())
    return o_pcd


# def calculate_curvature(pcd):
#     # 估计法线
#     pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
    
#     # 计算曲率
#     curvatures = []
#     for i in range(len(pcd.points)):
#         normal = np.array(pcd.normals[i])
#         k_neighbors = pcd.select_by_index(pcd.nearest_k_neighbor(i, 10)[1])  # 获取最近的10个邻居
#         neighbor_normals = np.array(k_neighbors.normals)
#         curvature = np.mean(np.abs(neighbor_normals - normal))  # 简单地取平均绝对差作为曲率估计
#         curvatures.append(curvature)
        
#     return np.array(curvatures)


def calculate_curvature(pcd):
    # 估计法线
    pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
    
    # 构建KDTree
    pcd_tree = o3d.geometry.KDTreeFlann(pcd)
    
    # 计算曲率
    curvatures = []
    for i in range(len(pcd.points)):
        [k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[i], 10)  # 获取最近的10个邻居
        neighbor_normals = np.array([pcd.normals[j] for j in idx])
        normal = np.array(pcd.normals[i])
        curvature = np.mean(np.abs(neighbor_normals - normal))  # 简单地取平均绝对差作为曲率估计
        curvatures.append(curvature)
        
    return np.array(curvatures)


def select_conners_from_mark_cloud_3d(inlier_cloud):
    # 2. 计算凸包以获取边界点
    hull, _ = inlier_cloud.compute_convex_hull()
    hull_points = np.asarray(hull.vertices)

    # 3. 将凸包顶点转换为点云对象以便后续处理
    hull_pcd = o3d.geometry.PointCloud()
    hull_pcd.points = o3d.utility.Vector3dVector(hull_points)

    # 4. 简单地假设凸包是近似方形，计算四边形的四个角点
    # 这里采用一种简化方法，即寻找距离最远的两个点对作为对角线端点
    distances = np.linalg.norm(hull_points[:, None] - hull_points, axis=2)
    max_distance_indices = np.unravel_index(np.argmax(distances), distances.shape)
    diagonal_points = []
    diagonal_points.append(hull_points[max_distance_indices[0]])
    diagonal_points.append(hull_points[max_distance_indices[1]])
    diagonal_points = np.asarray(diagonal_points)
    # 找到剩余两个角点，它们应位于对角线两端点构成的直线上
    remaining_points = np.delete(hull_points, max_distance_indices, axis=0)
    idx = []
    for i in range(len(remaining_points)):
        pt = remaining_points[i]   
        if np.linalg.norm(pt - diagonal_points[0]) < 0.3 or np.linalg.norm(pt - diagonal_points[1]) < 0.3 :
            idx.append(i)
    remaining_points = np.delete(remaining_points, idx, axis=0)
    distances = np.linalg.norm(remaining_points[:, None] - remaining_points, axis=2)
    max_distance_indices = np.unravel_index(np.argmax(distances), distances.shape)
    diagonal_points2 = []
    diagonal_points2.append(remaining_points[max_distance_indices[0]])
    diagonal_points2.append(remaining_points[max_distance_indices[1]])
    diagonal_points2 = np.asarray(diagonal_points2)

    corner_points = [diagonal_points[0], diagonal_points[1],
                    diagonal_points2[0], diagonal_points2[1]]
    
    Log.info("3D标记角点，得到得边长为：")
    print(np.linalg.norm(diagonal_points[0] - diagonal_points2[0]))
    print(np.linalg.norm(diagonal_points[0] - diagonal_points2[1]))
    print(np.linalg.norm(diagonal_points[1] - diagonal_points2[0]))
    print(np.linalg.norm(diagonal_points[1] - diagonal_points2[1]))

    # 5. 创建一个新的点云来表示角点，并可视化
    corner_pcd = o3d.geometry.PointCloud()
    corner_pcd.points = o3d.utility.Vector3dVector(corner_points)

    corner_pcd.paint_uniform_color([1, 0, 0])  
    return corner_pcd

from sklearn.cluster import DBSCAN
import copy
import scipy
def select_conners_from_mark_cloud_3d_v2(inlier_cloud):
    # 修正
    downsampled_inlier_cloud = copy.deepcopy(inlier_cloud)
    for i in range(len(downsampled_inlier_cloud.points)):
        if i % 2 == 0:
            downsampled_inlier_cloud.points[i][0] += 0.0001
    # 估计法线
    downsampled_inlier_cloud.estimate_normals()
    # 寻找边界点
    cl, ind = downsampled_inlier_cloud.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
    boundary_points = downsampled_inlier_cloud.select_by_index(ind, invert=True)
    # 将边界点转换为numpy数组
    points = np.asarray(boundary_points.points)
     # 假设我们正在寻找的是矩形，我们可以尝试用凸包算法来找到最小包围四边形
    hull = scipy.spatial.ConvexHull(points)
    # 凸包顶点索引
    hull_vertices = hull.vertices
    # 如果凸包有4个顶点，那么这些可能是我们要找的角点
    if len(hull_vertices) == 4:
        corner_points = points[hull_vertices]
    else:
        # 如果不是4个顶点，则需要进一步处理
        # 这里简化处理，直接返回凸包顶点作为角点
        corner_points = points[hull_vertices]


    # for i in range(len(downsampled_inlier_cloud.points)):
    #     if i % 2 == 0:
    #         downsampled_inlier_cloud.points[i][0] += 0.0001
    # obb = downsampled_inlier_cloud.get_oriented_bounding_box()

    # 提取角点
    # corner_points = np.asarray(obb.get_box_points())

    # 创建新的点云对象表示角点
    corner_pcd = o3d.geometry.PointCloud()
    corner_pcd.points = o3d.utility.Vector3dVector(corner_points)

    # 可视化结果
    o3d.visualization.draw_geometries([inlier_cloud, corner_pcd])

    # corner_points = [diagonal_points[0], diagonal_points[1],
    #                 diagonal_points2[0], diagonal_points2[1]]
    
    # Log.info("3D标记角点，得到得边长为：")
    # print(np.linalg.norm(diagonal_points[0] - diagonal_points2[0]))
    # print(np.linalg.norm(diagonal_points[0] - diagonal_points2[1]))
    # print(np.linalg.norm(diagonal_points[1] - diagonal_points2[0]))
    # print(np.linalg.norm(diagonal_points[1] - diagonal_points2[1]))

    # 5. 创建一个新的点云来表示角点，并可视化
    # corner_pcd = o3d.geometry.PointCloud()
    # corner_pcd.points = o3d.utility.Vector3dVector(corner_points)

    # corner_pcd.paint_uniform_color([1, 0, 0])  
    return corner_pcd


def create_spheres(points, sizes=None, color=[0, 0.706, 1]):
    """
    创建一系列球体以替代点云中的点，允许每个点有不同的大小。
    
    参数:
    points: N x 3 数组，表示N个点的位置。
    sizes: N维数组或列表，表示对应点的半径大小。如果为None，则所有球体大小相同。
    color: RGB颜色值，默认是橙色。
    
    返回:
    spheres: 包含所有球体的Open3D几何对象列表。
    """
    spheres = []
    if sizes is None:
        sizes = [1] * len(points)  # 如果没有提供大小，则默认所有球体大小相同
    for i, point in enumerate(points):
        sphere = o3d.geometry.TriangleMesh.create_sphere(radius=sizes[i])
        sphere.translate(point)  # 将球体移动到点的位置
        sphere.paint_uniform_color(color)
        spheres.append(sphere)
    return spheres


def cal_marks_3d_conners(show=False):
    marks_3d_conners = []
    for i in range(4):
        # mark_ply_file_path = './detect_cloud/' + str(i) + '.ply'
        mark_ply_file_path = conf_temp_cloud_path + str(i) + '.ply'

        # 计算标定板尺寸
        # mark_ply_file_path = './detect_cloud/1.ply'
        mark_pcd = o3d.io.read_point_cloud(mark_ply_file_path)
        plane_model, inlier_cloud, inlier_pcd_correct = correct_mark_cloud(mark_pcd)
        
        # t_pcd = o2t(inlier_cloud)
        # _dir = dir(t_pcd)
        # for it in _dir:
        #     print(it)
        # t_boundaries, mask = t_pcd.compute_boundary_points(0.05, 60)
        # boundaries = t2o(t_boundaries)

        # # 计算曲率
        # curvatures = calculate_curvature(inlier_cloud)
        # threshold = np.percentile(curvatures, 95)  # 使用95百分位数作为阈值
        # corner_indices = np.where(curvatures > threshold)[0]
        # t_corner_pcd = inlier_cloud.select_by_index(corner_indices.tolist())
        # t_corner_pcd.paint_uniform_color([0, 0, 1])  # 将角点涂成红色
        
        used_pcd = inlier_cloud # inlier_cloud / inlier_pcd_correct
        pcd_conners_2d = select_conners_from_mark_cloud_2d(used_pcd, plane_model)
        pcd_conners_2d_correct, center_point = correct_mask_conner(used_pcd, pcd_conners_2d)
        Log.info("修正后2D标记角点，得到得边长为：")
        for i in range(4):
            print(np.linalg.norm(pcd_conners_2d_correct.points[i] - pcd_conners_2d_correct.points[(i+1)%4]))
        # pcd_conners_3d = select_conners_from_mark_cloud_3d(inlier_pcd_correct)
        marks_3d_conners.append(pcd_conners_2d)

        if show:
            # 角点显示的圆球半径，单位（米）
            r = 0.006
            sizes = [r,r,r,r]
            spheres_2d = create_spheres(pcd_conners_2d.points, sizes, color=[0, 0.706, 1])
            spheres_3d = create_spheres(pcd_conners_2d_correct.points, sizes, color=[0, 0, 0])
            spheres_center_3d = create_spheres([center_point], [0.01], color=[0, 1, 1])
            o3d.visualization.draw_geometries([
                spheres_2d[0],spheres_2d[1],spheres_2d[2],spheres_2d[3],
                spheres_3d[0],spheres_3d[1],spheres_3d[2],spheres_3d[3],
                spheres_center_3d[0],
                mark_pcd.paint_uniform_color([1, 0, 0]),
                inlier_pcd_correct.paint_uniform_color([0, 1, 0]),])
    
    # mark 顺序按照 左上，右上，左下，右下
    # 每个mark的角点按照左、上、右、底顺序返回
    return marks_3d_conners

def cal_marks_3d_conners_simple(marks_pcd_4, show=False):
    marks_3d_conners = []
    for i in range(4):
        # mark_ply_file_path = './detect_cloud/' + str(i) + '.ply'
        mark_ply_file_path = conf_temp_cloud_path + str(i) + '.ply'

        # 计算标定板尺寸
        # mark_ply_file_path = './detect_cloud/1.ply'
        # 不从文件读取
        # mark_pcd = o3d.io.read_point_cloud(mark_ply_file_path)
        mark_pcd = marks_pcd_4[i]
        plane_model, inlier_cloud, inlier_pcd_correct = correct_mark_cloud(mark_pcd)
        
        # t_pcd = o2t(inlier_cloud)
        # _dir = dir(t_pcd)
        # for it in _dir:
        #     print(it)
        # t_boundaries, mask = t_pcd.compute_boundary_points(0.05, 60)
        # boundaries = t2o(t_boundaries)

        # # 计算曲率
        # curvatures = calculate_curvature(inlier_cloud)
        # threshold = np.percentile(curvatures, 95)  # 使用95百分位数作为阈值
        # corner_indices = np.where(curvatures > threshold)[0]
        # t_corner_pcd = inlier_cloud.select_by_index(corner_indices.tolist())
        # t_corner_pcd.paint_uniform_color([0, 0, 1])  # 将角点涂成红色
        
        used_pcd = inlier_cloud # inlier_cloud / inlier_pcd_correct
        # inlier_cloud -> box 
        # inlier_pcd_correct -> box2\box3
        pcd_conners_2d = select_conners_from_mark_cloud_2d(used_pcd, plane_model)
        pcd_conners_2d_correct, center_point = correct_mask_conner(used_pcd, pcd_conners_2d)
        Log.info("修正后2D标记角点，得到得边长为：")
        for i in range(4):
            print(np.linalg.norm(pcd_conners_2d_correct.points[i] - pcd_conners_2d_correct.points[(i+1)%4]))
        # pcd_conners_3d = select_conners_from_mark_cloud_3d(inlier_pcd_correct)
        marks_3d_conners.append(pcd_conners_2d_correct) # pcd_conners_2d / pcd_conners_2d_correct

        
        # print(marks_3d_conners[0].points)
        # print(marks_3d_conners[0].points[0])
        # print(np.linalg.norm(marks_3d_conners[0].points[0] - marks_3d_conners[0].points[1]))
        # print(np.linalg.norm(marks_3d_conners[0].points[1] - marks_3d_conners[0].points[2]))
        # print(np.linalg.norm(marks_3d_conners[0].points[2] - marks_3d_conners[0].points[3]))
        # print(np.linalg.norm(marks_3d_conners[0].points[3] - marks_3d_conners[0].points[0]))
        # Log.info("3D->3D 通过相邻角点计算得到的边长进行对比")
        if show:
            # 角点显示的圆球半径，单位（米）
            r = 0.006
            sizes = [r,r,r,r]
            spheres_2d = create_spheres(pcd_conners_2d.points, sizes, color=[0, 0.706, 1])
            spheres_3d = create_spheres(pcd_conners_2d_correct.points, sizes, color=[0, 0, 0])
            spheres_center_3d = create_spheres([center_point], [0.01], color=[0, 1, 1])
            o3d.visualization.draw_geometries([
                spheres_2d[0],spheres_2d[1],spheres_2d[2],spheres_2d[3],
                spheres_3d[0],spheres_3d[1],spheres_3d[2],spheres_3d[3],
                spheres_center_3d[0],
                mark_pcd.paint_uniform_color([1, 0, 0]),
                inlier_pcd_correct.paint_uniform_color([0, 1, 0]),])
    
    # mark 顺序按照 左上，右上，左下，右下
    # 每个mark的角点按照左、上、右、底顺序返回
    return marks_3d_conners

def print_diff(aurco_pt1, aurco_pt2, lidar_pt1, lidar_pt2):
    aurco_distance = np.linalg.norm(aurco_pt1 - aurco_pt2)
    lidar_distance = np.linalg.norm(lidar_pt1 - lidar_pt2)
    print(aurco_distance)
    print(lidar_distance)
    print(abs(aurco_distance - lidar_distance))


# 测试2d角点分割算法
def _test_select_conners_from_mark_cloud_2d_2():
    pass


if __name__ == '__main__':
    marks_3d_conners = []
    ply_file_path = os.path.dirname(__file__) + "\\samples\\calibration\\11\\output.ply" 
    marks_pcd = seg_mark_cloud_simple(ply_file_path, kk)
    for i in range(4):
        mark_ply_file_path = conf_temp_cloud_path + str(i) + '.ply'
        mark_pcd = o3d.io.read_point_cloud(mark_ply_file_path)
        plane_model, inlier_cloud, inlier_pcd_correct = correct_mark_cloud(mark_pcd)
        # pcd_conners_2d = select_conners_from_mark_cloud_2d(mark_pcd, plane_model)
        pcd_conners_2d = select_conners_from_mark_cloud_3d_v2(inlier_pcd_correct)
    
    
    # image_project = cv2.imread("../caowei/new/output.png")
    # # if image_project is None: 
    # #     print("image_project is None!") 
    # #     exit()
    
    # # # 分割标定板
    # ply_file_path = '../caowei/new/20241227175529496.ply' 

    # marks_pcd = seg_mark_cloud(ply_file_path, kk)
    # if len(marks_pcd) != MARK_COUNT:
    #     exit()

    # LEFT = 0
    # TOP = 1
    # RIGHT = 2
    # BOTTOM = 3

    # frame, marks_aurco = aurco_3d_detect.cal_marks_3d_conners("../caowei/new/output.png")

    # marks_pcd = cal_marks_3d_conners()
    # marks_pcd_new = []
    # # marks_pcd_new.append(marks_pcd[2].points)
    # # marks_pcd_new.append(marks_pcd[3].points)
    # # marks_pcd_new.append(marks_pcd[1].points)
    # # marks_pcd_new.append(marks_pcd[0].points)

    # tmmp = []
    # for i in range(4): 
    #     tmmp.append(marks_pcd[2].points[i])
    # marks_pcd_new.append(tmmp)
    # tmmp = []
    # for i in range(4): 
    #     tmmp.append(marks_pcd[3].points[i])
    # marks_pcd_new.append(tmmp)
    # tmmp = []
    # for i in range(4): 
    #     tmmp.append(marks_pcd[1].points[i])
    # marks_pcd_new.append(tmmp)
    # tmmp = []
    # for i in range(4): 
    #     tmmp.append(marks_pcd[0].points[i])
    # marks_pcd_new.append(tmmp)

    # print("======== 0 left - 1 right =========")
    # print_diff(marks_aurco[0][LEFT] , marks_aurco[1][RIGHT], marks_pcd_new[0][LEFT], marks_pcd_new[1][RIGHT])
    # print("======== 0 left - 2 right =========")
    # print_diff(marks_aurco[0][LEFT] , marks_aurco[2][RIGHT], marks_pcd_new[0][LEFT], marks_pcd_new[2][RIGHT])
    # print("======== 0 left - 3 right =========")
    # print_diff(marks_aurco[0][LEFT] , marks_aurco[3][RIGHT], marks_pcd_new[0][LEFT], marks_pcd_new[3][RIGHT])
    # print("======== 0 right - 1 left =========")
    # print_diff(marks_aurco[0][RIGHT] , marks_aurco[1][LEFT], marks_pcd_new[0][RIGHT] , marks_pcd_new[1][LEFT])
    # print("======== 0 top - 3 bottom =========")
    # print_diff(marks_aurco[0][TOP] , marks_aurco[3][BOTTOM], marks_pcd_new[0][TOP] , marks_pcd_new[3][BOTTOM])
    # print("======== 2 left - 1 right =========")
    # print_diff(marks_aurco[2][LEFT] , marks_aurco[1][RIGHT], marks_pcd_new[2][LEFT] , marks_pcd_new[1][RIGHT])
    # print("======== 2 left - 3 right =========")
    # print_diff(marks_aurco[2][LEFT] , marks_aurco[3][RIGHT], marks_pcd_new[2][LEFT] , marks_pcd_new[3][RIGHT])

    # exit()