（5）图像处理

 1、从matlab2022b读取 单目相机标定参数

import numpy as np
import cv2
"""此文件单独运行，用来导入Matlab2022b标定单目相机参数"""
############## matlab的标定值##################
# 1103.04069810984    0    952.894900237209
# 0    1104.11471051557    492.954822481377
# 0    0    1

# 径向 0.0303940709745075    -0.0635669030050014
# 切向 4.61519930830675e-05    -0.000416382854898642
# 假设从MATLAB导出的参数如下 (替换为你的实际值)


fx = 1103.04069810984    # 焦距x
fy = 1104.11471051557    # 焦距y
cx = 952.894900237209     # 主点x
cy = 492.954822481377     # 主点y
s = 0.0        # 倾斜系数 (通常为0)

# MATLAB标定的畸变系数 (2径向 + 2切向)
k1, k2, k3= 0.0303940709745075, -0.0635669030050014, 0        # 径向畸变系数
p1, p2 = 4.61519930830675e-05, -0.000416382854898642    # 切向畸变系数
image_size = (1920, 1080)    # 图像宽高

# 转换为OpenCV相机矩阵
camera_matrix = np.array([
    [fx, s, cx],
    [0, fy, cy],
    [0, 0, 1]
], dtype=np.float32)

# 转换为OpenCV畸变系数 (k3补0)
dist_coefficients = np.array([k1, k2, p1, p2, k3], dtype=np.float32)


#为了方便使用，可以将参数保存为文件
# 保存参数
np.savez('camera_params.npz',
         camera_matrix=camera_matrix,
         dist_coeffs=dist_coefficients,
         image_size=image_size)

# 加载参数
params = np.load('camera_params.npz')
camera_matrix = params['camera_matrix']
dist_coefficients = params['dist_coeffs']
image_size = tuple(params['image_size'])

2、img.py 识别圆点

 

# 文件名：img.py
# 20250504
import cv2
import numpy as np

#导入自己的类
import log
logger = log.logger_init()
#########################################################################
# 获取 含有疑似椭圆的灰度图
def get_object_mask(gray,img_show=True):
    # 初始化MSER检测器（使用正确参数名）
    mser = cv2.MSER_create(
        delta=5,
        min_area=100,
        max_area=800,
        max_variation=0.25,
        min_diversity=0.5
    )
    # 检测区域
    regions, _ = mser.detectRegions(gray)
    # 过滤 一部分不需要的objects
    contours = []
    for region in regions:
        if len(region) >= 20:  # 至少5个点才能拟合椭圆
            ellipse = cv2.fitEllipse(np.array(region))
            # 椭圆有效性验证
            _, (a, b), angle = ellipse
            if min(a, b) <= 0 :  # 过滤过于扁平的椭圆
                continue
            if max(a,b)/min(a,b)>=1.2:
                continue
            # area = cv2.contourArea(region)
            # if 3.14 * a * b / 4 > 1.2 * area:  # 轮廓包围面积过小
            #     continue
            contours.append(region)
    if img_show:
        # 可视化结果
        vis = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
        cv2.drawContours(vis,contours,-1,color=(0,255,0),thickness=2)
        cv2.imshow('MSER Detection', vis)
        cv2.waitKey(0)
        cv2.destroyAllWindows()
    # 创建模板
    rows, cols = gray.shape
    mask = np.zeros((rows, cols), dtype=np.uint8)
    cv2.drawContours(mask, contours, -1, 255, -1)
    # 使用模板
    # roi_gray = cv2.bitwise_and(gray, mask)
    # roi_gray = cv2.bitwise_or(roi_gray, mask)
    if img_show:
        cv2.imshow('mask',mask)
        cv2.waitKey(0)
        cv2.destroyAllWindows()
    return mask
def find_edges_in_gray(gray):
    """在灰度图中通过
    高斯滤波 ->canny->形态闭运算->找到外轮廓"""
    # 使用直方图均衡化  对低对比度图像
    # equalized = cv2.equalizeHist(gray)
    # 使用高斯模糊降噪（可选，推荐）
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)

    # Canny边缘检测
    low_threshold = 100
    high_threshold = 200
    edges = cv2.Canny(blurred, low_threshold, high_threshold)

    # 形态学操作  连接断裂边缘：
    kernel = np.ones((7, 7), np.uint8)
    closed = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel)
    edges = closed
    # 查找轮廓
    contours, hierarchy = cv2.findContours(
        edges,
        cv2.RETR_EXTERNAL,  # 只检测外部轮廓
        cv2.CHAIN_APPROX_SIMPLE  # 压缩水平、垂直和对角线段，只保留端点
    )
    return contours
def find_ellipse_in_edges(contours):
    """在边界曲线内找到接近圆的椭圆的圆心、轮廓线"""
    min_contour_area = 400  # 根据实际调整
    ellipse_contours = []
    ellipses = []
    for cnt in contours:
        # 过滤过小轮廓（根据实际调整）
        if len(cnt) < 10:  # 至少需要5个点拟合椭圆（实际可能需要更多）
            continue

        # 椭圆拟合
        if len(cnt) >= 10:
            ellipse = cv2.fitEllipse(cnt)
            (x, y), (major, minor), angle = ellipse

            # 筛选有效椭圆（根据实际需求调整条件）
            if max(major, minor) / min(major, minor) < 1.2:  # 按照近似为圆的特点筛选
                area = cv2.contourArea(cnt)
                # print(area)
                if area*1.4 < minor*minor*3.14/4:  # 面积特张筛选
                    continue  # 跳过小轮廓

                ellipses.append(ellipse)
                ellipse_contours.append(cnt)
    return ellipses, ellipse_contours

#########################################################################
# 获取 矩形的四个顶点
def get_centroid(points):
    """计算多边形顶点的质心（几何中心）"""
    x = [p[0] for p in points]
    y = [p[1] for p in points]
    centroid_x = sum(x) / len(points)
    centroid_y = sum(y) / len(points)
    return centroid_x, centroid_y
def sort_vertice_indices_clockwise(points):
    """按顺时针顺序排序顶点（支持凸/凹多边形）"""
    centroid = get_centroid(points)
    points = np.array(points)

    # 计算每个点相对于质心的极角（角度）
    angles = np.arctan2(points[:, 1] - centroid[1],#  1,2象限 【0 180】 3，4象限 【-180 0】
                        points[:, 0] - centroid[0])  # 极角计算时，建议使用np.arctan2而非np.arctan，避免除零错误

    # 按极角排序（顺时针）
    sorted_indices = np.argsort(angles)
    #sorted_points = points[sorted_indices]
    return sorted_indices

def point_to_line_distance(p, p1, p2):
    """
    计算点p到直线(p1, p2)的距离 p1=[x1,y1]，作为判断是否在直线上的依据
    """
    if np.allclose(p1, p2, atol=1e-6):
        return np.linalg.norm(p - p1)  # 如果p1和p2重合，退化为点到点距离
    return np.abs(np.cross(p2 - p1, p - p1)) / np.linalg.norm(p2 - p1)

def points_classifier_with_line(ellipses,max_ellipse_area =300):
    """
    (x, y), (major, minor), angle = ellipse
    将点按照是否近似在同一条直线上为依据，分类为集合
    """
    # 搜索三点共线的点
    from itertools import combinations
    lines = []
    n = len(ellipses)
    if n < 3:
        return lines
    line_point_indexes_list= list()
    # 遍历所有两点组合
    for i, j in combinations(range(n), 2):

        # 取椭圆圆心作为确定直线的两个点
        ellipse1, ellipse2 = ellipses[i], ellipses[j]
        p1 =  np.array(ellipse1[0],dtype=np.float32)
        p2 =  np.array(ellipse2[0],dtype=np.float32)
        ab1 =ellipse1[1]
        ab2 =ellipse2[1]
        if  ab1[0]*ab1[1]*3.14/4 > max_ellipse_area or ab2[0]*ab2[1]*3.14/4 > max_ellipse_area:
            continue
        # 跳过重合点
        thresh = min(ellipse1[1][0],ellipse1[1][1],ellipse2[1][0],ellipse2[1][1] )#阈值
        if np.allclose(p1, p2, atol=1e-2):
            continue
        # 找到当前直线上的所有点
        line_points = [p1, p2]
        line_index_set =[i,j]
        for k in range(n):
            if k == i or k == j:#去重复
                continue

            p3 = np.array([ellipses[k][0][0],ellipses[k][0][1]],dtype=np.float32)

            # 计算点到直线的距离
            epsilon = min(thresh,ellipses[k][1][0],ellipses[k][1][1])*0.1
            if point_to_line_distance(p3, p1, p2) < epsilon:
                line_points.append(p3)
                line_index_set.append(k)
                #print(p1,p2,p3)
        # 如果当前直线有至少3个点，添加记录
        if len(line_index_set) >= 3:
            line_point_indexes_list.append(set(line_index_set))
    # 去除重复的线
    for i in range(len(line_point_indexes_list)):
        set_i = line_point_indexes_list[i]
        if len(set_i)==0:
            continue
        for j in range(len(line_point_indexes_list)):
            if i == j:
                continue
            set_j = line_point_indexes_list[j]
            if len(set_j) == 0:
                continue
            set_ij = set_i.intersection(set_j)
            if len(set_ij)>=3:
                set_i = set_i.union(set_j)
                line_point_indexes_list[i] = set_i
                line_point_indexes_list[j] =set()

    line_point_indexes_list_ret = list()
    for each in line_point_indexes_list:
        if len(each)>=3:
            line_point_indexes_list_ret.append(sorted(list(each)))
    # 按直线上点的数量降序排序
    lines_sorted = sorted(line_point_indexes_list_ret, key=lambda x: len(x), reverse=True)
    return lines_sorted

def fit_line_opencv(points):
    """# 方法2：OpenCV的fitLine（可处理垂直线）,含垂直线
        points1 = np.array([[x1, y1], [x2, y2], ..., [xn, yn]], dtype=np.int32)
        points2 = np.array([[x1, y1], [x2, y2], ..., [xm, ym]], dtype=np.int32)
    """
    vx, vy, x0, y0 = cv2.fitLine(points, cv2.DIST_L2, 0, 0.01, 0.01)
    #(vx, vy)    是单位方向向量    (x0, y0)    是直线上的一点
    # 计算直线参数
    return float(vx), float(vy), float(x0), float(y0)
def fit_line_opencv_intersection(line1, line2):
    """
    计算两条直线 (p1-p2 和 p3-p4) 的交点
    :return: 交点坐标 (x, y)，若平行则返回 None
    """
    vx1, vy1, x1, y1 = line1
    vx2, vy2, x2, y2 = line2
    # 解方程组求t1和t2
    A = np.array([[vx1, -vx2], [vy1, -vy2]])
    b = np.array([[x2 - x1], [y2 - y1]])

    try:
        t1, t2 = np.linalg.solve(A, b)
        intersection_point = (float(x1 + t1 * vx1),float(y1 + t1 * vy1))
        return intersection_point
    except np.linalg.LinAlgError:
        # 直线平行或重合，无交点或无穷多交点
        return None
def fit_line_opencv_angle(line1, line2):
    vx1, vy1, _, _ = line1
    vx2, vy2, _, _ = line2
    # 计算方向向量的点积
    dot_product = vx1 * vx2 + vy1 * vy2
    # 计算向量的模
    magnitude1 = np.sqrt(vx1**2 + vy1**2)
    magnitude2 = np.sqrt(vx2**2 + vy2**2)
    # 计算夹角的余弦值
    cos_theta = dot_product / (magnitude1 * magnitude2)
    # 防止浮点误差导致arccos参数超出[-1,1]范围
    cos_theta = np.clip(cos_theta, -1.0, 1.0)
    # 计算夹角（弧度）
    angle_rad = np.arccos(cos_theta)
    # 转换为角度
    angle_deg = np.degrees(angle_rad)
    # 返回锐角
    return min(angle_deg, 180 - angle_deg)


def point_in_rotated_rectangle(point, vertices):
    """
    判断点是否在任意方向的矩形内（向量叉积法）
    :param point: 待检测的点 (x, y)
    :param vertices: 矩形的四个顶点 [(x1, y1), (x2, y2), (x3, y3), (x4, y4)]，按顺序排列(ccw 或cw)
    :return: True 如果在矩形内或边上，否则 False
    """
    x, y = point
    n = len(vertices)
    inside = True
    sign_now = True
    sign_last = None
    for i in range(n):
        x1, y1 = vertices[i]
        x2, y2 = vertices[(i + 1) % n]

        # 计算叉积 (P - P1) × (P2 - P1)
        cross = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1)

        # 如果叉积符号不一致，说明点在外部
        sign_now = True
        if cross < 0:
            sign_now = False

        if sign_last is None or (sign_now == sign_last):
            sign_last = sign_now
            continue
        else:
            inside = False
            break
    return inside
def find_k_closest_product_points(points, k):
    """
    找出 k 个点，使得它们的 x*y 最接近
    :param points: 点列表 [(x1, y1), (x2, y2), ...]
    :param k: 需要选取的点的数量
    :return: 最接近的 k 个点及其极差
    """
    if len(points) < k:
        return None, None  # 不足 k 个点，无法选取

    # 计算每个点的 x*y，并存储 (x*y, (x, y))
    products = [(x * y, (x, y)) for x, y in points]
    products.sort()  # 按 x*y 排序

    min_range = float('inf')
    best_window = []

    # 滑动窗口法，寻找极差最小的 k 个连续点
    for i in range(len(products) - k + 1):
        current_window = products[i:i + k]
        current_range = current_window[-1][0] - current_window[0][0]
        if current_range < min_range:
            min_range = current_range
            best_window = current_window

    # 提取点的坐标
    closest_points = [point for (prod, point) in best_window]
    # print(f"最接近的 {k} 个点: {closest_points}")
    # print(f"它们的 x*y 极差: {min_diff}")
    return closest_points, min_range
def app_get_rectangle_corner(ellipses,img_show=False):
    """
    :param ellipses, ellipse_contours: find_ellipse_in_edges()的返回值
    :return: corners 模板的四个点对  obj_points and img_points,顺时针排序
    """
    # 按照是否近似在一条直线上做分类
    line_indices = points_classifier_with_line(ellipses)
    #根据分类结果拟合直线方程
    line_equation_parameters = list()
    lines_points = list()
    for i in range(0, len(line_indices)):
        line = line_indices[i]
        if len(line) <= 3:
            continue
        line_points = list()
        for index in line:
            x, y = ellipses[index][0]
            line_points.append([x, y])
        points = np.array(line_points, dtype=np.float32)
        vx, vy, x0, y0 = fit_line_opencv(points)
        line_equation_parameters.append([vx, vy, x0, y0])
    # 遍历所有两直线组合，求取他们的交点和夹角，保留在矩形范围内的键值对
    two_line_intersection_angle = dict()
    from itertools import combinations
    for i, j in combinations(range(len(line_equation_parameters)), 2):
        line1, line2 = line_equation_parameters[i], line_equation_parameters[j]
        intersection = fit_line_opencv_intersection(line1, line2)
        # if not point_in_rotated_rectangle(intersection, [(100, 100), (1000, 100), (1000, 800), (100, 800)]):
        #      continue
        angle = fit_line_opencv_angle(line1, line2)
        two_line_intersection_angle[f'{i}_{j}'] = (intersection, angle)
    # 直角点
    del_keys =list()
    for key in two_line_intersection_angle.keys():
        value = two_line_intersection_angle[key]
        if value[1]<88:# 交叉角小于88度，认为不是矩形的顶点
            del_keys.append(key)
    for key in del_keys:
        del two_line_intersection_angle[key]
    # 顺时针排序
    points = []
    for value in two_line_intersection_angle.values():
        pt =  value[0]
        points.append(pt)
    if len(points)<4:
        logger.error("图像内信息不足。或有遮挡，去除遮挡重拍")
        return None
    centroid = get_centroid(points) # 多点的中心
    sorted_items = sorted(two_line_intersection_angle.items(), key=lambda item: np.arctan2(item[1][0][1] -centroid[1],item[1][0][0] -centroid[0]) )
    # 转换为有序字典（可选）
    from collections import OrderedDict
    sorted_dict = OrderedDict(sorted_items)
    if img_show:
        # 输出结果
        for key, value in sorted_items:
            logger.info(f"\n{key}: {value}")
    if len(sorted_dict)<4:
        logger.error("图像内信息不足。或有遮挡，去除遮挡重拍")
        return None
    # 取点坐标
    points = list()
    for value in sorted_dict.values():
        points.append(value[0])
    return points

def app_get_triangle_hole(ellipses,vertices,img_show=False):
    """
    查找零件的三个安装孔
    :param ellipses:  椭圆集
    :param vertices:  查找范围的包围点
    :param img_show:  是否显示调试信息
    :return: 三个点坐标 顺时针排列
    """
    # 缩小范围

    centroid = get_centroid(vertices)  # 多点的中心
    vertices_shrink=list()
    for point in vertices:
        x,y = point
        x,y = (x-centroid[0])*0.9 + centroid[0], (y - centroid[1]) * 0.9 + centroid[1]
        vertices_shrink.append([x,y])
    # 待选 椭圆
    candidate = list()
    for each in ellipses:
        point = each[0]
        if not point_in_rotated_rectangle(point, vertices_shrink):
            continue
        candidate.append(each)
    # 排序
    candidate_sorted = sorted(candidate, key= lambda each: each[1][0]*each[1][1],reverse=True)
    #print(candidate_sorted,len(candidate_sorted))

    if len(candidate_sorted)<3:
        logger.error("图像内信息不足。或有遮挡，去除遮挡重拍")
        return None
    k = 3
    product_list = list()
    for each in candidate_sorted:
        product_list.append(each[1][0]*each[1][1])
    score =1e2
    start = 0
    for i in range(len(product_list)-3+1):
        product =0
        data =product_list[i:i+3]
        # 样本均值
        mean = np.mean(data)
        #print("样本均值:", mean)  # 输出: 6.0
        # 样本标准差
        std_dev = np.std(data, ddof=1)# ddof=1 表示使用 n-1 校正
        #print("样本标准差:", std_dev)  # 输出: 3.162
        if mean <=0:
            mean = -mean
        mean = mean + 1e-2

        if std_dev/mean < score:#记录移动起点
           score = std_dev/mean
           start = i
    if img_show:
        logger.info(candidate_sorted)
        logger.info(candidate_sorted[start:start+3]  )

    #排序 顺时针
    hole_ellipses = candidate_sorted[start:start + 3]
    points = list()
    for each in hole_ellipses:
        points.append(each[0])
    cx,cy = get_centroid(points)  # 多点的中心
    hole_points = sorted(points,key=lambda point: np.arctan2(point[1] - cy, point[0] - cx))

    pt1, pt2, pt3 = np.array(hole_points[0], dtype=np.float32), np.array(hole_points[1], dtype=np.float32), np.array(
        hole_points[2], dtype=np.float32)

    """计算两点间欧几里得距离"""
    dist_1_2 = cv2.norm(pt1, pt2, cv2.NORM_L2)
    dist_2_3 = cv2.norm(pt2, pt3, cv2.NORM_L2)
    dist_3_1 = cv2.norm(pt1, pt3, cv2.NORM_L2)
    score = [0,0,0]
    # 判别等腰三角形顶点
    score[0] = (dist_1_2 - dist_3_1) ** 2
    score[1] = (dist_1_2 - dist_2_3) ** 2
    score[2] = (dist_2_3 - dist_3_1) ** 2
    id = min(range(len(score)), key=lambda i: score[i])# 顶点的下标
    sorted_hole_points= list()
    len_ls = len(hole_points)
    for i in range(len_ls): #第一点 是 等腰三角形的顶点
        j = i + id
        j = j %(len_ls)
        point = hole_points[j]
        sorted_hole_points.append(point)
    return sorted_hole_points

if __name__ == "__main__":
    # 读取图像
    image= cv2.imread('part04.jpg')
    cv2.imshow('灰度图', image)
    # 转换为灰度图
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    cv2.imwrite("result.jpg",gray)
    # 近似椭圆的object的灰度图
    gray = get_object_mask(gray=gray,img_show=False)

    cv2.imshow('灰度图', gray)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

    # 边界轮廓线
    edges = find_edges_in_gray(gray)
    # 椭圆及轮廓线
    ellipses, ellipse_contours = find_ellipse_in_edges(edges)
    #椭圆集所在矩形的顶点，顺时针排序
    vertices = app_get_rectangle_corner(ellipses,img_show=False)
    # 零件的三个安装孔 第一个点是等腰三角形的顶点
    hole_points = app_get_triangle_hole(ellipses,vertices,img_show=False)
    print(hole_points)

    line_indices = points_classifier_with_line(ellipses)
    line_contours = list()
    for i in range(0,len(line_indices)):
        line = line_indices[i]
        if len(line)<=3:
            continue
        line_points =list()
        for index in line:
            x,y = ellipses[index][0]
            line_points.append([[int(x),int(y)]])
        line_contours.append( np.array(line_points, dtype=np.int32))


    vis = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
    # 椭圆
    cv2.drawContours(vis,ellipse_contours,-1,color=(0,0,255),thickness=2)
    # 线段
    cv2.polylines(vis, line_contours, isClosed=False, color=(0, 255, 0), thickness=2)
    # 定位孔
    cv2.polylines(vis, [np.array(hole_points,dtype=np.int32)], isClosed=True, color=(255, 0, 0), thickness=2)
    cv2.imshow('ellipse_contours', vis)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

 

3） 根据像素坐标、物体自身坐标 计算它的相机坐标

# 文件名  calibration_robot.py
import cv2
import numpy as np

#导入自己的类
import log

logger = log.logger_init()

def get_chessboard_object_points(chessboard_size = (11, 8),square_size=1,translation_x =0,translation_y =0,translation_z =0):
    """
     使用棋盘格标定 机器人坐标和相机坐标
    :param chessboard_size:  (8, 6) 棋盘格的大小 (内部角点的数量)
    :param square_size: 棋盘格边长 mm
    :param translation_x: 在机器人坐标系下，棋盘格起点坐标
    :param translation_y: 在机器人坐标系下，棋盘格起点坐标
    :param translation_z: 在机器人坐标系下，棋盘格起点坐标
    :return:
    """

    # 准备对象点 (例如 (0,0,0), (1,0,0), (2,0,0), ....,(7,5,0))
    objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
    #print(objp)
    #objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
    objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
    objp = objp * square_size+[translation_x,translation_y,translation_z]
    # 使用列表推导式交换每个子列表的第0和第1元素
    #objp = [[7-sublist[1], sublist[0], *sublist[2:]] for sublist in objp]
    objp = [[sublist[1], sublist[0], *sublist[2:]] for sublist in objp]
    return objp

def get_chessboard_image_points(image=None,chessboard_size=(8, 6)):
    """
    一张图中找到棋盘格的角点
    :param image: 图像文件路径
    :param chessboard_size: 棋盘格的大小 (内部角点的数量)
    :return:
    """
    img = image
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 找到棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    # 如果找到角点，则添加对象点和图像点
    if ret:
        # 3. 优化角点精度（亚像素级）
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        return corners
    else:
        return None
def detect_and_label_checkerboard(image_path, pattern_size=(11, 8),image_show=True):
    """
    识别棋盘格并标注序号
    :param image_path: 图像路径
    :param pattern_size: 棋盘格内角点数量 (rows, cols)
    """
    # 1. 读取图像
    img = cv2.imread(image_path)
    if img is None:
        print("Error: 图像无法加载！")
        return

    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 2. 检测棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if not ret:
        print("Error: 未检测到棋盘格！")
        return

    # 3. 优化角点精度（亚像素级）
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

    if image_show:
        # 4. 绘制角点并标注序号
        img_draw = img.copy()
        cv2.drawChessboardCorners(img_draw, pattern_size, corners, ret)

        # 标注序号 (0,0), (0,1), ..., (n,m)
        for i, corner in enumerate(corners):
            x, y = corner.ravel().astype(int)
            row = i // pattern_size[1]  # 行号
            col = i % pattern_size[1]  # 列号
            cv2.putText(img_draw, f"({row},{col})", (x + 10, y - 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

        # 5. 显示结果
        cv2.imshow("Checkerboard with Labels", img_draw)
        cv2.waitKey(0)
        cv2.destroyAllWindows()

    # 可选：保存结果
    #cv2.imwrite("labeled_checkerboard.jpg", img_draw)
    return corners

#代码
class LocationPart:
    """在相机坐标系下，
    零件的定位
    零件的角度
    模板的定位
    """
    def __init__(self):
        # 加载相机标定参数
        params = np.load('camera_params.npz')
        self.camera_matrix = params['camera_matrix']
        self.dist_coefficients = params['dist_coeffs']
        self.image_size = tuple(params['image_size'])

    def get_rotation_transform_vectors(self,obj_points,img_points):
        """物体的坐标为平面三个点或以上，三点不共线
           返回 ret, rotation_vec, transform_vec
        """
        # 只有3个点时
        # obj_points = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float32)
        # img_points = np.array([[1000, 600], [1200, 580], [980, 400]], dtype=np.float32)
        # 方法A：使用SQPNP算法（需要OpenCV 3.4+） 平面三个点及以上
        ret, rotation_vec, transform_vec = cv2.solvePnP(obj_points, img_points, self.camera_matrix, self.dist_coefficients,
                                       flags=cv2.SOLVEPNP_SQPNP)
        # 返回结果
        return ret, rotation_vec, transform_vec

    @staticmethod
    def get_camera_coordinates(obj_points,rotation_vec, transform_vec):
        """
        由物体的世界点坐标，计算相机坐标系下的点坐标
        :param obj_points:
        # 平面物体上的三个点 (Z=0)
        obj_points = np.array([[0, 0, 0],    # 原点
                               [1, 0, 0],    # X轴1单位
                               [0, 1, 0]],   # Y轴1单位
                              dtype=np.float32)
        :param rotation_vec:
        :param transform_vec:
        :return:
        """
        # 1. 转换旋转向量为矩阵
        R, _ = cv2.Rodrigues(rotation_vec)
        # 2. 计算相机坐标系坐标
        camera_points = []
        for pw in obj_points:
            pc = R @ pw.reshape(3, 1) + transform_vec # 旋转，然后平移
            camera_points.append(pc.flatten())
        camera_points = np.array(camera_points)
        return camera_points

    @staticmethod
    def get_triangle_obj_points(img_points):
        """
        已抓取中心为原点，抓取方向定坐标轴
        根据安装孔位置成等腰三角形，已知位置尺寸，已计算它的坐标  [(0, 77,0), (52.5, -50,0), (-52.5, -50,0)]
        这里匹配 世界坐标和像素坐标
        :param img_points:
        :return: obj_points mm
        """
        pt1,pt2,pt3 = np.array(img_points[0],dtype=np.float32),np.array(img_points[1],dtype=np.float32),np.array(img_points[2],dtype=np.float32)

        """计算两点间欧几里得距离"""
        dist_1_2 = cv2.norm(pt1, pt2, cv2.NORM_L2)
        dist_2_3 = cv2.norm(pt2, pt3, cv2.NORM_L2)
        dist_3_1 = cv2.norm(pt1, pt3, cv2.NORM_L2)
        score =list()
        # 判别等腰三角形顶点
        score[0] = (dist_1_2 - dist_3_1) ** 2
        score[1] = (dist_1_2 - dist_2_3) ** 2
        score[2] = (dist_2_3 - dist_3_1) ** 2

        id = min(range(len(score)), key=lambda i: score[i])
        if id ==0:
            return [(0, 77,0), (52.5, -50,0), (-52.5, -50,0)]
        if id ==1:
            return [ (52.5, -50, 0), (0, 77, 0),(-52.5, -50, 0)]
        if id ==2:
            return [(52.5, -50, 0), (-52.5, -50, 0), (0, 77, 0)]

    @staticmethod
    def get_rectangle_obj_points(img_points):
        """
        已知矩形的长宽分别是：240*280 mm
        :param img_points:
        :return:obj_points
        根据像素坐标，匹配物体为参照的世界坐标值
           (0,0),(240,0),(240,280),(0,280)
           (0,0),(280,0),(280,240),(0,240)
        """
        if len(img_points) !=4:
            return None
        pt1,pt2,pt3 = np.array(img_points[0],dtype=np.float32),np.array(img_points[1],dtype=np.float32),np.array(img_points[2],dtype=np.float32)
        """计算两点间欧几里得距离"""
        dist_1_2 = cv2.norm(pt1, pt2, cv2.NORM_L2)
        dist_2_3 = cv2.norm(pt2, pt3, cv2.NORM_L2)
        if dist_1_2> dist_2_3:
            return (0,0,0),(280,0,0),(280,240,0),(0,240,0)
        else:
            return (0,0,0),(240,0,0),(240,280,0),(0,280,0)

if __name__ == "__main__":
    #读取图像
    image = cv2.imread('chessboard.jpg')
    if image is None:
        raise FileNotFoundError("未找到图像文件，请检查路径是否正确")

    obj_points=get_chessboard_object_points(square_size=1,translation_x =0,translation_y =0,translation_z =0)
    img_points = get_chessboard_image_points(image=image,chessboard_size=(11,8))

    # 示例调用
    img_points = detect_and_label_checkerboard("chessboard.jpg", pattern_size=(11, 8))

    for i in range(len(img_points)):
        print(img_points[i],obj_points[i])