import os import numpy as np import cv2 import itertools from scipy.optimize import least_squares from .reconstruction import get_camera_parameters from .pose_estimation import get_ground_truth_seams, estimate_camera_pose from .final_reconstruction import merge_seams # 之前的merge_seams函数依然可用 def get_initial_parameters_with_solvepnp(all_2d_endpoints, ground_truth): """ 【新】使用 solvePnP 为全局优化提供一个更好的初始位姿。 """ print("\n--- Step 1: Getting a good initial guess for poses using solvePnP ---") # 1. 内参和3D点 (与之前相同) cam_params_L, cam_params_R, _ = get_camera_parameters() camera_intrinsics = np.array([ cam_params_L['fc'][0], cam_params_L['fc'][1], cam_params_L['cc'][0], cam_params_L['cc'][1], cam_params_R['fc'][0], cam_params_R['fc'][1], cam_params_R['cc'][0], cam_params_R['cc'][1] ]) points_3d_init = np.array([ ground_truth['up_line1']['start_3d'], ground_truth['up_line1']['end_3d'], ground_truth['up_line2']['start_3d'], ground_truth['up_line2']['end_3d'], ground_truth['bottom_line1']['start_3d'], ground_truth['bottom_line1']['end_3d'], ]) # 2. 【关键】为每个相机独立计算初始位姿 camera_poses_init = np.zeros((4, 6)) camera_map = {0: ('up', 'L'), 1: ('up', 'R'), 2: ('bottom', 'L'), 3: ('bottom', 'R')} for i in range(4): part_type, side = camera_map[i] obj_pts, img_pts = [], [] # 收集该相机能看到的所有点 for line_name in ['line1', 'line2']: if f"{part_type}_{line_name}" in ground_truth: gt_key = f"{part_type}_{line_name}" img_key = f"{part_type}_{side.lower()}_{line_name}" # 确定3D点在 points_3d_init 中的索引 if gt_key == 'up_line1': p_indices = [0, 1] elif gt_key == 'up_line2': p_indices = [2, 3] elif gt_key == 'bottom_line1': p_indices = [4, 5] elif gt_key == 'bottom_line2': p_indices = [2, 3] # bottom_line2也对应第2,3个点 obj_pts.extend([points_3d_init[p_indices[0]], points_3d_init[p_indices[1]]]) img_pts.extend([all_2d_endpoints[img_key]['start'], all_2d_endpoints[img_key]['end']]) # 使用我们之前验证过的点对应寻找逻辑 best_err = float('inf') best_pose_for_cam = None for a, b in itertools.product([0, 1], repeat=2): # 假设最多两条线 current_img_pts = list(img_pts) if a: current_img_pts[0], current_img_pts[1] = current_img_pts[1], current_img_pts[0] if b: current_img_pts[2], current_img_pts[3] = current_img_pts[3], current_img_pts[2] rvec, tvec = estimate_camera_pose(current_img_pts, obj_pts, side) if rvec is not None: cam = cam_params_L if side == 'L' else cam_params_R proj_pts, _ = cv2.projectPoints(np.array(obj_pts), rvec, tvec, cam['K'], cam['kc']) err = cv2.norm(np.array(current_img_pts, dtype=np.float32), proj_pts.reshape(-1, 2).astype(np.float32)) if err < best_err: best_err = err best_pose_for_cam = np.concatenate([rvec.flatten(), tvec.flatten()]) if best_pose_for_cam is not None: camera_poses_init[i] = best_pose_for_cam print(f"Initial pose for camera {i} ({part_type}-{side}) found with error {best_err:.2f}") else: print(f"Warning: Failed to find initial pose for camera {i}") # 3. 准备2D观测点 (与之前相同) obs_2d, p_indices, c_indices = [], [], [] # ... (这部分代码与上一版 get_initial_parameters 完全相同，直接复制) point_map = {'up_line1': [0, 1], 'up_line2': [2, 3], 'bottom_line1': [4, 5], 'bottom_line2': [2, 3]} for cam_idx, (part, side) in camera_map.items(): for line in ['line1', 'line2']: img_key = f"{part}_{side.lower()}_{line}" gt_key = f"{part}_{line}" if img_key in all_2d_endpoints: obs_2d.extend([all_2d_endpoints[img_key]['start'], all_2d_endpoints[img_key]['end']]) p_indices.extend(point_map[gt_key]) c_indices.extend([cam_idx, cam_idx]) return camera_intrinsics, camera_poses_init, points_3d_init, np.array(obs_2d), np.array(p_indices), np.array( c_indices) def cost_function(params, n_cameras, n_points, camera_indices, point_indices, points_2d, fixed_kcs, fixed_3d_points_init): """BA的代价函数（V2 - 带有固定参数）。""" # 1. 从一维参数向量中解析出需要优化的参数 intrinsics_flat = params[:8] camera_poses_flat = params[8: 8 + n_cameras * 6] # 【关键修改】三维点不再全部从params里取 # 我们只优化除了第一个点之外的所有点 points_3d_optimizable_flat = params[8 + n_cameras * 6:] camera_poses = camera_poses_flat.reshape((n_cameras, 6)) # 重新构建完整的三维点列表 points_3d = np.zeros((n_points, 3)) points_3d[0] = fixed_3d_points_init[0] # 第一个点是固定的！ points_3d[1:] = points_3d_optimizable_flat.reshape((n_points - 1, 3)) # ... 函数的其余部分（计算残差）完全不变 ... residuals = [] for i in range(len(points_2d)): cam_idx = camera_indices[i] point_idx = point_indices[i] pose = camera_poses[cam_idx] point_3d = points_3d[point_idx] if cam_idx in [0, 2]: # Left cameras fx, fy, cx, cy = intrinsics_flat[:4] kc = fixed_kcs[0] else: # Right cameras fx, fy, cx, cy = intrinsics_flat[4:] kc = fixed_kcs[1] K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) reproj_pt, _ = cv2.projectPoints(point_3d, pose[:3], pose[3:], K, kc) residuals.extend((reproj_pt.ravel() - points_2d[i]).tolist()) return np.array(residuals) def run_global_optimization(all_2d_endpoints, ground_truth): """执行全局优化（V2 - 修正尺度不确定性）。""" # 1. 获取初始值 (不变) intrinsics_init, poses_init, points_3d_init, obs_2d, p_indices, c_indices = get_initial_parameters_with_solvepnp( all_2d_endpoints, ground_truth) # 2. 【关键修改】将参数分为固定部分和优化部分 # 我们要优化的三维点是除了第一个之外的所有点 optimizable_points_3d_init = points_3d_init[1:] # 打包所有需要优化的参数 params_init = np.concatenate([ intrinsics_init.ravel(), poses_init.ravel(), optimizable_points_3d_init.ravel() # 只打包需要优化的点 ]) # 3. 准备固定参数 fixed_kcs = [get_camera_parameters()[0]['kc'], get_camera_parameters()[1]['kc']] # 4. 执行优化 (args 增加了 fixed_3d_points_init) n_cameras = 4 n_points = points_3d_init.shape[0] print("\n--- Step 2: Running Global Bundle Adjustment (with scale constraint) ---") result = least_squares( cost_function, params_init, verbose=2, x_scale='jac', ftol=1e-6, method='trf', args=(n_cameras, n_points, c_indices, p_indices, obs_2d, fixed_kcs, points_3d_init), # 传入固定的初始3D点 max_nfev=2000 # 可以适当增加迭代次数 ) params_final = result.x n_cameras = 4 # 确保 n_cameras 和 n_points 在这里可用 n_points = points_3d_init.shape[0] # --- 以下是新增的解析和保存部分 --- # 5a. 解析所有最终参数 intrinsics_final_flat = params_final[:8] camera_poses_final_flat = params_final[8: 8 + n_cameras * 6] optimizable_points_3d_final_flat = params_final[8 + n_cameras * 6:] intrinsics_final = intrinsics_final_flat.reshape(2, 4) camera_poses_final = camera_poses_final_flat.reshape((n_cameras, 6)) points_3d_final = np.zeros((n_points, 3)) points_3d_final[0] = points_3d_init[0] points_3d_final[1:] = optimizable_points_3d_final_flat.reshape((n_points - 1, 3)) # 5b. 打印到控制台，以便即时查看 print("\n--- Optimized Camera Intrinsics ---") print(f"Left Cam (fx, fy, cx, cy): {intrinsics_final[0]}") print(f"Right Cam (fx, fy, cx, cy): {intrinsics_final[1]}") print("\n--- Optimized Camera Poses (Rodrigues vector + translation) ---") print(f"Pose of up-left cam: {camera_poses_final[0]}") print(f"Pose of up-right cam: {camera_poses_final[1]}") print(f"Pose of bottom-left cam: {camera_poses_final[2]}") print(f"Pose of bottom-right cam: {camera_poses_final[3]}") # 5c. 【核心】将所有参数保存到文件 # 定义保存路径 current_dir = os.path.dirname(os.path.abspath(__file__)) project_root = os.path.dirname(current_dir) save_path = os.path.join(project_root, 'optimized_camera_parameters.npz') # 获取固定的畸变系数 cam_L_params, cam_R_params, _ = get_camera_parameters() np.savez( save_path, # 优化后的内参 optimized_intrinsics_L=intrinsics_final[0], # [fx, fy, cx, cy] optimized_intrinsics_R=intrinsics_final[1], # 固定的畸变系数 (BA中未优化) dist_coeffs_L=cam_L_params['kc'], dist_coeffs_R=cam_R_params['kc'], # 优化后的相机位姿 (相对于物体坐标系) pose_up_L=camera_poses_final[0], # [rvec, tvec] pose_up_R=camera_poses_final[1], pose_bottom_L=camera_poses_final[2], pose_bottom_R=camera_poses_final[3], # 还可以保存一个计算出的新外参作为参考 # (up-right 相对于 up-left 的变换) new_extrinsics=calculate_new_extrinsics(camera_poses_final[0], camera_poses_final[1]), # 优化后的三维点坐标 optimized_3d_points=points_3d_final ) print(f"\n✅ All optimized parameters have been saved to: {save_path}") # 6. 整理输出 (这部分不变) final_seams = { 'up_line1': {'start_3d': points_3d_final[0], 'end_3d': points_3d_final[1]}, 'up_line2': {'start_3d': points_3d_final[2], 'end_3d': points_3d_final[3]}, 'bottom_line1': {'start_3d': points_3d_final[4], 'end_3d': points_3d_final[5]}, 'bottom_line2': {'start_3d': points_3d_final[2], 'end_3d': points_3d_final[3]} } return final_seams def calculate_new_extrinsics(pose_L, pose_R): """根据两个相机相对于物体的位姿，计算它们之间的相对位姿（外参）。""" # 从物体到左相机的变换 rvec_L, tvec_L = pose_L[:3], pose_L[3:] R_L_from_obj, _ = cv2.Rodrigues(rvec_L) T_L_from_obj = tvec_L.reshape(3, 1) # 从物体到右相机的变换 rvec_R, tvec_R = pose_R[:3], pose_R[3:] R_R_from_obj, _ = cv2.Rodrigues(rvec_R) T_R_from_obj = tvec_R.reshape(3, 1) # 计算从左相机到右相机的变换 # T_R_from_L = R_R @ inv(R_L).T @ (T_L - T_R) 是一种错误的推导 # 正确推导: P_obj = inv(R_L)@(P_camL - T_L) = inv(R_R)@(P_camR - T_R) # => P_camR = R_R @ inv(R_L) @ P_camL + (T_R - R_R @ inv(R_L) @ T_L) # => R_R_from_L = R_R @ R_L.T # => T_R_from_L = T_R - R_R_from_L @ T_L R_R_from_L = R_R_from_obj @ R_L_from_obj.T t_R_from_L = T_R_from_obj - (R_R_from_L @ T_L_from_obj) return {'R': R_R_from_L, 't': t_R_from_L}