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) * 5X[:, 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).Tpts2_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.5pts2 += np.random.randn(*pts2.shape) * 0.5print("="*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 + tdepths2 = points_3d_cam2[2, :] # 在相机2坐标系中的深度# 检查正深度valid_mask = (depths1 > 0) & (depths2 > 0)if np.sum(valid_mask) < 5:print("警告: 正深度点太少")return 1.0valid_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 RANSACRegressorX = 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.0return 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) > 0if np.linalg.det(U) < 0:U = -Uif np.linalg.det(Vt) < 0:Vt = -VtW = np.array([[0, -1, 0],[1, 0, 0],[0, 0, 1]])# 两种可能的旋转R1 = U @ W @ VtR2 = 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 = Nonebest_score = -1for 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 + tdepths2 = points_3d_cam2[2, :]score = np.sum((depths1 > 0) & (depths2 > 0))print(f"解{i+1}: 正深度点数 = {score}/{len(pts1)}")if score > best_score:best_score = scorebest_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 = 50X = np.random.randn(n_points, 3) * 3X[:, 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).Tpts2_homo = (P2 @ X_homo.T).Tpts1 = 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 选择了不同解")
