"""
AprilTag诊断工具

帮助诊断为什么AprilTag检测失败
"""

import cv2
import sys
from pathlib import Path
import dt_apriltags as apriltag

# 测试图片路径
image_path = '../data/raw_images/IMG_20260123_140305.jpg'

print("="*60)
print("AprilTag诊断工具")
print("="*60)

# 读取图像
image = cv2.imread(image_path)
print(f"\n1. 图像加载: {image_path}")
print(f"   尺寸: {image.shape}")

# 转灰度
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

# 尝试所有常见的AprilTag家族
families_to_try = ['tag36h11', 'tag25h9', 'tag16h5', 'tagCircle21h7', 'tagStandard41h12']

print(f"\n2. 尝试检测所有常见AprilTag家族...")

for family in families_to_try:
    print(f"\n   [{family}]", end=' ')

    try:
        detector = apriltag.Detector(families=family)
        detections = detector.detect(gray)

        if len(detections) > 0:
            print(f"✓ 检测到 {len(detections)} 个Tag！")
            for det in detections:
                print(f"      - Tag ID: {det.tag_id}, 置信度: {det.decision_margin:.2f}")
                print(f"        中心: ({det.center[0]:.1f}, {det.center[1]:.1f})")
        else:
            print(f"✗ 未检测到")
    except Exception as e:
        print(f"✗ 错误: {e}")

print("\n" + "="*60)
print("诊断建议:")
print("="*60)
print("如果所有家族都检测不到，可能原因：")
print("  1. 图片中没有AprilTag")
print("  2. AprilTag被遮挡或模糊")
print("  3. AprilTag太小（在图像中<30像素）")
print("  4. 光照问题（过暗/过曝）")
print("\n请检查图片: " + image_path)
print("="*60)
