# -*- 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)