koko/innovate_project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

三维重构终版

Browse Source
This commit is contained in:
koko
2025-11-02 21:36:35 +08:00
parent f91b09da9d
commit f39009b853
126 changed files with 2870 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
3D_construction/script/global_optimizer.py Normal file
View File

@@ -0,0 +1,265 @@
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}