fix(修复两个bug)

calibrate.py

@@ -32,16 +32,16 @@ def enu_to_lonlat(enu_point, center_lon, center_lat):
     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
+    f_x = float(resolution_w) / (2 * np.tan(np.radians(float(hfov)) / 2)); f_y = f_x
+    cx = float(resolution_w) / 2; cy = float(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_base = R.from_euler('x', -90, degrees=True); p_corrected = float(p) - float(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)
+    return enu_to_lonlat(intersection_enu, float(cam_lon), float(cam_lat))
 
 ## -------------------------------------------------------------------------- ##
 ## 2. RANSAC 及最终计算逻辑
@@ -54,9 +54,9 @@ def solve_model_for_subset(subset_points, camera_params, initial_guesses):
     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)
+            pred_lon, pred_lat = project_pixel_to_lonlat(int(point[0]), int(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'])
+            true_enu = lonlat_to_enu(float(point[2]), float(point[3]), float(camera_params['cam_lon']), float(camera_params['cam_lat'])); pred_enu = lonlat_to_enu(pred_lon, pred_lat, float(camera_params['cam_lon']), float(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})
@@ -74,9 +74,9 @@ def calculate_final_parameters(inliers, camera_params, initial_guesses):
     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)
+            pred_lon, pred_lat = project_pixel_to_lonlat(float(point[0]), float(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'])
+            true_enu = lonlat_to_enu(float(point[2]), float(point[3]), float(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
 
@@ -94,19 +94,17 @@ def run_ransac_calibration(all_points, camera_params, initial_guesses,
     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)
+            pred_lon, pred_lat = project_pixel_to_lonlat(int(point[0]), int(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'])
+                true_enu = lonlat_to_enu(float(point[2]), float(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)
 
@@ -130,20 +128,25 @@ def run_ransac_calibration(all_points, camera_params, initial_guesses,
 ## 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),
+        (1161,726,112.89460963008204,28.221291037581736),
+        (640,501,112.89519149486185,28.221227074133417),
+        (613,792, 112.89466360382977,28.22146804322909),
+        (1266,1286, 112.89424869288924,28.22149200894583),
     ]
+
+# Convert the 'points' array into the desired list of tuples format
+
     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
+        "resolution_w": 2688, "resolution_h": 1520, "cam_lon": 112.893799, "cam_lat": 28.221701,
+        "north_p_value": 39.0, "p": 271.6, "t": 18.3
     }
-    initial_guesses = {"height": 0, "hfov": 0}
+    initial_guesses = {"height": 30, "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)
+        ransac_iterations=200, subset_size=3, error_threshold=5.0)
 
     if final_height is not None:
         print("\n" + "#"*50); print("#" + " "*12 + "RANSAC 自动标定成功完成" + " "*13 + "#"); print("#"*50)