ymj_bim_test/search.py

import cv2

from utils import filter_rectangle, get_hd_cam_rect, get_hd_roi_from_txt

print(cv2.__version__)  # 4.9.0
import json
import math
import copy
import numpy as np
import time
from scipy.spatial import distance
from scipy.optimize import linear_sum_assignment


def get_params(num_param, start, end):
    return copy.deepcopy(num_param[start:end + 1])


def parmas_to_num(text_param):
    for item in text_param:
        # item['center'] = int(float(item['center']) * 1000)
        item['center'] = int(float(item['center']))
        item['x'] = int(float(item['x']))
        item['w'] = int(item['w'])
        item['h'] = int(item['h'])
    return text_param


def parmas_to_text(num_param):
    for item in num_param:
        item['center'] = str(item['center'])
        item['x'] = str(item['x'])
        item['w'] = str(item['w'])
        item['h'] = str(item['h'])
        item['x1'] = str(item['x1'])
        item['y1'] = str(item['y1'])
        item['x2'] = str(item['x2'])
        item['y2'] = str(item['y2'])
        item['x3'] = str(item['x3'])
        item['y3'] = str(item['y3'])
        item['x4'] = str(item['x4'])
        item['y4'] = str(item['y4'])
        item['x_center'] = str(item['x_center'])
        item['y_center'] = str(item['y_center'])
    return num_param


def sort_params(params):
    sorted_params = sorted(params, key=lambda item: (item['center'], item['x']))
    return sorted_params


def print_params(sort_params):
    for param in sort_params:
        print(param["center"], param["x"], param["w"], param["h"])


def print_path(search_path):
    for path in search_path:
        print(path[0], path[1])


def search_path(sort_params):
    searchPath = []
    for i in range(len(sort_params) - 1):
        (r, theta) = cartesian_to_polar(sort_params[i]["x"], sort_params[i]["center"],
                                        sort_params[i + 1]["x"], sort_params[i + 1]["center"])
        searchPath.append([r, theta])
    return searchPath


def normalize_params_and_path(sort_params, search_path, index=0):
    base = sort_params[index]["h"]
    for param in sort_params:
        param['center'] /= base
        param['x'] /= base
        param['w'] /= base
        param['h'] /= base
        param['w/h'] = param['w'] / param['h']
    if search_path != None:
        for path in search_path:
            path[0] /= base
            # path[1] /= base
    return sort_params, search_path


def read_from_json(file_path):
    with open(file_path, 'r') as f:
        loaded_array = json.load(f)
    return loaded_array


def cartesian_to_polar(x1, y1, x2, y2):
    dx = x2 - x1
    dy = y2 - y1
    r = math.sqrt(dx ** 2 + dy ** 2)
    theta = math.atan2(dy, dx)
    return r, theta


def calculate_second_point(x1, y1, r, theta_radians):
    # theta_radians = math.radians(theta_degrees)
    x2 = x1 + r * math.cos(theta_radians)
    y2 = y1 + r * math.sin(theta_radians)

    return x2, y2


def cal_c1c2c3c4(param, heigt):
    '''
    按照上左、上右、下右、下左的顺时针顺序.
    返回的数据是转换成了以左上角为原点的坐标系下的坐标点。
    '''
    param['x1'] = int(param['x'] - param['w'] / 2)
    param['y1'] = heigt - int(param['center'] + param['h'] / 2)

    param['x2'] = int(param['x'] + param['w'] / 2)
    param['y2'] = heigt - int(param['center'] + param['h'] / 2)

    param['x3'] = int(param['x'] + param['w'] / 2)
    param['y3'] = heigt - int(param['center'] - param['h'] / 2)

    param['x4'] = int(param['x'] - param['w'] / 2)
    param['y4'] = heigt - int(param['center'] - param['h'] / 2)

    param['x_center'] = int((param['x1'] + param['x2']) / 2)
    param['y_center'] = int((param['y1'] + param['y3']) / 2)
    return param


def gen_im_from_params(params, type="lines"):
    # 依据bim数据生成bim图
    # type: # line points
    ## 确定整个bim图的长度和宽度边界
    max_y = -999999  # y坐标最大值
    max_y_idx = -1  # y坐标最大值的索引
    max_x = -999999  # x坐标最大值
    max_x_idx = -1  # x坐标最大值的索引

    # 遍历,找到矩形x坐标最大值和矩形y坐标最大值
    for i, param in enumerate(params):
        # x中心点坐标加上宽度的一半 = 当前矩形的x最大坐标
        if param["x"] + param["w"] / 2 > max_x:
            # 如果是最大的x坐标就更新
            max_x = param["x"] + param["w"] / 2
            max_x_idx = i
        # y中心点坐标加上高度的一半 = 当前矩形的y最大坐标
        if param["center"] + param["h"] / 2 > max_y:
            # 如果是最大的y坐标就更新
            max_y = param["center"] + param["h"] / 2
            max_y_idx = i

    bim_width = int(max_x)
    bim_height = int(max_y)
    bim_channels = 3
    im = np.zeros((bim_height, bim_width, bim_channels), dtype=np.uint8)

    for i, param in enumerate(params):
        cal_c1c2c3c4(param, bim_height)
        if type == "lines":
            pts = np.asarray([[param['x1'], param['y1']],
                              [param['x2'], param['y2']],
                              [param['x3'], param['y3']],
                              [param['x4'], param['y4']]])
            cv2.polylines(im, [pts], True, (255, 255, 0), 8)
        elif type == "points":
            cv2.circle(im, (param['x1'], param['y1']), 1, 255, 20)
            cv2.circle(im, (param['x2'], param['y2']), 1, 255, 20)
            cv2.circle(im, (param['x3'], param['y3']), 1, 255, 20)
            cv2.circle(im, (param['x4'], param['y4']), 1, 255, 20)
            cv2.circle(im, (param['x'], int((param['y3'] + param['y2']) / 2)), 1, (255, 0, 0), 8)

    return im


def get_bim_height(bim_json):
    """
    确定整个bim图的高度
    """
    max_y = -999999  # y坐标最大值

    # 遍历,找到矩形y坐标最大值
    for i, param in enumerate(bim_json):
        # y中心点坐标加上高度的一半 = 当前矩形的y最大坐标
        if param["center"] + param["h"] / 2 > max_y:
            # 如果是最大的y坐标就更新
            max_y = param["center"] + param["h"] / 2
    bim_height = int(max_y)

    return bim_height


"""
判断点是否在区域内部
    True 在
    False 不在
如果没有提供区域,不做判断,返回True
"""


def is_inside_roi(point, roi_w, roi_h):
    if (roi_w != None and roi_h != None):
        x = point[0]
        y = point[1]
        if (x <= 0 or y <= 0 or y >= roi_h or x >= roi_w):
            # 不在区域内部
            return False
        else:
            # 在区域内部
            return True
    else:
        # 没有提供区域,不做判断,默认返回True
        return True


def gen_points_from_params(params, roi_w=None, roi_h=None):
    # 依据bim数据生成bim图
    # type line points
    ## 计算bim图长和宽
    max_y = -999999
    max_y_idx = -1
    max_x = -999999
    max_x_idx = -1
    for i, param in enumerate(params):
        if param["x"] + param["w"] / 2 > max_x:
            max_x = param["x"] + param["w"] / 2
            max_x_idx = i
        if param["center"] + param["h"] / 2 > max_y:
            max_y = param["center"] + param["h"] / 2
            max_y_idx = i
    bim_height = int(max_y)

    points = []
    for i, param in enumerate(params):
        # 排序点 按照上左、上右、下右、下左的顺时针顺序
        if (roi_w == None and roi_h == None):
            cal_c1c2c3c4(param, bim_height)
        # 过滤点,把在roi区域之外的点全部过滤掉.
        if (is_inside_roi([param['x1'], param['y1']], roi_w, roi_h)):
            points.append([param['x1'], param['y1'], i])
        if (is_inside_roi([param['x2'], param['y2']], roi_w, roi_h)):
            points.append([param['x2'], param['y2'], i])
        if (is_inside_roi([param['x3'], param['y3']], roi_w, roi_h)):
            points.append([param['x3'], param['y3'], i])
        if (is_inside_roi([param['x4'], param['y4']], roi_w, roi_h)):
            points.append([param['x4'], param['y4'], i])
        if (is_inside_roi([param['x_center'], param['y_center']], roi_w, roi_h)):
            points.append([param['x_center'], param['y_center'], i])
    if (roi_w != None and roi_h != None):
        print(f"[经区域过滤之后的点数一共为] ====== [{len(points)}]")
    return points


def topological_similarity(adj1, adj2):
    """
    计算两个拓扑结构的相似度
    """
    # 计算邻接矩阵的相似度
    similarity = np.sum(adj1 == adj2) / (adj1.shape[0] * adj2.shape[1])
    return similarity


def find_topological_matches(points1, adj1, points2, adj2, threshold=0.8):
    """
    基于拓扑结构寻找匹配点集
    """
    matches = []
    for i in range(len(points1)):
        for j in range(len(points2)):
            # 计算拓扑相似度
            sim = topological_similarity(adj1[i], adj2[j])
            if sim > threshold:
                matches.append((i, j, sim))
    # 按相似度排序
    matches.sort(key=lambda x: x[2], reverse=True)
    return matches


from sklearn.linear_model import RANSACRegressor


def ransac_shape_matching(points, reference_points):
    model_ransac = RANSACRegressor(random_state=42)
    try:
        model_ransac.fit(reference_points, points[:len(reference_points)])  # 假设点数相同
    except ValueError as e:
        print("Error fitting the model:", e)
        return []

    inlier_mask = model_ransac.inlier_mask_
    best_match_subset = points[inlier_mask]

    return best_match_subset


def polar_to_cartesian(polar_points):
    r = polar_points[:, 0]
    theta = polar_points[:, 1]
    x = r * np.cos(theta)
    y = r * np.sin(theta)
    return np.vstack((x, y)).T


def compute_shape_context(points, nbins_r=5, nbins_theta=12):
    n = points.shape[0]
    shape_contexts = []
    r_max = np.max(distance.pdist(points))
    r_edges = np.logspace(-1, np.log10(r_max), nbins_r)
    theta_edges = np.linspace(-np.pi, np.pi, nbins_theta + 1)

    for i in range(n):
        current_point = points[i]
        relative_points = points - current_point
        rs = np.hypot(relative_points[:, 0], relative_points[:, 1])
        thetas = np.arctan2(relative_points[:, 1], relative_points[:, 0])
        H, _, _ = np.histogram2d(thetas, rs, bins=[theta_edges, r_edges])
        H /= np.sum(H) + 1e-8  # 归一化
        shape_contexts.append(H.flatten())

    return np.array(shape_contexts)


def match_shapes(sc1, sc2):
    cost_matrix = distance.cdist(sc1, sc2, metric='sqeuclidean')
    row_ind, col_ind = linear_sum_assignment(cost_matrix)
    return cost_matrix[row_ind, col_ind].sum()


def _sobel(image):
    '''
        _sobel
    '''
    if image.ndim > 2:
        image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    # todo 增加几个参数 http://blog.csdn.net/sunny2038/article/details/9170013
    _sobelx = cv2.Sobel(image, cv2.CV_64F, 1, 0, ksize=1)
    _sobely = cv2.Sobel(image, cv2.CV_64F, 0, 1, ksize=1)

    _sobelx = np.uint8(np.absolute(_sobelx))
    _sobely = np.uint8(np.absolute(_sobely))
    _sobelcombine = cv2.bitwise_or(_sobelx, _sobely)
    return _sobelcombine


def _findContours(image):
    '''
    _findContours
    http://blog.csdn.net/mokeding/article/details/20153325
    '''
    contours, _ = cv2.findContours(image.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    return sorted(contours, key=cv2.contourArea, reverse=True)


def bim_compare_to_hd_roi(hd_roi_list, hd_img_width, hd_img_height, bim_im_height,
                          bim_sub_area, bim_rect_list):
    """
    预埋件号码匹配

    将高清摄像头照片的ROI和实际BIM图中的号码匹配上

    参数:
    hd_roi_list : 高清摄像头的ROI数据列表，每个ROI表示一个可能的预埋件区域
            1.每个第二层子数组中的坐标顺序为左上、右上、右下、左下（顺时针）
            2.是以左上角为原点的常规的图片坐标系。
            格式如下。
            [
             [[x1, y1], [x2, y2], [x3, y3], [x4, y4]],
             [[x1, y1], [x2, y2], [x3, y3], [x4, y4]],
            ]
    hd_img_width (int): 高清相机拍摄图片原始宽度
    hd_img_height (int): 高清相机拍摄图片原始高度
    bim_im_height(int): bim图的高度
    bim_sub_area : 包含高清，广角，等坐标矩形区域数据
    bim_rect_list : bim的原始数据，坐标系上以左下角为原点的数学坐标系。
    返回: 数组，数组内包含多个字典，每个字典有如下元素。
        [
            {
                "bim_rect": bim_rect_item,
                "hd_roi": hd_roi
            }
        ]
    """
    # 高清相机坐标转换为bim图坐标,宽高肯定都是等比的直接按照bim上面高清矩形的宽度，算下比例。
    scale_w = bim_sub_area["hd_cam_w_bim"] / hd_img_width
    scale_h = bim_sub_area["hd_cam_h_bim"] / hd_img_height
    hd_roi_out_of_range_index = []  # 超出边界的ROI的索引
    for hd_roi_index, hd_roi in enumerate(hd_roi_list):
        for i in range(4):
            # 所有高清照片的ROI先按照这个比例转换 ，并且加上坐标偏移，转换为bim上实际坐标
            hd_roi[i][0] = int(hd_roi[i][0] * scale_w) + bim_sub_area["hd_cam_x_bim"]
            hd_roi[i][1] = int(hd_roi[i][1] * scale_h) + bim_sub_area["hd_cam_y_bim"]
        # 如果ROI坐标形成的矩形，横跨bim图上高清矩形范围边界框，就剔除这个矩形
        is_x_cross_left_border_line = hd_roi[0][0] < bim_sub_area["hd_cam_x_bim"] <= hd_roi[1][0]
        is_x_cross_right_border_line = hd_roi[0][0] < bim_sub_area["hd_cam_x_bim"] + bim_sub_area["hd_cam_w_bim"] <= \
                                       hd_roi[1][0]
        is_y_cross_top_border_line = hd_roi[0][1] < bim_sub_area["hd_cam_y_bim"] <= hd_roi[2][1]
        is_y_cross_bottom_border_line = hd_roi[0][1] < bim_sub_area["hd_cam_y_bim"] + bim_sub_area["hd_cam_h_bim"] <= \
                                        hd_roi[2][1]
        if is_x_cross_left_border_line or is_x_cross_right_border_line or is_y_cross_top_border_line or is_y_cross_bottom_border_line:
            hd_roi_out_of_range_index.append(hd_roi_index)
        # 画出来试试
        # bim_im = cv2.rectangle(bim_im, (hd_roi[0][0], hd_roi[0][1]), (hd_roi[2][0], hd_roi[2][1]), (0, 0, 255), 30)
        # 写上i编号
        # cv2.putText(bim_im, str(hd_roi_index), (hd_roi[0][0], hd_roi[0][1]), cv2.FONT_HERSHEY_SIMPLEX, 5, (0, 0, 255),20)
    # print(f"超出边界的ROI的索引是:{hd_roi_out_of_range_index}")

    # bim所有的矩形，注意bim原数据是以左下角为原点的坐标系 ！！！
    # bim所有的矩形，注意bim原数据是以左下角为原点的坐标系 ！！！
    # bim所有的矩形，注意bim原数据是以左下角为原点的坐标系 ！！！
    # 遍历bim所有的矩形，找到与高清相机ROI匹配的矩形
    bim_rect_hit_list = []
    for bim_rect_index, bim_rect_item in enumerate(bim_rect_list):
        # x不需要转换
        bim_rect_item_center_x = bim_rect_item["x"]
        # 把y坐标系转换成左上角坐标系
        bim_rect_item_center_y = bim_im_height - bim_rect_item["center"]
        # 逐一和高清相机ROI坐标进行匹配
        for hd_roi_index, hd_roi in enumerate(hd_roi_list):
            if(hd_roi_index in hd_roi_out_of_range_index):
                continue
            # 如果中心点的坐标在某个高清相机ROI内，就认为匹配上了。
            bim_rect_item_center_x_inside = hd_roi[0][0] < bim_rect_item_center_x < hd_roi[1][0]
            bim_rect_item_center_y_inside = hd_roi[0][1] < bim_rect_item_center_y < hd_roi[2][1]
            if bim_rect_item_center_x_inside and bim_rect_item_center_y_inside:
                bim_rect_hit_list.append({
                    "bim_rect": bim_rect_item,
                    "hd_roi": hd_roi
                })
    return bim_rect_hit_list
    # 画出bim_rect_hit_list
    # for bim_rect_hit_item in bim_rect_hit_list:
    #     bim_rect = bim_rect_hit_item["bim_rect"]
    #     hd_roi = bim_rect_hit_item["hd_roi"]
    #     bim_im = cv2.rectangle(bim_im, (hd_roi[0][0], hd_roi[0][1]), (hd_roi[2][0], hd_roi[2][1]), (0, 0, 255), 30)
    #     # 写上i编号
    #     cv2.putText(bim_im, str(bim_rect["code"]), (hd_roi[0][0]+100, hd_roi[0][1]+200), cv2.FONT_HERSHEY_SIMPLEX, 5, (0, 0, 255),
    #                 20)


def search(data_bim_json, data_sub_json, wide_cam_img_width, wide_cam_img_height):
    """
    根据广角相机的照片定位到bim图上面对应的位置

    参数：
        data_bim_json ：bim的json数据，左下角为原点的数学坐标系
        data_sub_json： 广角识别之后的ROI数据，左上角为原点的图片坐标系
        wide_cam_img_width:原始广角图片的宽度
        wide_cam_img_height：原始广角图片的高度

    返回：
        找到了：返回包含一堆坐标数据的字典
        找不到：返回None
    """

    # 读取并处理数据
    data_bim = {}
    data_bim["type"] = 0
    data_bim["params"] = data_bim_json
    data_bim["point"] = []
    data_bim["params"] = parmas_to_num(data_bim["params"])

    data_sub = {}
    data_sub["type"] = 0
    data_sub["params"] = []

    # 广角相机拍照照片之后左右边界的裁剪比例。
    wide_cam_left_cut_rate = 0.1
    wide_cam_right_cut_rate = 0.1
    # 高清相机的视场矩形在广角相机里面的坐标。cv2下的图片坐标系，以左上角为坐标原点
    hd_cam_x, hd_cam_y, hd_cam_w, hd_cam_h, = get_hd_cam_rect(wide_cam_left_cut_rate)

    # 创建测试子集
    # sub_im = cv2.imread("wide_image.png")
    # sub_zero = np.zeros_like(sub_im)
    # _im_gray = cv2.cvtColor(sub_im, cv2.COLOR_BGR2GRAY)
    # _im_gray = cv2.GaussianBlur(_im_gray, (5, 5), 0)
    # _im_edge_sobel = _sobel(_im_gray)
    # _, _im_thresh = cv2.threshold(_im_edge_sobel, 5, 255, cv2.THRESH_BINARY)
    # cnts = _findContours(_im_thresh)
    # for contour in cnts:
    #     x, y, w, h = cv2.boundingRect(contour)
    #     cv2.rectangle(sub_zero, (x, y), (x + w, y + h), (255, 255, 255), -1)
    # _im_edge_sobel = _sobel(sub_zero)
    # _, _im_thresh = cv2.threshold(_im_edge_sobel, 5, 255, cv2.THRESH_BINARY)
    # cnts = _findContours(_im_thresh)
    # 过滤矩形
    # cnts 过滤之后的矩形
    # sub_im 裁剪之后的图像
    cnts, wide_cam_img_width_after_cut = filter_rectangle(wide_cam_img_width, wide_cam_img_height, data_sub_json,
                                                          wide_cam_left_cut_rate,
                                                          wide_cam_right_cut_rate)
    # sub_zero = np.zeros_like(sub_im)
    for contour in cnts:
        # x, y, w, h = cv2.boundingRect(contour)
        x = contour["x"]
        y = contour["y"]
        w = contour["width"]
        h = contour["height"]

        # 由于定位框大小大于预埋件大小，因此这里需要做缩放处理
        # kh = int(h * 0.17)  # roi1
        # kw = int(w * 0.17)  # roi1
        # x += int(kw)
        # y += int(kh)
        # w -= int(kw)
        # h -= int(kh)
        x += 5
        y += 5
        w -= 10
        h -= 10
        param = {}
        param["x1"] = x
        param["y1"] = y
        param["x2"] = x + w
        param["y2"] = y
        param["x3"] = x + w
        param["y3"] = y + h
        param["x4"] = x
        param["y4"] = y + h
        param['x_center'] = int((param['x1'] + param['x2']) / 2)
        param['y_center'] = int((param['y1'] + param['y3']) / 2)
        param["w"] = param["x2"] - param["x1"]
        param["h"] = param["y3"] - param["y1"]
        param["x"] = int((param["x1"] + param["x2"]) / 2)
        param['center'] = wide_cam_img_height - int((param["y1"] + param["y3"]) / 2)
        data_sub["params"].append(param)
        # cv2.rectangle(sub_zero, (x, y), (x + w, y + h), (0, 255, 0), 1)
    # bim_im = gen_im_from_params(data_bim["params"])
    # cv2.namedWindow("bim", cv2.WINDOW_NORMAL)
    # cv2.imshow("bim", bim_im)
    # cv2.imwrite("bim_im.png", bim_im)
    # cv2.waitKey(0)
    # cv2.imshow("sub_zero", sub_zero)
    # cv2.waitKey(0)

    # data_sub = {}
    # data_sub["type"] = 0
    # data_sub["params"] = get_params(data_bim["params"], 1,8)
    # data_sub["point"] = []
    # data_sub["params"][0]["center"] += 20
    # data_sub["params"][0]["x"] += 13
    # data_sub["params"][0]["w"] += 40

    ##################### 开始计算 ####################
    # 1、计算sub的搜索路径
    ## 1.1 排序 y升序，x升序
    data_sub["params"] = sort_params(data_sub["params"])
    _sub_sort_params = data_sub["params"]
    start_time = time.time()

    # sub_roi_height = int(max_y) #????
    # sub_roi_width = int(max_x) #????
    sub_roi_height = wide_cam_img_height  # 广角图片的高度没有裁剪
    sub_roi_width = wide_cam_img_width_after_cut  # 广角图片的宽度裁剪过后变化了
    sub_roi_params_select_id = -1
    sub_roi_w_base_len = 0  # 选中预埋件的宽度作为基础长度
    sub_roi_divide_w_h = 1
    polar_origin_x = 0  # 极坐标原点 x
    polar_origin_y = 0  # 极坐标原点 y
    start_x = 0
    start_y = 0
    ## 1.2 选择一块完整的预埋件
    for i, param in enumerate(_sub_sort_params):
        if 0 < param['x1'] and param['x2'] < sub_roi_width \
                and 0 < param['y1'] and param['y3'] < sub_roi_height:
            sub_roi_params_select_id = i
            sub_roi_w_base_len = param['x2'] - param['x1']
            sub_roi_divide_w_h = param['w'] / param['h']
            polar_origin_x = int(param['x1'])  # 当前选择的预埋件的左上角 x 坐标
            polar_origin_y = int(param['y1'])  # 当前选择的预埋件的左上角 y 坐标
            break
    if sub_roi_params_select_id == -1 or sub_roi_w_base_len == 0:
        print("[ERROR]\t 拍摄的图像中没有完整的预埋件信息\n")
        assert (0)
    ## 1.2.2 将其他预埋件相对于它的极坐标进行填写
    for i, param in enumerate(_sub_sort_params):
        if i != sub_roi_params_select_id:
            param['r'], param['theta'] = cartesian_to_polar(_sub_sort_params[sub_roi_params_select_id]['x_center'],
                                                            _sub_sort_params[sub_roi_params_select_id]['y_center'],
                                                            param['x_center'], param['y_center'])

    ## 1.3计算所有点到该预埋件左上点的，点个数，平均极半径 和 平均极角度
    sum_r, sum_theta = 0.0, 0
    count = 0
    # 测试,画出所有的pts
    # for i, p in enumerate(_sub_sort_params):
    #     # 画点
    #     cv2.circle(sub_im, (p["x_center"], p["y_center"]), 2, (0, 255, 255), -1)
    # cv2.imshow("_sub_sort_params", sub_im)
    # cv2.waitKey(0)
    pts = gen_points_from_params(_sub_sort_params, sub_roi_width, sub_roi_height)
    # # 测试,画出所有的pts
    # for i, p in enumerate(pts):
    #     # 画点
    #     cv2.circle(sub_im, (p[0], p[1]), 2, (0, 255, 255), -1)
    #     # 写编号
    #     cv2.putText(sub_im, str(i), (p[0], p[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
    # cv2.rectangle(sub_im, (hd_cam_x, hd_cam_y), (hd_cam_x + hd_cam_w, hd_cam_y + hd_cam_h), (0, 255, 0), 4)
    # cv2.imshow("sub_im_points", sub_im)
    # cv2.waitKey(0)
    polar_list = []
    for i, pt in enumerate(pts):
        r, theta = cartesian_to_polar(polar_origin_x, polar_origin_y, pt[0], pt[1])
        sum_r += r
        sum_theta += theta
        count += 1
        polar_list.append([r, theta])
    sum_r /= count * sub_roi_w_base_len
    sum_theta /= count

    # 初始化候选预埋件
    candi_params = []
    for i, param in enumerate(data_bim["params"]):
        temp_div_w_h = param['w'] / param['h']
        if temp_div_w_h / sub_roi_divide_w_h < 1.5 and temp_div_w_h / sub_roi_divide_w_h > 0.66:
            candi_params.append(param)
    print(f"形状筛选后还剩下：{len(candi_params)} 个候选, w / h = {sub_roi_divide_w_h}")

    rst_params = []
    bim_all_pts = gen_points_from_params(data_bim["params"])
    # bim_im = gen_im_from_params(data_bim["params"])
    # sub_im = gen_im_from_params(data_sub["params"])# 需要读取
    min_match_score = 999999
    for i, param in enumerate(candi_params):
        tmp_roi_w_base_len = param['x2'] - param['x1']
        scale = tmp_roi_w_base_len / sub_roi_w_base_len
        tmp_roi_width = int(scale * sub_roi_width)
        tmp_roi_height = int(scale * sub_roi_height)
        # 计算广角矩形相对于bim图的坐标
        tmp_roi_start_x = int(param['x1'] - scale * polar_origin_x)
        tmp_roi_end_x = tmp_roi_start_x + tmp_roi_width

        tmp_roi_start_y = int(param['y1'] - scale * polar_origin_y)
        tmp_roi_end_y = tmp_roi_start_y + tmp_roi_height

        # 计算高清矩形，在bim上面的坐标。相当于被scale一起放大了
        hd_cam_x_bim = int(scale * hd_cam_x + tmp_roi_start_x)  # 左上角x
        hd_cam_y_bim = int(scale * hd_cam_y + tmp_roi_start_y)  # 左上角y
        hd_cam_w_bim = int(scale * hd_cam_w)
        hd_cam_h_bim = int(scale * hd_cam_h)

        tmp_roi_conners = [[tmp_roi_start_x, tmp_roi_start_y],
                           [tmp_roi_end_x, tmp_roi_start_y],
                           [tmp_roi_end_x, tmp_roi_end_y],
                           [tmp_roi_start_x, tmp_roi_end_y]]
        tmp_sum_r, tmp_sum_theta = 0.0, 0
        tmp_count = 0
        tmp_polar_list = []
        # 这里需要把选中的预埋件也提取出来
        param['effective_points'] = []
        for j, pt in enumerate(bim_all_pts):
            if cv2.pointPolygonTest(np.asarray(tmp_roi_conners), (pt[0], pt[1]), False) > 0:
                r, theta = cartesian_to_polar(param['x1'], param['y1'], pt[0], pt[1])
                tmp_sum_r += r
                tmp_sum_theta += theta
                tmp_count += 1
                tmp_polar_list.append([r, theta])
                # 搜集有效点，以供后续继续处理
                param['effective_points'].append(pt)
        tmp_sum_r /= tmp_count * tmp_roi_w_base_len
        tmp_sum_theta /= tmp_count

        # 预埋件数量相差 30%，则不进行计算
        # if tmp_count / count > 1.3 or tmp_count / count < 0.77: continue

        if abs(tmp_count - count) == 0:
            score = 0.5
        elif abs(tmp_count - count) <= 10:
            score = 0.4
        elif abs(tmp_count - count) <= 20:
            score = 0.3
        elif abs(tmp_count - count) <= 30:
            score = 0.2
        else:
            score = 0.0

        # else: score = (1 / abs(tmp_count - count) ) * 0.7
        score += (1 - abs(tmp_sum_r - sum_r) / sub_roi_width) * 0.25
        score += (1 - abs(tmp_sum_theta - sum_theta) / 3.14) * 0.35

        print("score=======", str(score))
        if score > 0.6:  # ????
            cartesian_points1 = polar_to_cartesian(np.asarray(tmp_polar_list))  # bim上的坐标
            cartesian_points2 = polar_to_cartesian(np.asarray(polar_list))  # sub上的坐标
            sc1 = compute_shape_context(cartesian_points1)
            sc2 = compute_shape_context(cartesian_points2)
            match_score = match_shapes(sc1, sc2)
            print("score>0.6")
            print(f"[score] ====== [{score}]")
            if match_score < 5.0:  # ????
                param["start_point"] = (tmp_roi_start_x, tmp_roi_start_y)
                param["end_point"] = (tmp_roi_end_x, tmp_roi_end_y)
                param["score"] = (score, tmp_count, tmp_sum_r * tmp_roi_w_base_len, tmp_sum_theta)
                param['match_score'] = match_score
                # 高清相机的矩形数据
                param['hd_cam_x_bim'] = hd_cam_x_bim
                param['hd_cam_y_bim'] = hd_cam_y_bim
                param['hd_cam_w_bim'] = hd_cam_w_bim
                param['hd_cam_h_bim'] = hd_cam_h_bim
                if min_match_score > match_score:
                    min_match_score = match_score
                rst_params.append(param)

    # 找到得分最大的
    max_score = -99999
    max_index = -1
    for i, param in enumerate(rst_params):
        score = param["score"]
        match_score = param["match_score"]
        if abs(score[0] / match_score) > max_score:
            max_score = abs(score[0] / match_score)
            max_index = i
    elapsed_time = time.time() - start_time
    print(f"Execution time: {elapsed_time:.4f} seconds")
    if max_index == -1:
        return None
    else:
        return rst_params[max_index]

    # 画出广角相机矩形框
    # bim_im = cv2.rectangle(bim_im, rst_p["start_point"], rst_p["end_point"], 100 * (i + 1), 50)
    # 画出高清相机矩形框
    # bim_im = cv2.rectangle(bim_im, (rst_p["hd_cam_x_bim"], rst_p["hd_cam_y_bim"]), (
    #     rst_p["hd_cam_x_bim"] + rst_p["hd_cam_w_bim"], rst_p["hd_cam_y_bim"] + rst_p["hd_cam_h_bim"],), (0, 0, 255), 40)

    # # 预埋件匹配
    # for i, param in enumerate(rst_params):
    #     score = param["score"]
    #     match_score = param["match_score"]
    #     print(f"[match_score] ====== [{match_score}]")
    #
    #     id = param["code"]
    #
    #     if min_match_score == match_score or match_score < 5.0: #????
    #     # if True:
    #         index_list = []
    #         for j in range(len(param['effective_points'])):
    #             pt = param['effective_points'][j]
    #             if pt[2] not in index_list:
    #                 index_list.append(param['effective_points'][j][2])
    #         # for i, param in enumerate(_sub_sort_params):
    #         #     param[]
    #         print(f"起始预埋件ID：{id}，置信度为：{score[0]}，点数量：{score[1]}，平均长度为：{score[2]}，平均角度为：{score[3]}")
    #         print(f"match_sscore为：{match_score}")
    #         print(f"[start_point] ====== [{param["start_point"]}]")
    #         print(f"[end_point] ====== [{param["end_point"]}]")
    #
    #         bim_im = cv2.rectangle(bim_im, param["start_point"], param["end_point"], 100 * (i + 1), 50)

    # img_matches = cv2.resize(bim_im, (int(bim_im.shape[1] / 6), int(bim_im.shape[0] / 6)))
    # sub_im = cv2.resize(sub_im, (int(sub_im.shape[1]/10), int(sub_im.shape[0]/10)))
    # cv2.imshow("2", img_matches)
    # cnts的矩形画在sub_im 上
    # for i in range(len(cnts)):
    #     p = cnts[i]
    #     cv2.rectangle(sub_im, (p['x'], p['y']), (p['x']+p['width'],p['y']+p['height']), (0, 0, 255), 2)
    #     # 写编号
    #     cv2.putText(sub_im, str(i), (p['x'], p['y']), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
    # cv2.imshow("sub_im_after_filter", sub_im)
    # cv2.waitKey(0)


if __name__ == "__main__":

    # ======================  广角定位 开始 =========================
    data_bim_json = read_from_json("data_bim.json")  # bim数据
    data_sub_json = read_from_json("data_sub/test_5/data_sub.json")  # 广角识别之后的ROI数据
    wide_cam_img_width = 640
    wide_cam_img_height = 480
    bim_sub_area = search(data_bim_json, data_sub_json, wide_cam_img_width, wide_cam_img_height)
    if bim_sub_area == None:
        print("未找到匹配区域")
        exit(0)
    else:
        print("bingo!!!!!!!")
    # ======================  广角定位 结束 =========================

    # ======================  预埋件号码匹配 开始 =========================
    bim_im = gen_im_from_params(data_bim_json)  # bim图
    hd_roi_list = get_hd_roi_from_txt("data_sub/test_1/roi_conners.txt")  # 高清摄像头的ROI数据
    hd_img_width = 9344
    hd_img_height = 7000
    bim_im_height = get_bim_height(data_bim_json)
    print(f"[bim_im_height] ====== [{bim_im_height}]")
    print(f"[bim_im.shape[0]] ====== [{bim_im.shape[0]}]")
    bim_rect_hit_list = bim_compare_to_hd_roi(hd_roi_list, hd_img_width, hd_img_height, bim_im_height, bim_sub_area,data_bim_json)
    # =====================  预埋件号码匹配 结束 ===========================


    # ==========  下面仅仅是测试画效果图 开始  ==========
    # 画出广角相机矩形框
    print("画出广角相机矩形框")
    print(f"起始点：{bim_sub_area['start_point']}")
    print(f"结束点：{bim_sub_area['end_point']}")
    cv2.rectangle(bim_im, bim_sub_area["start_point"], bim_sub_area["end_point"], 100, 50)
    # 画出高清相机矩形框
    cv2.rectangle(bim_im, (bim_sub_area["hd_cam_x_bim"], bim_sub_area["hd_cam_y_bim"]), (
        bim_sub_area["hd_cam_x_bim"] + bim_sub_area["hd_cam_w_bim"], bim_sub_area["hd_cam_y_bim"] + bim_sub_area["hd_cam_h_bim"],), (0, 0, 255), 40)
    # 在bim上画出高清识别对应的件号
    for bim_rect_hit_item in bim_rect_hit_list:
        bim_rect = bim_rect_hit_item["bim_rect"]
        hd_roi = bim_rect_hit_item["hd_roi"]
        cv2.rectangle(bim_im, (hd_roi[0][0], hd_roi[0][1]), (hd_roi[2][0], hd_roi[2][1]), (0, 0, 255), 30)
        # 写上i编号
        cv2.putText(bim_im, str(bim_rect["code"]), (hd_roi[0][0]+100, hd_roi[0][1]+200), cv2.FONT_HERSHEY_SIMPLEX, 5, (0, 0, 255),
                    20)
    bim_im_resize = cv2.resize(bim_im, (int(bim_im.shape[1] / 6), int(bim_im.shape[0] / 6)))
    cv2.imshow("bim_im_resize", bim_im_resize)

    image = cv2.imread("data_sub/test_5/wide_image.png")
    image_x_min = int(640 * 0.1)  # 左边界的裁剪点
    image_x_max = int(640 * (1 - 0.1))  # 右边界的裁剪点
    cropped_image = image[:, image_x_min:image_x_max]
    # 展示
    cv2.imshow("cropped_image", cropped_image)
    cv2.imshow("image", image)



    cv2.waitKey(0)
    # ==========  下面仅仅是测试画效果图 结束  ==========