video-location/calibrate.py

# -*- coding: utf-8 -*-
"""
带有RANSAC功能的摄像头自动标定工具 (v3.2 结构优化版)

功能:
- RANSAC算法自动剔除“坏点”。
- 新增 calculate_final_parameters 函数，使用所有“好点”进行最终的联合优化，计算出最精确结果。

v3.2 更新日志:
- 将最终优化步骤封装到独立的 calculate_final_parameters 函数中，使流程更清晰。
"""
import numpy as np
from scipy.optimize import minimize
from scipy.spatial.transform import Rotation as R
import random
import warnings

warnings.filterwarnings("ignore", category=RuntimeWarning)

## -------------------------------------------------------------------------- ##
## 1. 核心算法函数 (无变化)
## -------------------------------------------------------------------------- ##
EARTH_RADIUS = 6378137.0
def lonlat_to_enu(lon, lat, center_lon, center_lat):
    delta_lon = np.radians(lon - center_lon); delta_lat = np.radians(lat - center_lat)
    x = EARTH_RADIUS * delta_lon * np.cos(np.radians(center_lat)); y = EARTH_RADIUS * delta_lat
    return np.array([x, y, 0])
def enu_to_lonlat(enu_point, center_lon, center_lat):
    x, y, _ = enu_point; delta_lat_rad = y / EARTH_RADIUS
    delta_lon_rad = x / (EARTH_RADIUS * np.cos(np.radians(center_lat)))
    lon = center_lon + np.degrees(delta_lon_rad); lat = center_lat + np.degrees(delta_lat_rad)
    return lon, lat
def project_pixel_to_lonlat(u, v, resolution_w, resolution_h, cam_lon, cam_lat,
                            cam_height, p, t, hfov, north_p_value=0.0, **kwargs):
    f_x = resolution_w / (2 * np.tan(np.radians(hfov) / 2)); f_y = f_x
    cx = resolution_w / 2; cy = resolution_h / 2
    vec_cam = np.array([(u - cx) / f_x, (v - cy) / f_y, 1.0]); vec_cam /= np.linalg.norm(vec_cam)
    r_base = R.from_euler('x', -90, degrees=True); p_corrected = p - north_p_value
    r_pan = R.from_euler('z', p_corrected, degrees=True); r_tilt = R.from_euler('x', -t, degrees=True)
    total_rotation = r_pan * r_tilt * r_base; vec_enu = total_rotation.apply(vec_cam)
    if vec_enu[2] >= -1e-9: return None, None
    cam_pos_enu = np.array([0, 0, cam_height]); s = -cam_height / vec_enu[2]
    intersection_enu = cam_pos_enu + s * vec_enu
    return enu_to_lonlat(intersection_enu, cam_lon, cam_lat)

## -------------------------------------------------------------------------- ##
## 2. RANSAC 及最终计算逻辑
## -------------------------------------------------------------------------- ##

def solve_model_for_subset(subset_points, camera_params, initial_guesses):
    """(RANSAC内部使用) 使用一个小的点集来快速计算一个模型参数的估计值"""
    static_args = camera_params.copy()
    initial_values = [initial_guesses['height'], initial_guesses['hfov']]
    def objective(params):
        total_error = 0.0
        for point in subset_points:
            pred_lon, pred_lat = project_pixel_to_lonlat(point[0], point[1], cam_height=params[0], hfov=params[1], **static_args)
            if pred_lon is None: return 1e12
            true_enu = lonlat_to_enu(point[2], point[3], camera_params['cam_lon'], camera_params['cam_lat']); pred_enu = lonlat_to_enu(pred_lon, pred_lat, camera_params['cam_lon'], camera_params['cam_lat'])
            total_error += np.sum((pred_enu - true_enu)**2)
        return total_error
    result = minimize(objective, initial_values, method='Nelder-Mead', options={'xatol': 1e-2, 'fatol': 1e-2})
    if result.success: return result.x[0], result.x[1]
    return None, None

def calculate_final_parameters(inliers, camera_params, initial_guesses):
    """
    [新增方法] 使用所有“好点”(inliers)进行最终的联合优化，计算最精确的平均高度和HFOV。
    """
    print("\n--- 使用所有内点进行最终优化 ---")
    static_args = camera_params.copy()
    initial_values = [initial_guesses['height'], initial_guesses['hfov']]

    def objective(params):
        total_error = 0.0
        for point in inliers:
            pred_lon, pred_lat = project_pixel_to_lonlat(point[0], point[1], cam_height=params[0], hfov=params[1], **static_args)
            if pred_lon is None: return 1e12
            true_enu = lonlat_to_enu(point[2], point[3], camera_params['cam_lon'], camera_params['cam_lat']); pred_enu = lonlat_to_enu(pred_lon, pred_lat, camera_params['cam_lon'], camera_params['cam_lat'])
            error = np.sum((pred_enu - true_enu)**2); total_error += error
        return total_error

    result = minimize(objective, initial_values, method='Nelder-Mead', options={'disp': True, 'xatol': 1e-6, 'fatol': 1e-6})

    if result.success and result.fun < 1e11:
        return result.x[0], result.x[1], result.fun
    else:
        return (result.x[0], result.x[1], result.fun)

def run_ransac_calibration(all_points, camera_params, initial_guesses,
                           ransac_iterations=100, subset_size=3, error_threshold=3.0):
    """执行RANSAC算法来寻找最佳的内点集。"""
    print("--- 开始执行RANSAC自动标定 ---")
    print(f"总点数: {len(all_points)}, RANSAC迭代次数: {ransac_iterations}, 误差阈值: {error_threshold}米")

    best_inliers, best_model_params = [], (0, 0)

    for i in range(ransac_iterations):
        if len(all_points) < subset_size: print("错误：总点数小于子集大小。"); return None, None, []

        subset = random.sample(all_points, subset_size)
        height_hyp, hfov_hyp = solve_model_for_subset(subset, camera_params, initial_guesses)
        if height_hyp is None: continue

        current_inliers = []
        for point in all_points:
            pred_lon, pred_lat = project_pixel_to_lonlat(point[0], point[1], cam_height=height_hyp, hfov=hfov_hyp, **camera_params)
            if pred_lon is not None:
                true_enu = lonlat_to_enu(point[2], point[3], camera_params['cam_lon'], camera_params['cam_lat']); pred_enu = lonlat_to_enu(pred_lon, pred_lat, camera_params['cam_lon'], camera_params['cam_lat'])
                error = np.linalg.norm(true_enu - pred_enu)
                if error < error_threshold: current_inliers.append(point)

        if len(current_inliers) > len(best_inliers):
            best_inliers = current_inliers
            best_model_params = (height_hyp, hfov_hyp)
            print(f"  迭代 {i+1}/{ransac_iterations}: 发现更好的模型，内点数量 = {len(best_inliers)}")

    if not best_inliers:
        print("\nRANSAC失败：未找到一致的点集。"); return None, None, []

    print(f"\nRANSAC完成：找到的最佳模型有 {len(best_inliers)} 个内点。")

    # [结构调整] 调用新增的函数进行最终计算
    final_height, final_hfov, _ = calculate_final_parameters(best_inliers, camera_params,
                                                             {"height": best_model_params[0], "hfov": best_model_params[1]})

    return final_height, final_hfov, best_inliers

## -------------------------------------------------------------------------- ##
## 3. 数据输入与执行
## -------------------------------------------------------------------------- ##
if __name__ == '__main__':
    all_calibration_data = [
        (548, 196, 112.894553, 28.221280), (91, 276, 112.894612, 28.221594), (216, 189, 112.894757, 28.221469),
        (850, 250, 112.89487, 28.22086), (450, 450, 112.89389, 28.22137), (1100, 500, 112.89468, 28.22050),
        (610, 415, 112.8955, 28.2220), (362, 527, 112.8930, 28.2210),
    ]
    camera_params = {
        "resolution_w": 1280, "resolution_h": 720, "cam_lon": 112.893799, "cam_lat": 28.221701,
        "north_p_value": 39.0, "p": 271.9, "t": 26.2
    }
    initial_guesses = {"height": 0, "hfov": 0}

    final_height, final_hfov, inliers = run_ransac_calibration(
        all_points=all_calibration_data, camera_params=camera_params, initial_guesses=initial_guesses,
        ransac_iterations=100, subset_size=3, error_threshold=3.0)

    if final_height is not None:
        print("\n" + "#"*50); print("#" + " "*12 + "RANSAC 自动标定成功完成" + " "*13 + "#"); print("#"*50)
        print(f"# >>> 最终标定结果 (Final Calibrated Results):")
        print(f"#   - 摄像头高度 (Height): {final_height:.4f} 米")
        print(f"#   - 水平视场角 (HFOV): {final_hfov:.4f} 度")
        print("#" + "-"*48 + "#")
        print(f"# 共找到 {len(inliers)} 个一致的“好点”(Inliers):")
        inlier_pixels = {(p[0], p[1]) for p in inliers}
        for point in all_calibration_data:
            if (point[0], point[1]) in inlier_pixels: print(f"#   - 像素点 ({point[0]}, {point[1]},{point[2]}, {point[3]}) [内点]")
        outliers = [p for p in all_calibration_data if (p[0], p[1]) not in inlier_pixels]
        if outliers:
            print(f"# 共剔除 {len(outliers)} 个“坏点”(Outliers):")
            for point in outliers: print(f"#   - 像素点 ({point[0]}, {point[1]},{point[2]}, {point[3]}) [外点]")
        print("#"*50)