# ── 環(huán)境準(zhǔn)備 ──────────────────────────────────────────────────────────────
import cv2
import numpy as np
# import matplotlib
import platform
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
# from matplotlib import font_manager
# import warnings
# warnings.filterwarnings('ignore')

# # 配置中文字體（使用系統(tǒng)內(nèi)置的 Noto CJK 字體）
# _FONT_PATH = '/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc'
# try:
#     _zh_font = font_manager.FontProperties(fname=_FONT_PATH)
#     _zh_font_sm = font_manager.FontProperties(fname=_FONT_PATH, size=9)
#     _zh_font_md = font_manager.FontProperties(fname=_FONT_PATH, size=10)
#     _zh_font_lg = font_manager.FontProperties(fname=_FONT_PATH, size=13)
#     _zh_font_xl = font_manager.FontProperties(fname=_FONT_PATH, size=14)
#     print("? 中文字體加載成功")
# except Exception:
#     _zh_font = _zh_font_sm = _zh_font_md = _zh_font_lg = _zh_font_xl = None
#     print("? 未找到 Noto CJK 字體，中文標(biāo)注可能顯示為方框")

if platform.system() == 'Windows':
    plt.rcParams['font.sans-serif'] = ['SimHei']
else:
    plt.rcParams['font.sans-serif'] = ['WenQuanYi Zen Hei']
plt.rcParams['axes.unicode_minus'] = False

print(f"OpenCV 版本: {cv2.__version__}")
print(f"NumPy  版本: {np.__version__}")

# ── 圖1：四個(gè)坐標(biāo)系關(guān)系示意圖 ──────────────────────────────────────────────
fig, ax = plt.subplots(figsize=(13, 3))
ax.axis('off')
ax.set_xlim(0, 11)
ax.set_ylim(0, 2.8)

boxes = [
    ('世界坐標(biāo)系\n$(X_w, Y_w, Z_w)$', 0.4),
    ('相機(jī)坐標(biāo)系\n$(X_c, Y_c, Z_c)$', 3.0),
    ('圖像物理坐標(biāo)系\n$(x, y)$', 5.6),
    ('像素坐標(biāo)系\n$(u, v)$', 8.2),
]
arrows = ['旋轉(zhuǎn) R\n平移 t', '透視投影\n焦距 f', '縮放\n平移']

for (label, x) in boxes:
    rect = mpatches.FancyBboxPatch((x, 0.9), 2.2, 1.2,
                                    boxstyle='round,pad=0.1',
                                    linewidth=1.8, edgecolor='steelblue',
                                    facecolor='#ddeeff')
    ax.add_patch(rect)
    # ax.text(x + 1.1, 1.5, label, ha='center', va='center',
    #         fontproperties=_zh_font_sm)
    ax.text(x + 1.1, 1.5, label, ha='center', va='center')
for arrow_label, x_start in zip(arrows, [2.6, 5.2, 7.8]):
    ax.annotate('', xy=(x_start + 0.4, 1.5), xytext=(x_start, 1.5),
                arrowprops=dict(arrowstyle='->', color='tomato', lw=2))
    # ax.text(x_start + 0.2, 2.1, arrow_label, ha='center', va='bottom',
    #         fontproperties=_zh_font_sm, color='tomato')
    ax.text(x_start + 0.2, 2.1, arrow_label, ha='center', va='bottom', color='tomato')

# ax.set_title('相機(jī)成像的四個(gè)坐標(biāo)系及其轉(zhuǎn)換關(guān)系', fontproperties=_zh_font_xl)
ax.set_title('相機(jī)成像的四個(gè)坐標(biāo)系及其轉(zhuǎn)換關(guān)系')
plt.tight_layout()
plt.show()

# ── 圖2：針孔相機(jī)成像原理示意圖 ────────────────────────────────────────────
fig, ax = plt.subplots(figsize=(11, 5))
ax.set_xlim(0, 10)
ax.set_ylim(-3.2, 3.2)
ax.set_aspect('equal')
ax.axis('off')

# 光軸
ax.annotate('', xy=(9.5, 0), xytext=(0.5, 0),
            arrowprops=dict(arrowstyle='->', color='gray', lw=1.5))
# ax.text(9.55, 0.15, '光軸 Z', fontsize=10, color='gray', fontproperties=_zh_font_md)
ax.text(9.55, 0.15, '光軸 Z', fontsize=10, color='gray')

# 物體
ax.plot([2, 2], [0, 2.2], 'g-', lw=4)
ax.plot([2], [2.2], 'g^', markersize=14)
# ax.text(1.4, -0.45, '物體 P', fontsize=10, ha='center', fontproperties=_zh_font_md)
ax.text(1.4, -0.45, '物體 P', fontsize=10, ha='center')

# 光心
ax.plot(5, 0, 'ko', markersize=12)
# ax.text(5, -0.5, '光心 O', fontsize=10, ha='center', fontproperties=_zh_font_md)
ax.text(5, -0.5, '光心 O', fontsize=10, ha='center')

# 成像平面
ax.plot([7.5, 7.5], [-2.2, 2.2], 'b-', lw=3)
# ax.text(7.5, -2.7, '成像平面', fontsize=10, ha='center', fontproperties=_zh_font_md)
ax.text(7.5, -2.7, '成像平面', fontsize=10, ha='center')

# 投影線
ax.plot([2, 7.5], [2.2, -1.65], 'r--', lw=1.5, alpha=0.7)
ax.plot([2, 7.5], [0, 0], 'r--', lw=1.5, alpha=0.7)

# 像點(diǎn)
ax.plot([7.5], [-1.65], 'r^', markersize=12)
# ax.text(7.85, -1.65, "像點(diǎn) p'", fontsize=10, fontproperties=_zh_font_md)
ax.text(7.85, -1.65, "像點(diǎn) p'", fontsize=10)

# 焦距標(biāo)注
ax.annotate('', xy=(7.5, -3.0), xytext=(5, -3.0),
            arrowprops=dict(arrowstyle='<->', color='purple', lw=1.5))
# ax.text(6.25, -3.2, '焦距 f', fontsize=10, ha='center', color='purple', fontproperties=_zh_font_md)
ax.text(6.25, -3.2, '焦距 f', fontsize=10, ha='center', color='purple')

# 物距標(biāo)注
ax.annotate('', xy=(5, 3.0), xytext=(2, 3.0),
            arrowprops=dict(arrowstyle='<->', color='darkgreen', lw=1.5))
# ax.text(3.5, 3.15, '物距 Z', fontsize=10, ha='center', color='darkgreen', fontproperties=_zh_font_md)
ax.text(3.5, 3.15, '物距 Z', fontsize=10, ha='center', color='darkgreen')

# ax.set_title('針孔相機(jī)成像原理示意圖', fontproperties=_zh_font_xl, pad=15)
ax.set_title('針孔相機(jī)成像原理示意圖', pad=15)
plt.tight_layout()
plt.show()

# ── 圖3：徑向畸變類型對比 ──────────────────────────────────────────────────
fig, axes = plt.subplots(1, 3, figsize=(13, 4.5))

def draw_distorted_grid(ax, distort_fn, title):
    ax.set_xlim(-1.5, 1.5)
    ax.set_ylim(-1.5, 1.5)
    ax.set_aspect('equal')
    ax.axis('off')
    # ax.set_title(title, fontproperties=_zh_font_lg)
    ax.set_title(title)
    for v in np.linspace(-1.2, 1.2, 7):
        pts = [distort_fn(u, v) for u in np.linspace(-1.2, 1.2, 120)]
        ax.plot([p[0] for p in pts], [p[1] for p in pts], 'steelblue', lw=1.2)
    for u in np.linspace(-1.2, 1.2, 7):
        pts = [distort_fn(u, v) for v in np.linspace(-1.2, 1.2, 120)]
        ax.plot([p[0] for p in pts], [p[1] for p in pts], 'steelblue', lw=1.2)

draw_distorted_grid(axes[0], lambda x, y: (x, y), '無畸變（理想模型）')
draw_distorted_grid(axes[1],
    lambda x, y: (x*(1 - 0.3*(x**2+y**2)), y*(1 - 0.3*(x**2+y**2))),
    '桶形畸變\n($k_1 < 0$，圖像向內(nèi)收縮)')
draw_distorted_grid(axes[2],
    lambda x, y: (x*(1 + 0.3*(x**2+y**2)), y*(1 + 0.3*(x**2+y**2))),
    '枕形畸變\n($k_1 > 0$，圖像向外擴(kuò)張)')

# plt.suptitle('徑向畸變類型示意圖', fontproperties=_zh_font_xl, y=1.02)
plt.suptitle('徑向畸變類型示意圖', y=1.02)
plt.tight_layout()
plt.show()

# ── 圖4：張正友標(biāo)定法完整流程圖 ────────────────────────────────────────────
fig, ax = plt.subplots(figsize=(11, 8))
ax.axis('off')
ax.set_xlim(0, 10)
ax.set_ylim(0, 10)

steps = [
    ('① 打印棋盤格標(biāo)定板，從多角度拍攝若干張圖像（建議 15~20 張）', '#4472C4', '#dce6f1'),
    ('② 調(diào)用 findChessboardCorners 提取每張圖像的內(nèi)角點(diǎn)像素坐標(biāo)', '#ED7D31', '#fce4d6'),
    ('③ 調(diào)用 cornerSubPix 將角點(diǎn)坐標(biāo)精化至亞像素級別', '#70AD47', '#e2efda'),
    ('④ 調(diào)用 calibrateCamera 求解內(nèi)參矩陣 K 和畸變系數(shù) D', '#FF0000', '#ffd7d7'),
    ('⑤ 利用 undistort / initUndistortRectifyMap 對圖像進(jìn)行去畸變', '#7030A0', '#ede7f6'),
    ('⑥ 評估重投影誤差，必要時(shí)增補(bǔ)圖像并重新標(biāo)定', '#C00000', '#f4cccc'),
]

y_positions = [9.0, 7.4, 5.8, 4.2, 2.6, 1.0]

for (text, edge_color, face_color), y in zip(steps, y_positions):
    rect = mpatches.FancyBboxPatch((0.4, y - 0.55), 9.2, 1.1,
                                    boxstyle='round,pad=0.1',
                                    linewidth=2, edgecolor=edge_color,
                                    facecolor=face_color)
    ax.add_patch(rect)
    # ax.text(5, y, text, ha='center', va='center',
    #         fontproperties=_zh_font_md, color='#1a1a1a')
    ax.text(5, y, text, ha='center', va='center', color='#1a1a1a')

for y_cur, y_next in zip(y_positions[:-1], y_positions[1:]):
    ax.annotate('', xy=(5, y_next + 0.55), xytext=(5, y_cur - 0.55),
                arrowprops=dict(arrowstyle='->', color='#444444', lw=2))

# ax.set_title('張正友相機(jī)標(biāo)定法完整流程（OpenCV 實(shí)現(xiàn)）',
#              fontproperties=_zh_font_xl, pad=12)
ax.set_title('張正友相機(jī)標(biāo)定法完整流程（OpenCV 實(shí)現(xiàn)）', pad=12)
plt.tight_layout()
plt.show()

# ── 步驟 1：生成模擬棋盤格圖像（實(shí)際應(yīng)用中應(yīng)替換為真實(shí)拍攝的照片）─────────

CHECKERBOARD = (6, 9)   # 棋盤格內(nèi)角點(diǎn)數(shù)量 (行數(shù), 列數(shù))
SQUARE_SIZE  = 0.025    # 每個(gè)方格的物理邊長（單位：米）

def generate_checkerboard(rows, cols, step=56, margin=60,
                           img_h=480, img_w=640):
    """生成標(biāo)準(zhǔn)棋盤格灰度圖像。"""
    img = np.ones((img_h, img_w), dtype=np.uint8) * 255
    for i in range(cols + 1):
        for j in range(rows + 1):
            if (i + j) % 2 == 0:
                x1 = margin + i * step
                y1 = margin + j * step
                x2 = min(x1 + step, img_w - 1)
                y2 = min(y1 + step, img_h - 1)
                cv2.rectangle(img, (x1, y1), (x2, y2), 0, -1)
    return img

# 生成一張基準(zhǔn)棋盤格圖像
base_board = generate_checkerboard(*CHECKERBOARD)

# 通過仿射變換模擬多角度拍攝，生成 12 張不同視角的圖像
def make_perspective_views(base, n=12):
    h, w = base.shape
    views = []
    np.random.seed(42)
    for _ in range(n):
        # 隨機(jī)仿射變換（模擬輕微旋轉(zhuǎn)和縮放）
        angle = np.random.uniform(-12, 12)
        scale = np.random.uniform(0.85, 1.0)
        M = cv2.getRotationMatrix2D((w // 2, h // 2), angle, scale)
        M[0, 2] += np.random.uniform(-20, 20)
        M[1, 2] += np.random.uniform(-15, 15)
        warped = cv2.warpAffine(base, M, (w, h),
                                flags=cv2.INTER_LINEAR,
                                borderMode=cv2.BORDER_CONSTANT,
                                borderValue=255)
        views.append(warped)
    return views

images = make_perspective_views(base_board, n=12)
print(f"共生成 {len(images)} 張模擬標(biāo)定圖像，尺寸：{images[0].shape}")

# ── 步驟 2：角點(diǎn)檢測與亞像素精化 ──────────────────────────────────────────

# 停止準(zhǔn)則：最大迭代次數(shù) 30 次，或精度達(dá)到 0.001 像素
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 準(zhǔn)備世界坐標(biāo)系中的角點(diǎn)坐標(biāo)（Z=0 平面）
objp = np.zeros((CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:CHECKERBOARD[0],
                        0:CHECKERBOARD[1]].T.reshape(-1, 2) * SQUARE_SIZE

objpoints = []  # 3D 世界坐標(biāo)點(diǎn)列表
imgpoints = []  # 2D 圖像像素坐標(biāo)點(diǎn)列表
valid_images = []

for idx, gray in enumerate(images):
    ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, None)
    if ret:
        # cornerSubPix：將角點(diǎn)精化到亞像素精度
        corners_refined = cv2.cornerSubPix(
            gray, corners, (11, 11), (-1, -1), criteria)
        objpoints.append(objp)
        imgpoints.append(corners_refined)
        valid_images.append(gray)

print(f"成功檢測到角點(diǎn)的圖像數(shù)量：{len(valid_images)} / {len(images)}")

# 可視化第一張有效圖像的角點(diǎn)檢測結(jié)果
img_vis = cv2.cvtColor(valid_images[0], cv2.COLOR_GRAY2BGR)
cv2.drawChessboardCorners(img_vis, CHECKERBOARD, imgpoints[0], True)

fig, axes = plt.subplots(1, 2, figsize=(13, 5))
axes[0].imshow(valid_images[0], cmap='gray')
# axes[0].set_title('原始棋盤格圖像', fontproperties=_zh_font_lg)
axes[0].set_title('原始棋盤格圖像')
axes[0].axis('off')
axes[1].imshow(cv2.cvtColor(img_vis, cv2.COLOR_BGR2RGB))
# axes[1].set_title('角點(diǎn)檢測結(jié)果（findChessboardCorners + cornerSubPix）',
#                    fontproperties=_zh_font_lg)
axes[1].set_title('角點(diǎn)檢測結(jié)果（findChessboardCorners + cornerSubPix）')
axes[1].axis('off')
# plt.suptitle('張正友標(biāo)定法 — 棋盤格角點(diǎn)檢測與精化', fontproperties=_zh_font_xl)
plt.suptitle('張正友標(biāo)定法 — 棋盤格角點(diǎn)檢測與精化')
plt.tight_layout()
plt.show()

# ── 步驟 3：執(zhí)行相機(jī)標(biāo)定 ───────────────────────────────────────────────────

img_size = (valid_images[0].shape[1], valid_images[0].shape[0])  # (width, height)

# cv2.calibrateCamera 返回：
#   ret    — 平均重投影誤差（越小越好，通常要求 < 1.0 像素）
#   mtx    — 3×3 相機(jī)內(nèi)參矩陣 K
#   dist   — 畸變系數(shù)向量 [k1, k2, p1, p2, k3]
#   rvecs  — 每張圖像的旋轉(zhuǎn)向量列表
#   tvecs  — 每張圖像的平移向量列表
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, img_size, None, None)

print("=" * 55)
print("  相機(jī)標(biāo)定結(jié)果")
print("=" * 55)
print(f"\n平均重投影誤差 (RMS): {ret:.4f} 像素")
print("\n內(nèi)參矩陣 K:")
print(f"  fx = {mtx[0,0]:.2f} px,  fy = {mtx[1,1]:.2f} px")
print(f"  cx = {mtx[0,2]:.2f} px,  cy = {mtx[1,2]:.2f} px")
print("\n  K =\n", np.round(mtx, 2))
print("\n畸變系數(shù) [k1, k2, p1, p2, k3]:")
print("  ", np.round(dist.ravel(), 6))
print("=" * 55)

# ── 步驟 4：去畸變處理 ────────────────────────────────────────────────────
# 模擬一張帶有桶形畸變的網(wǎng)格圖像，演示去畸變效果

h_g, w_g = 400, 400
img_grid = np.ones((h_g, w_g, 3), dtype=np.uint8) * 255
for i in range(0, h_g, 40):
    cv2.line(img_grid, (0, i), (w_g, i), (160, 160, 160), 1)
for j in range(0, w_g, 40):
    cv2.line(img_grid, (j, 0), (j, h_g), (160, 160, 160), 1)
cv2.rectangle(img_grid, (0, 0), (w_g-1, h_g-1), (0, 0, 0), 2)

# 使用模擬的相機(jī)參數(shù)和畸變系數(shù)引入桶形畸變
K_sim = np.array([[350., 0., 200.], [0., 350., 200.], [0., 0., 1.]])
D_sim = np.array([-0.4, 0.15, 0., 0., 0.])
map1, map2 = cv2.initUndistortRectifyMap(
    K_sim, D_sim, None, K_sim, (w_g, h_g), cv2.CV_32FC1)
img_distorted = cv2.remap(img_grid, map1, map2, cv2.INTER_LINEAR)

# 使用 cv2.undistort 進(jìn)行去畸變
img_undistorted = cv2.undistort(img_distorted, K_sim, D_sim)

fig, axes = plt.subplots(1, 3, figsize=(14, 5))
axes[0].imshow(img_grid)
# axes[0].set_title('原始網(wǎng)格圖像（理想無畸變）', fontproperties=_zh_font_lg)
axes[0].set_title('原始網(wǎng)格圖像（理想無畸變）')
axes[0].axis('off')
axes[1].imshow(img_distorted)
# axes[1].set_title('引入桶形畸變后的圖像', fontproperties=_zh_font_lg)
axes[1].set_title('引入桶形畸變后的圖像')
axes[1].axis('off')
axes[2].imshow(img_undistorted)
# axes[2].set_title('cv2.undistort 去畸變結(jié)果', fontproperties=_zh_font_lg)
axes[2].set_title('cv2.undistort 去畸變結(jié)果')
axes[2].axis('off')
# plt.suptitle('相機(jī)標(biāo)定 — 去畸變效果對比', fontproperties=_zh_font_xl)
plt.suptitle('相機(jī)標(biāo)定 — 去畸變效果對比')
plt.tight_layout()
plt.show()

# ── 步驟 5：重投影誤差評估 ────────────────────────────────────────────────
# 重投影誤差是衡量標(biāo)定質(zhì)量的核心指標(biāo)：
# 將已知3D點(diǎn)用標(biāo)定結(jié)果重新投影到圖像，計(jì)算與實(shí)際檢測點(diǎn)的距離誤差

errors = []
for i in range(len(objpoints)):
    # 將3D點(diǎn)重新投影到圖像平面
    projected, _ = cv2.projectPoints(
        objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    # 計(jì)算與實(shí)際檢測角點(diǎn)的歐氏距離
    actual = imgpoints[i].reshape(-1, 2).astype(np.float32)
    proj   = projected.reshape(-1, 2).astype(np.float32)
    err = np.sqrt(np.mean(np.sum((actual - proj)**2, axis=1)))
    errors.append(err)

fig, ax = plt.subplots(figsize=(9, 4))
bars = ax.bar(range(1, len(errors)+1), errors,
              color='steelblue', edgecolor='white', linewidth=0.8)
ax.axhline(np.mean(errors), color='tomato', lw=2, ls='--',
           label=f'平均誤差 = {np.mean(errors):.4f} px')
# ax.set_xlabel('圖像編號', fontproperties=_zh_font_md)
ax.set_xlabel('圖像編號')
# ax.set_ylabel('重投影誤差（像素）', fontproperties=_zh_font_md)
ax.set_ylabel('重投影誤差（像素）')
# ax.set_title('各圖像的重投影誤差分布', fontproperties=_zh_font_xl)
ax.set_title('各圖像的重投影誤差分布')
# ax.legend(prop=_zh_font_md)
ax.legend()
ax.set_xticks(range(1, len(errors)+1))
plt.tight_layout()
plt.show()

print(f"平均重投影誤差：{np.mean(errors):.4f} 像素")
print("（通常認(rèn)為誤差 < 1.0 像素為合格，< 0.5 像素為優(yōu)秀）")

# ── 比例因子法：生成演示場景 ──────────────────────────────────────────────
# 場景描述：
#   左側(cè)：參考物（已知寬度 = 2.0 cm，像素寬度 = 100 px）
#   右側(cè)：待測物（寬度未知，像素寬度約 230 px）

KNOWN_WIDTH_CM = 2.0  # 參考物的實(shí)際寬度（厘米）

# 構(gòu)建演示圖像
scene = np.ones((420, 720, 3), dtype=np.uint8) * 245

# 參考物（橙色邊框）：像素寬度 = 180-80 = 100 px
cv2.rectangle(scene, (80, 150), (180, 260), (30, 30, 30), -1)
cv2.rectangle(scene, (80, 150), (180, 260), (0, 120, 220), 3)
cv2.putText(scene, 'Reference Object', (60, 140),
            cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 120, 220), 1)
# 寬度標(biāo)注
cv2.arrowedLine(scene, (80, 285), (180, 285), (0, 120, 220), 2, tipLength=0.15)
cv2.arrowedLine(scene, (180, 285), (80, 285), (0, 120, 220), 2, tipLength=0.15)
cv2.putText(scene, '100 px = 2.0 cm', (55, 308),
            cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 120, 220), 1)

# 待測物（綠色邊框）：像素寬度 = 560-330 = 230 px
cv2.rectangle(scene, (330, 110), (560, 300), (30, 30, 30), -1)
cv2.rectangle(scene, (330, 110), (560, 300), (0, 170, 80), 3)
cv2.putText(scene, 'Unknown Object', (330, 100),
            cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 170, 80), 1)
# 寬度標(biāo)注
cv2.arrowedLine(scene, (330, 325), (560, 325), (0, 170, 80), 2, tipLength=0.08)
cv2.arrowedLine(scene, (560, 325), (330, 325), (0, 170, 80), 2, tipLength=0.08)
cv2.putText(scene, '230 px = ? cm', (360, 348),
            cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 170, 80), 1)

# 公式說明
cv2.putText(scene, 'Scale Factor = 100 px / 2.0 cm = 50 px/cm',
            (40, 380), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (140, 0, 160), 2)
cv2.putText(scene, 'Unknown Width = 230 px / 50 px/cm = 4.6 cm',
            (40, 410), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (140, 0, 160), 2)

fig, ax = plt.subplots(figsize=(11, 6))
ax.imshow(cv2.cvtColor(scene, cv2.COLOR_BGR2RGB))
# ax.set_title('比例因子計(jì)算方法 — 尺寸測量示意圖', fontproperties=_zh_font_xl)
ax.set_title('比例因子計(jì)算方法 — 尺寸測量示意圖')
ax.axis('off')
plt.tight_layout()
plt.show()

# ── 比例因子法：完整代碼實(shí)現(xiàn) ──────────────────────────────────────────────
# 本示例演示如何利用輪廓檢測自動計(jì)算比例因子并測量物體尺寸

def order_points(pts):
    """將矩形的四個(gè)頂點(diǎn)按 [左上, 右上, 右下, 左下] 排序。"""
    rect = np.zeros((4, 2), dtype='float32')
    s = pts.sum(axis=1)
    rect[0] = pts[np.argmin(s)]   # 左上
    rect[2] = pts[np.argmax(s)]   # 右下
    diff = np.diff(pts, axis=1)
    rect[1] = pts[np.argmin(diff)]  # 右上
    rect[3] = pts[np.argmax(diff)]  # 左下
    return rect

def midpoint(ptA, ptB):
    return ((ptA[0] + ptB[0]) * 0.5, (ptA[1] + ptB[1]) * 0.5)

# 1. 圖像預(yù)處理
gray_scene = cv2.cvtColor(scene, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray_scene, (7, 7), 0)
edged = cv2.Canny(blurred, 30, 100)
edged = cv2.dilate(edged, None, iterations=1)
edged = cv2.erode(edged, None, iterations=1)

# 2. 輪廓檢測
cnts, _ = cv2.findContours(
    edged.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

# 過濾面積過小的噪聲輪廓，并按 x 坐標(biāo)從左到右排序
cnts = [c for c in cnts if cv2.contourArea(c) > 500]
cnts = sorted(cnts, key=lambda c: cv2.boundingRect(c)[0])

pixels_per_cm = None
result_img = scene.copy()
measurements = []

for c in cnts:
    # 計(jì)算最小外接矩形
    box = cv2.minAreaRect(c)
    box_pts = cv2.boxPoints(box)
    box_pts = np.intp(box_pts)
    ordered = order_points(box_pts.astype('float32'))

    tl, tr, br, bl = ordered
    # 計(jì)算矩形的寬和高（像素）
    width_px  = np.linalg.norm(tr - tl)
    height_px = np.linalg.norm(bl - tl)

    # 第一個(gè)（最左側(cè)）輪廓作為參考物，初始化比例因子
    if pixels_per_cm is None:
        pixels_per_cm = width_px / KNOWN_WIDTH_CM
        label = f'Ref: {KNOWN_WIDTH_CM:.1f}cm'
        color = (0, 120, 220)
    else:
        w_cm = width_px  / pixels_per_cm
        h_cm = height_px / pixels_per_cm
        label = f'{w_cm:.1f} x {h_cm:.1f} cm'
        color = (0, 170, 80)
        measurements.append((w_cm, h_cm))

    cv2.drawContours(result_img, [box_pts], -1, color, 2)
    cx, cy = int(np.mean(ordered[:, 0])), int(np.mean(ordered[:, 1]))
    cv2.putText(result_img, label, (cx - 50, cy),
                cv2.FONT_HERSHEY_SIMPLEX, 0.65, color, 2)

print(f"計(jì)算得到的比例因子：{pixels_per_cm:.2f} px/cm")
for i, (w, h) in enumerate(measurements):
    print(f"待測物 {i+1} 的實(shí)際尺寸：寬 = {w:.2f} cm，高 = {h:.2f} cm")

fig, ax = plt.subplots(figsize=(11, 6))
ax.imshow(cv2.cvtColor(result_img, cv2.COLOR_BGR2RGB))
# ax.set_title('比例因子法 — 自動輪廓檢測與尺寸測量結(jié)果', fontproperties=_zh_font_xl)
ax.set_title('比例因子法 — 自動輪廓檢測與尺寸測量結(jié)果')
ax.axis('off')
plt.tight_layout()
plt.show()

# ── 圖：兩種方法對比可視化 ────────────────────────────────────────────────
fig, ax = plt.subplots(figsize=(13, 5.5))
ax.axis('off')

headers = ['對比維度', '張正友標(biāo)定法', '比例因子計(jì)算方法']
rows = [
    ['標(biāo)定物', '二維棋盤格標(biāo)定板（已知格點(diǎn)尺寸）', '任意已知尺寸的參考物體'],
    ['輸出結(jié)果', '內(nèi)參矩陣 K、外參 [R|t]、畸變系數(shù) D', '像素/物理單位的換算比例'],
    ['精度', '高（可達(dá)亞像素級別）', '中等（受透視和畸變影響）'],
    ['適用場景', '三維重建、機(jī)器人定位、雙目視覺', '固定場景下的平面尺寸估算'],
    ['操作復(fù)雜度', '較高（需多角度拍攝 15~20 張）', '低（只需一張含參考物的圖像）'],
    ['關(guān)鍵 API', 'calibrateCamera, undistort', 'findContours, minAreaRect'],
]

col_widths = [0.22, 0.39, 0.39]
col_starts = [0.0, 0.22, 0.61]
row_height = 0.13
header_y = 0.88

# 繪制表頭
for j, (h, x, w) in enumerate(zip(headers, col_starts, col_widths)):
    rect = mpatches.FancyBboxPatch((x + 0.005, header_y), w - 0.01, row_height,
                                    transform=ax.transAxes,
                                    boxstyle='square,pad=0', clip_on=False,
                                    linewidth=0, facecolor='#2e5fa3')
    ax.add_patch(rect)
    # ax.text(x + w/2, header_y + row_height/2, h,
    #         transform=ax.transAxes, ha='center', va='center',
    #         fontproperties=_zh_font_md, color='white', fontweight='bold')
    ax.text(x + w/2, header_y + row_height/2, h,
            transform=ax.transAxes, ha='center', va='center',
            color='white', fontweight='bold')

# 繪制數(shù)據(jù)行
for i, row in enumerate(rows):
    y = header_y - (i + 1) * (row_height + 0.005)
    bg = '#f0f4ff' if i % 2 == 0 else '#ffffff'
    for j, (cell, x, w) in enumerate(zip(row, col_starts, col_widths)):
        rect = mpatches.FancyBboxPatch((x + 0.005, y), w - 0.01, row_height,
                                        transform=ax.transAxes,
                                        boxstyle='square,pad=0', clip_on=False,
                                        linewidth=0.5, edgecolor='#cccccc',
                                        facecolor=bg if j > 0 else '#e8f0fe')
        ax.add_patch(rect)
        # ax.text(x + w/2, y + row_height/2, cell,
        #         transform=ax.transAxes, ha='center', va='center',
        #         fontproperties=_zh_font_sm, color='#222222')
        ax.text(x + w/2, y + row_height/2, cell,
                transform=ax.transAxes, ha='center', va='center',
                color='#222222')

# ax.set_title('張正友標(biāo)定法 vs. 比例因子計(jì)算方法 — 綜合對比',
#              fontproperties=_zh_font_xl, pad=15)
ax.set_title('張正友標(biāo)定法 vs. 比例因子計(jì)算方法 — 綜合對比', pad=15)
plt.tight_layout()
plt.show()