python opencv计算F矩阵分解RT

 

 

 

 

import cv2
import numpy as np
from scipy.spatial.transform import Rotation as R

def 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 = 50
    X_plane = np.random.rand(n_points, 2) * 4 - 2
    X_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).T
    pts2_homo = (P2 @ X_homo.T).T
    
    pts1 = pts1_homo[:, :2] / pts1_homo[:, 2:3]
    pts2 = pts2_homo[:, :2] / pts2_homo[:, 2:3]
    
    # 添加适度的噪声
    noise_scale = 0.5
    pts1 += np.random.randn(*pts1.shape) * noise_scale
    pts2 += 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 = -1
    best_score = -1
    
    for 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 = score
            best_solution = i
    
    print(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_scale
        print(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_scale

def 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 + t
    depths2 = 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).T
    pts2_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.2
    
    return total_score

def 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_inv
    return 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 > 0
    points_3d_valid = points_3d[:, valid_mask]
    
    if len(points_3d_valid[0]) < 5:
        return 0.0
    
    # 估计平面距离
    # 对于平面上的点，满足 n·X = d
    n = n.flatten()
    estimated_d = np.median(np.dot(n, points_3d_valid))
    
    # 尺度因子是 estimated_d / d_prior
    if abs(d_prior) > 1e-6:
        scale = estimated_d / d_prior
    else:
        scale = 1.0
    
    return scale

def 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 = 50
    X_plane = np.random.rand(n_points, 2) * 4 - 2
    X_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).T
    pts2_homo = (P2 @ X_homo.T).T
    
    pts1 = pts1_homo[:, :2] / pts1_homo[:, 2:3]
    pts2 = pts2_homo[:, :2] / pts2_homo[:, 2:3]
    
    # 添加噪声
    pts1 += np.random.randn(*pts1.shape) * 0.5
    pts2 += 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 = 0
    best_score = -1
    for 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 = score
            best_idx = i
    
    R_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_est

def 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()