python opencv计算E矩阵分解RT

 

 

 

 

import cv2
import numpy as np

# 示例：从特征点匹配恢复相对位姿
def estimate_relative_pose_from_matches(keypoints1, keypoints2, matches, K):
    """
    从特征点匹配估计相对位姿
    
    参数:
        keypoints1: 第一幅图像的关键点列表
        keypoints2: 第二幅图像的关键点列表
        matches: 匹配对列表
        K: 相机内参矩阵
    """
    # 1. 提取匹配点的坐标
    pts1 = np.float32([keypoints1[m.queryIdx].pt for m in matches])
    pts2 = np.float32([keypoints2[m.trainIdx].pt for m in matches])
    
    # 2. 计算本质矩阵 (使用RANSAC)
    E, mask_E = cv2.findEssentialMat(pts1, pts2, K, 
                                     method=cv2.RANSAC, 
                                     prob=0.999, 
                                     threshold=1.0)
    
    # 3. 从本质矩阵恢复位姿
    num_inliers, R, t, mask_pose = cv2.recoverPose(
        E, pts1, pts2, K,
        mask=mask_E  # 可选：只使用内点
    )
    
    # 4. 统计结果
    print(f"内点数量: {np.sum(mask_pose)}/{len(matches)}")
    print(f"旋转矩阵 R:\n{R}")
    print(f"平移向量 t (单位向量):\n{t}")
    
    return R, t, mask_pose

# 更完整的示例
def complete_pose_recovery_example():
    """完整的位姿恢复示例"""
    # 模拟数据
    np.random.seed(42)
    
    # 相机内参
    K = np.array([[800, 0, 320],
                  [0, 800, 240],
                  [0, 0, 1]], dtype=np.float32)
    
    # 生成模拟特征点
    n_points = 100
    
    # 3D 点（在相机1的坐标系中）
    X = np.random.randn(n_points, 3) * 5
    X[:, 2] += 10  # 确保深度为正
    
    # 相机1的位姿（世界坐标系）
    R1 = np.eye(3)
    t1 = np.zeros((3, 1))
    P1 = K @ np.hstack([R1, t1])  # 投影矩阵
    
    # 相机2的位姿（相对于相机1）
    true_R = cv2.Rodrigues(np.array([0.1, 0.2, 0.3]))[0]  # 小旋转
    true_t = np.array([[0.5], [0.1], [0.2]])  # 平移
    
    P2 = K @ np.hstack([true_R, true_t])
    
    # 投影到图像平面
    X_homo = np.hstack([X, 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
    
    print("="*60)
    print("模拟数据恢复相对位姿")
    print("="*60)
    print(f"真实旋转矩阵 R:\n{true_R}")
    print(f"真实平移向量 t:\n{true_t}")
    print(f"平移向量长度: {np.linalg.norm(true_t):.4f}")
    
    # 计算本质矩阵
    E, mask_E = cv2.findEssentialMat(pts1, pts2, K, 
                                     method=cv2.RANSAC,
                                     prob=0.999,
                                     threshold=0.5)
    print(f"\n计算出的本质矩阵 E:\n{E}")
    
    # 恢复位姿
    num, R, t, mask = cv2.recoverPose(E, pts1, pts2, K, mask=mask_E)
    
    print(f"\n恢复的位姿:")
    print(f"内点数量: {np.sum(mask)}/{n_points}")
    print(f"旋转矩阵 R:\n{R}")
    print(f"旋转误差: {np.linalg.norm(R - true_R):.6f}")
    
    # 注意：恢复的 t 是单位向量
    print(f"\n平移向量 t (单位向量):\n{t}")
    
    # 需要估计尺度
    true_t_norm = true_t / np.linalg.norm(true_t)
    print(f"真实平移向量 (单位化):\n{true_t_norm}")
    print(f"平移方向误差: {np.linalg.norm(t - true_t_norm):.6f}")
    
    return R, t, mask

# 处理尺度问题
def estimate_scale_with_depth(R, t, pts1, pts2, K, method='median'):
    """
    估计平移向量的尺度
    
    参数:
        R, t: 恢复的相对位姿 (t 是单位向量)
        pts1, pts2: 匹配点
        K: 相机内参
        method: 尺度估计方法 ('median', 'mean', 'ransac')
    """
    n = len(pts1)
    
    # 创建投影矩阵
    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, :]  # 在相机1坐标系中的深度
    
    # 转换到相机2坐标系
    points_3d_cam2 = R @ points_3d + t
    depths2 = points_3d_cam2[2, :]  # 在相机2坐标系中的深度
    
    # 检查正深度
    valid_mask = (depths1 > 0) & (depths2 > 0)
    
    if np.sum(valid_mask) < 5:
        print("警告: 正深度点太少")
        return 1.0
    
    valid_depths1 = depths1[valid_mask]
    
    if method == 'median':
        scale = np.median(valid_depths1)
    elif method == 'mean':
        scale = np.mean(valid_depths1)
    elif method == 'ransac':
        # 使用RANSAC鲁棒估计
        from sklearn.linear_model import RANSACRegressor
        X = np.ones((len(valid_depths1), 1))
        y = valid_depths1.reshape(-1, 1)
        ransac = RANSACRegressor()
        ransac.fit(X, y)
        scale = ransac.estimator_.coef_[0]
    else:
        scale = 1.0
    
    return scale

# 实际应用示例
def visual_odometry_example(image1, image2, K):
    """
    视觉里程计示例：估计两帧之间的运动
    """
    import cv2
    
    # 1. 特征检测与匹配
    # 使用ORB特征
    orb = cv2.ORB_create(nfeatures=1000)
    
    # 检测特征点
    kp1, des1 = orb.detectAndCompute(image1, None)
    kp2, des2 = orb.detectAndCompute(image2, None)
    
    # 匹配特征点
    bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
    matches = bf.match(des1, des2)
    
    # 按距离排序
    matches = sorted(matches, key=lambda x: x.distance)
    
    # 提取匹配点坐标
    pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
    pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])
    
    # 2. 计算本质矩阵
    E, mask_E = cv2.findEssentialMat(pts1, pts2, K, 
                                     method=cv2.RANSAC,
                                     prob=0.999,
                                     threshold=1.0)
    
    # 3. 恢复位姿
    num, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, K, mask=mask_E)
    
    # 4. 估计尺度（如果有真实尺度信息）
    # 在实际SLAM中，尺度通常从其他来源获取
    # 例如：IMU、已知物体尺寸、深度信息等
    
    return R, t, mask_pose, matches

# 验证 recoverPose 的四种解
def analyze_four_solutions(E, pts1, pts2, K):
    """
    分析从本质矩阵分解的四种可能解
    """
    # 对 E 进行 SVD 分解
    U, S, Vt = np.linalg.svd(E)
    
    # 确保 det(U) > 0, det(V) > 0
    if np.linalg.det(U) < 0:
        U = -U
    if np.linalg.det(Vt) < 0:
        Vt = -Vt
    
    W = np.array([[0, -1, 0],
                  [1, 0, 0],
                  [0, 0, 1]])
    
    # 两种可能的旋转
    R1 = U @ W @ Vt
    R2 = U @ W.T @ Vt
    
    # 可能的平移（t 是 E 的零空间向量）
    t = U[:, 2]
    t = t.reshape(3, 1)
    
    # 四种组合
    solutions = [
        (R1, t),
        (R1, -t),
        (R2, t),
        (R2, -t)
    ]
    
    # 测试每种解
    best_solution = None
    best_score = -1
    
    for i, (R, t) in enumerate(solutions):
        # 创建投影矩阵
        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, :]
        
        score = np.sum((depths1 > 0) & (depths2 > 0))
        
        print(f"解{i+1}: 正深度点数 = {score}/{len(pts1)}")
        
        if score > best_score:
            best_score = score
            best_solution = (R, t, i+1)
    
    print(f"\n最佳解: 解{best_solution[2]}")
    return best_solution

# 主测试
if __name__ == "__main__":
    print("cv2.recoverPose() 函数详解")
    print("="*60)
    
    # 示例1: 模拟数据
    print("\n示例1: 模拟数据恢复位姿")
    R, t, mask = complete_pose_recovery_example()
    
    # 示例2: 分析四种解
    print("\n" + "="*60)
    print("示例2: 分析四种可能的解")
    print("="*60)
    
    # 创建测试数据
    np.random.seed(42)
    K = np.array([[800, 0, 320],
                  [0, 800, 240],
                  [0, 0, 1]])
    
    n_points = 50
    X = np.random.randn(n_points, 3) * 3
    X[:, 2] += 5
    
    # 真实位姿
    true_R = cv2.Rodrigues(np.array([0.2, 0.1, 0.3]))[0]
    true_t = np.array([[0.3], [0.1], [0.2]])
    
    # 投影
    P1 = K @ np.hstack([np.eye(3), np.zeros((3, 1))])
    P2 = K @ np.hstack([true_R, true_t])
    
    X_homo = np.hstack([X, 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]
    
    # 计算本质矩阵
    E, _ = cv2.findEssentialMat(pts1, pts2, K)
    
    # 分析四种解
    best_R, best_t, sol_num = analyze_four_solutions(E, pts1, pts2, K)
    
    print(f"\n真实解应该对应解{sol_num}")
    print(f"旋转误差: {np.linalg.norm(best_R - true_R):.6f}")
    
    # 使用 recoverPose 验证
    print("\n" + "="*60)
    print("使用 cv2.recoverPose() 验证")
    print("="*60)
    
    num, R_cv, t_cv, mask_cv = cv2.recoverPose(E, pts1, pts2, K)
    
    print(f"cv2.recoverPose 选择了解: {np.sum(mask_cv)}/{n_points} 个内点")
    print(f"旋转矩阵:\n{R_cv}")
    
    # 检查是否选择了正确解
    if np.linalg.norm(R_cv - best_R) < 1e-6:
        print("✓ cv2.recoverPose 选择了正确解")
    else:
        print("✗ cv2.recoverPose 选择了不同解")