import cv2
import numpy as np

# 读取左右视角图像（灰度模式）
img1 = cv2.imread('images/left.jpg', 0)
img2 = cv2.imread('images/right.jpg', 0)

# 初始化SIFT特征检测器
sift = cv2.SIFT_create()  # 创建SIFT对象

# 检测关键点并计算描述符
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)

# FLANN匹配器参数配置
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)  # KD树算法参数
search_params = dict(checks=50)  # 搜索参数：检查次数

# 创建FLANN匹配器实例
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)  # k近邻匹配

pts1 = []
pts2 = []
good_matches = []  # 初始化优质匹配列表

# 使用Lowe's比率测试筛选优质匹配
for i, (m, n) in enumerate(matches):
    if m.distance < 0.7 * n.distance:  # 修改此处：调整距离比阈值
        good_matches.append(m)
        pts2.append(kp2[m.trainIdx].pt)  # 记录右图匹配点坐标
        pts1.append(kp1[m.queryIdx].pt)  # 记录左图匹配点坐标

if len(good_matches) > 8:
    pts1 = np.int32(pts1)
    pts2 = np.int32(pts2)

    # 计算基础矩阵（使用RANSAC算法）
    F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)
    print("基础矩阵:\n", F)

    # 通过基础矩阵恢复旋转和平移变换
    _, R, t, _ = cv2.recoverPose(F, pts1, pts2)
    print("旋转矩阵:\n", R)
    print("平移向量:\n", t)

    # 可视化优质匹配结果
    img_match = cv2.drawMatches(img1, kp1, img2, kp2, good_matches, None, 
                               flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
    cv2.imshow('特征匹配', img_match)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
else:
    print("匹配点不足，无法计算基础矩阵。")