|
@@ -0,0 +1,162 @@
|
|
|
|
|
+# -*- 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)
|