import cv2
import numpy as np
from scipy.spatial.transform import Rotation as Rdef improved_decompose_homography():"""改进的单应性矩阵分解，处理尺度问题"""print("="*60)print("改进的单应性分解 - 处理尺度问题")print("="*60)# 生成更合理的测试数据np.random.seed(42)# 相机内参K = np.array([[800, 0, 320],[0, 800, 240],[0, 0, 1]], dtype=np.float32)# 平面参数n_true = np.array([0, 0, 1]) # 平面法向量 (z=0平面)d_true = 5.0 # 相机到平面的距离# 生成平面上的3D点n_points = 50X_plane = np.random.rand(n_points, 2) * 4 - 2X_plane = np.hstack([X_plane, np.zeros((n_points, 1))]) # z=0# 相机1位姿 (在平面正上方)R1 = np.eye(3)t1 = np.array([0, 0, d_true]) # 相机在(0,0,d)处# 相机2位姿 (有小的旋转和平移)# 小的旋转angle = 0.2 # 约11.5度axis = np.array([0.1, 0.2, 1.0])axis = axis / np.linalg.norm(axis)R2_true = R.from_rotvec(axis * angle).as_matrix()# 小的平移 (在平面内移动)t2_true = np.array([0.5, 0.2, 0]) # 主要在X方向移动print("真实参数:")print(f"平面法向量 n: {n_true}")print(f"平面距离 d: {d_true}")print(f"相机2旋转 R:\n{R2_true}")print(f"相机2平移 t: {t2_true}")print(f"平移长度: {np.linalg.norm(t2_true):.4f}")# 计算真实的单应性矩阵# H = K (R - t n^T/d) K^{-1}n = n_true.reshape(3, 1)H_true = K @ (R2_true - t2_true.reshape(3, 1) @ n.T / d_true) @ np.linalg.inv(K)H_true = H_true / H_true[2, 2] # 归一化print(f"\n真实单应性矩阵 H:\n{H_true}")# 投影3D点到图像P1 = K @ np.hstack([R1, t1.reshape(3, 1)])P2 = K @ np.hstack([R2_true, (t1 + t2_true).reshape(3, 1)]) # 注意：t2是相对平移X_homo = np.hstack([X_plane, np.ones((n_points, 1))])pts1_homo = (P1 @ X_homo.T).Tpts2_homo = (P2 @ X_homo.T).Tpts1 = pts1_homo[:, :2] / pts1_homo[:, 2:3]pts2 = pts2_homo[:, :2] / pts2_homo[:, 2:3]# 添加适度的噪声noise_scale = 0.5pts1 += np.random.randn(*pts1.shape) * noise_scalepts2 += np.random.randn(*pts2.shape) * noise_scale# 从点估计单应性矩阵H_est, mask = cv2.findHomography(pts1, pts2, cv2.RANSAC, 3.0)mask = mask.flatten().astype(bool)pts1_inliers = pts1[mask]pts2_inliers = pts2[mask]print(f"\n估计的单应性矩阵 H:\n{H_est}")print(f"内点数量: {np.sum(mask)}/{n_points}")# 分解单应性矩阵print("\n" + "="*60)print("分解结果")print("="*60)num_solutions, rotations, translations, normals = cv2.decomposeHomographyMat(H_est, K, pts1_inliers, pts2_inliers)print(f"找到 {num_solutions} 个解")# 分析每个解best_solution = -1best_score = -1for i in range(num_solutions):R_i = rotations[i]t_i = translations[i]n_i = normals[i]# 计算分数score = evaluate_solution(R_i, t_i, n_i, pts1_inliers, pts2_inliers, K, d_true)# 转换为欧拉角便于理解euler = R.from_matrix(R_i).as_euler('xyz', degrees=True)print(f"\n解 {i+1}:")print(f" 欧拉角: [{euler[0]:.2f}, {euler[1]:.2f}, {euler[2]:.2f}]°")print(f" 平移向量: {t_i.flatten()}")print(f" 平移长度: {np.linalg.norm(t_i):.6f}")print(f" 法向量: {n_i.flatten()}")print(f" 评分: {score:.4f}")if score > best_score:best_score = scorebest_solution = iprint(f"\n最佳解: 解{best_solution+1}")# 提取最佳解R_best = rotations[best_solution]t_best = translations[best_solution]n_best = normals[best_solution]# 评估误差print("\n" + "="*60)print("误差分析")print("="*60)# 旋转误差R_error = np.linalg.norm(R_best - R2_true)# 平移方向误差（注意尺度）t_best_norm = t_best / np.linalg.norm(t_best)t_true_norm = t2_true / np.linalg.norm(t2_true)t_dir_error = np.arccos(np.clip(np.dot(t_best_norm.flatten(), t_true_norm), -1.0, 1.0))# 法向量误差n_error = np.arccos(np.clip(np.dot(n_best.flatten(), n_true), -1.0, 1.0))print(f"旋转误差: {R_error:.6f}")print(f"平移方向误差: {t_dir_error:.6f} rad ({np.degrees(t_dir_error):.2f}°)")print(f"法向量误差: {n_error:.6f} rad ({np.degrees(n_error):.2f}°)")# 尺度估计print("\n" + "="*60)print("尺度估计")print("="*60)# 从单应性估计尺度estimated_scale = estimate_scale_from_homography(R_best, t_best, n_best, pts1_inliers, pts2_inliers, K, d_true)print(f"估计的尺度因子: {estimated_scale:.6f}")# 如果有真实尺度，可以缩放平移向量if estimated_scale > 0:t_scaled = t_best * estimated_scaleprint(f"缩放后的平移: {t_scaled.flatten()}")print(f"真实平移: {t2_true}")t_scaled_error = np.linalg.norm(t_scaled.flatten() - t2_true)print(f"缩放后平移误差: {t_scaled_error:.6f}")return R_best, t_best, n_best, estimated_scaledef evaluate_solution(R, t, n, pts1, pts2, K, d=1.0):"""评估解的合理性使用多个标准：1. 正深度点数量2. 重投影误差3. 平面法向量的合理性"""n_points = len(pts1)# 1. 正深度检查P1 = K @ np.hstack([np.eye(3), np.zeros((3, 1))])P2 = K @ np.hstack([R, t])points_4d = cv2.triangulatePoints(P1, P2, pts1.T, pts2.T)points_3d = points_4d[:3] / points_4d[3]depths1 = points_3d[2, :]points_3d_cam2 = R @ points_3d + tdepths2 = points_3d_cam2[2, :]positive_mask = (depths1 > 0) & (depths2 > 0)positive_score = np.sum(positive_mask) / n_points# 2. 重投影误差H_estimated = compute_homography_from_pose(R, t, K, n, d)pts1_homo = np.hstack([pts1, np.ones((n_points, 1))])pts2_proj_homo = (H_estimated @ pts1_homo.T).Tpts2_proj = pts2_proj_homo[:, :2] / pts2_proj_homo[:, 2:3]reproj_errors = np.linalg.norm(pts2 - pts2_proj, axis=1)reproj_score = 1.0 / (np.mean(reproj_errors) + 1e-6)# 3. 法向量合理性（假设平面大致朝前）# 相机前方是z轴正方向camera_forward = np.array([0, 0, 1])n_dot = np.abs(np.dot(n.flatten(), camera_forward))# 综合评分total_score = positive_score * 0.5 + reproj_score * 0.3 + n_dot * 0.2return total_scoredef compute_homography_from_pose(R, t, K, n, d=1.0):"""从位姿计算单应性矩阵"""K_inv = np.linalg.inv(K)H = K @ (R - t @ n.T / d) @ K_invreturn H / H[2, 2]def estimate_scale_from_homography(R, t, n, pts1, pts2, K, d_prior=None):"""从单应性估计尺度方法：通过三角测量恢复3D点，然后估计平面距离"""n_points = len(pts1)if d_prior is None:d_prior = 1.0 # 先验距离# 创建投影矩阵P1 = K @ np.hstack([np.eye(3), np.zeros((3, 1))])P2 = K @ np.hstack([R, t])# 三角测量points_4d = cv2.triangulatePoints(P1, P2, pts1.T, pts2.T)points_3d = points_4d[:3] / points_4d[3]# 过滤无效点depths = points_3d[2, :]valid_mask = depths > 0points_3d_valid = points_3d[:, valid_mask]if len(points_3d_valid[0]) < 5:return 0.0# 估计平面距离# 对于平面上的点，满足 n·X = dn = n.flatten()estimated_d = np.median(np.dot(n, points_3d_valid))# 尺度因子是 estimated_d / d_priorif abs(d_prior) > 1e-6:scale = estimated_d / d_priorelse:scale = 1.0return scaledef test_planar_motion_scenarios():"""测试不同的平面运动场景"""print("="*60)print("不同平面运动场景测试")print("="*60)scenarios = [{'name': '纯旋转','R': R.from_euler('xyz', [10, 5, 15], degrees=True).as_matrix(),'t': np.array([0, 0, 0])},{'name': '平面内平移','R': np.eye(3),'t': np.array([0.5, 0.2, 0])},{'name': '旋转+小平移','R': R.from_euler('xyz', [5, 3, 8], degrees=True).as_matrix(),'t': np.array([0.3, 0.1, 0])},{'name': '大旋转','R': R.from_euler('xyz', [30, 15, 20], degrees=True).as_matrix(),'t': np.array([0.2, 0.1, 0])}]for scenario in scenarios:print(f"\n场景: {scenario['name']}")print("-"*40)test_scenario(scenario['R'], scenario['t'])def test_scenario(R_true, t_true):"""测试特定场景"""# 相机内参K = np.array([[800, 0, 320],[0, 800, 240],[0, 0, 1]])# 平面参数n_true = np.array([0, 0, 1])d_true = 5.0# 生成平面点n_points = 50X_plane = np.random.rand(n_points, 2) * 4 - 2X_plane = np.hstack([X_plane, np.zeros((n_points, 1))])# 相机位姿R1 = np.eye(3)t1 = np.array([0, 0, d_true])# 投影P1 = K @ np.hstack([R1, t1.reshape(3, 1)])P2 = K @ np.hstack([R_true, (t1 + t_true).reshape(3, 1)])X_homo = np.hstack([X_plane, np.ones((n_points, 1))])pts1_homo = (P1 @ X_homo.T).Tpts2_homo = (P2 @ X_homo.T).Tpts1 = pts1_homo[:, :2] / pts1_homo[:, 2:3]pts2 = pts2_homo[:, :2] / pts2_homo[:, 2:3]# 添加噪声pts1 += np.random.randn(*pts1.shape) * 0.5pts2 += np.random.randn(*pts2.shape) * 0.5# 估计单应性H, mask = cv2.findHomography(pts1, pts2, cv2.RANSAC, 3.0)mask = mask.flatten().astype(bool)pts1_inliers = pts1[mask]pts2_inliers = pts2[mask]# 分解num_solutions, rotations, translations, normals = cv2.decomposeHomographyMat(H, K, pts1_inliers, pts2_inliers)# 选择最佳解best_idx = 0best_score = -1for i in range(num_solutions):score = evaluate_solution(rotations[i], translations[i], normals[i], pts1_inliers, pts2_inliers, K, d_true)if score > best_score:best_score = scorebest_idx = iR_est = rotations[best_idx]t_est = translations[best_idx]n_est = normals[best_idx]# 计算误差R_error = np.linalg.norm(R_est - R_true)t_est_norm = t_est / np.linalg.norm(t_est)t_true_norm = t_true / (np.linalg.norm(t_true) + 1e-6)t_dir_error = np.arccos(np.clip(np.dot(t_est_norm.flatten(), t_true_norm), -1.0, 1.0))n_error = np.arccos(np.clip(np.dot(n_est.flatten(), n_true), -1.0, 1.0))print(f" 旋转误差: {R_error:.6f}")print(f" 平移方向误差: {np.degrees(t_dir_error):.2f}°")print(f" 法向量误差: {np.degrees(n_error):.2f}°")return R_est, t_est, n_estdef practical_advice():"""实用建议"""print("\n" + "="*60)print("单应性分解实用建议")print("="*60)advice = ["1. 单应性分解只能恢复单位平移，没有尺度信息","2. 平移尺度需要从其他来源获取："," - 已知平面距离"," - 其他传感器（IMU、深度相机）"," - 多帧三角测量","3. 选择正确解的关键："," - 检查正深度点数量"," - 比较重投影误差"," - 验证平面法向量的合理性","4. 适用场景："," - 平面或近似平面场景"," - 相机纯旋转"," - 远距离场景（近似平面）","5. 局限性："," - 非平面场景不适用"," - 需要足够的特征点"," - 对噪声敏感","6. 实际应用："," - 增强现实中的平面跟踪"," - 文档扫描和校正"," - 图像拼接"]for line in advice:print(line)if __name__ == "__main__":print("改进的单应性矩阵分解分析")print("="*60)# 运行改进的分解R_best, t_best, n_best, scale = improved_decompose_homography()# 测试不同场景test_planar_motion_scenarios()# 提供实用建议practical_advice()
