使用单应性矩阵标定相机俯仰角

import cv2 as cv
import numpy as np
import json
import argparse
import os
import glob


def find_lidar_blobs(input_img, show=False):
    params = cv.SimpleBlobDetector_Params()
    params.minThreshold = 0
    params.blobColor = 0
    params.filterByArea = True
    params.minArea = 1000
    params.maxArea = 12000
    params.filterByCircularity = True
    params.minCircularity = 0.01
    params.filterByConvexity = True
    params.minConvexity = 0.3
    params.filterByInertia = True
    params.minInertiaRatio = 0.05

    detector = cv.SimpleBlobDetector_create(params)
    keypoints = detector.detect(input_img)
    img_with_keypoints = cv.drawKeypoints(
        input_img, keypoints, np.array([]), (0,255,0),
        cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS
    )
    if show:
        cv.namedWindow("Keypoints", cv.WINDOW_NORMAL)
        cv.imshow('Keypoints', img_with_keypoints)
        cv.waitKey(0)
        cv.destroyAllWindows()

    if not keypoints:
        print("未检测到任何圆！")
        return []

    pts = np.array([kp.pt for kp in keypoints])  
    sorted_pts = pts[pts[:,1].argsort()]

    rows = []
    current_row = [sorted_pts[0]]
    for p in sorted_pts[1:]:
        if abs(p[1] - current_row[-1][1]) < 30:
            current_row.append(p)
        else:
            rows.append(current_row)
            current_row = [p]
    rows.append(current_row)

    rows = rows[:3]
    rows = [sorted(row, key=lambda x: x[0]) for row in rows]

    if show:
        img_with_keypoints = cv.drawKeypoints(
            input_img, keypoints, np.array([]), (0,255,0),
            cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS
        )
        for i, row in enumerate(rows):
            for j, (x,y) in enumerate(row):
                cv.putText(img_with_keypoints, f"{i}-{j}", (int(x),int(y)),
                           cv.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1)
        
        cv.namedWindow("Keypoints", cv.WINDOW_NORMAL)
        cv.imshow("Keypoints", img_with_keypoints)
        cv.waitKey(0)
        cv.destroyAllWindows()

    return rows


def process_one_image(img_path, json_path, show=False):
    img = cv.imread(img_path, cv.IMREAD_GRAYSCALE)
    if img is None:
        print(f"图像读取失败: {img_path}")
        return
    
    with open(json_path, "r") as f:
        params = json.load(f)

    inner = params["ir_inner"]
    ir_outer = params["ir_outer"]
    pitch = ir_outer["pitch"]
    print(f"原始pitch: {pitch}°")
    camera_matrix = np.array([
        [inner["Camera.fx"], 0, inner["Camera.cx"]],
        [0, inner["Camera.fy"], inner["Camera.cy"]],
        [0, 0, 1]
    ], dtype=np.float32)
    dist_coeffs = np.array([
        inner["Camera.k1"],
        inner["Camera.k2"],
        inner["Camera.p1"],
        inner["Camera.p2"],
        inner["Camera.k3"]
    ], dtype=np.float32)

    rows = find_lidar_blobs(img, show=show)
    if not rows:
        return

    img_points = []
    for i, row in enumerate(rows):
        for j, (x,y) in enumerate(row):
            img_points.append([x, y])

    world_points = [
        [497, 169.4, 0],
        [557, 169.4, 0],
        [617, 169.4, 0],
        [677, 169.4, 0],
        [737, 169.4, 0],
        [557, 109.4, 0],
        [617, 109.4, 0],
        [677, 109.4, 0]
    ]
    
    img_points_np = np.array(img_points, dtype=np.float32)
    cv.undistortPoints(img_points_np, camera_matrix, dist_coeffs, P=camera_matrix, dst=img_points_np)
    world_points_np = np.array(world_points, dtype=np.float32)
    H, mask = cv.findHomography(world_points_np, img_points_np, method=cv.RANSAC)
    if H is None:
        print(f"findHomography 失败: {img_path}")
        return

    H_norm = np.linalg.inv(camera_matrix) @ H

    r1 = H_norm[:, 0]
    r2 = H_norm[:, 1]
    t  = H_norm[:, 2]

    r1 /= np.linalg.norm(r1)
    r2 /= np.linalg.norm(r2)
    r3 = np.cross(r1, r2)

    R = np.column_stack((r1, r2, r3))
    U, _, Vt = np.linalg.svd(R)
    R = U @ Vt

    pitch_rad = np.arctan2(-R[2,1], R[2,2])
    yaw_rad   = np.arctan2(R[2,0], np.sqrt(R[2,1]**2 + R[2,2]**2))
    roll_rad  = np.arctan2(-R[1,0], R[0,0])

    # print(f"\n处理文件: {img_path}")
    # print("旋转矩阵 R:\n", R)
    # print("平移向量 t (未定尺度):\n", t)
    print("修正后俯仰角 pitch: {:.3f}° \n".format(abs(np.degrees(pitch_rad)) - 90))
    # print("偏航角 yaw:   {:.3f}°".format(np.degrees(yaw_rad)))
    # print("滚转角 roll:  {:.3f}°".format(np.degrees(roll_rad)))


def main(root_dir, show=True):
    # 遍历根目录下所有子目录
    for subdir in os.listdir(root_dir):
        subdir_path = os.path.join(root_dir, subdir, "IR")
        if not os.path.isdir(subdir_path):
            continue
        
        # 找 IR 目录下的 json 文件
        json_files = glob.glob(os.path.join(subdir_path, "*.json"))
        if not json_files:
            print(f"⚠️ {subdir_path} 下没有找到 json 文件，跳过。")
            continue
        json_path = json_files[0]

        # 找 IR 目录下的 png 图片
        img_files = glob.glob(os.path.join(subdir_path, "*.png"))
        if not img_files:
            print(f"⚠️ {subdir_path} 下没有 png 图片，跳过。")
            continue

        print(f"\n==== 处理目录 {subdir_path} ====")
        for img_path in img_files:
            process_one_image(img_path, json_path, show=show)


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="批量遍历子文件夹执行外参标定")
    parser.add_argument("--root", help="根目录路径，例如")
    parser.add_argument("--show", action="store_true", help="是否显示检测到的关键点")
    args = parser.parse_args()

    main(args.root, show=args.show)