import numpy as np
import cv2
from backend.__detector import detect_markers_solo
from backend._gs_ import aruco_dict, stereo_camera#  通过基础矩阵 验证左右两幅图像中的点是否是匹配点
def check_match_by_fundentalmat(uvs1, uvs2, F):for i in range(len(uvs1)):uv1 = uvs1[i]uv2 = uvs2[i]ret = np.dot( F, np.array([uv1[0],uv1[1],1]).T)ret = np.dot( [uv2[0],uv2[1],1] , ret)print(ret)def check_match_by_polar(uvs1, uvs2, F):# 获取极点U, S, V = np.linalg.svd(F)e = V[-1]epi_polar = e / e[2]# 极线方程for i, uv1 in enumerate( uvs1):line = np.dot(F, [uv1[0], uv1[1], 1])ret = np.dot(line, [uvs2[i][0], uvs2[i][1], 1])print('ret: ', ret)def main():imgf1 = r'D:\mydocs\ftp\stereo_test\stereo_cali\small_test1\75_1.png'imgf2 = r'D:\mydocs\ftp\stereo_test\stereo_cali\small_test1\89_1.png'img1 = cv2.imread(imgf1)img2 = cv2.imread(imgf2)ret1 = detect_markers_solo(img1, aruco_dict, do_filter=True, pattern_num= 1)ret2 = detect_markers_solo(img2, aruco_dict, do_filter=True, pattern_num=1)if ret1 and ret2:frame1, marker_corners1, marker_ids1 = ret1frame2, marker_corners2, marker_ids2 = ret2# corners1 = np.array(marker_corners1).reshape(-1,2)# corners2 = np.array(marker_corners2).reshape(-1, 2)## corners2_new = [ corners2[(ii+1)%len(corners2)]  for ii, item in enumerate(corners2)]# check_match_by_fundentalmat(corners1, corners2_new, stereo_camera.F)# shared_ids = [idx[0] for idx in marker_ids1 if idx in marker_ids2]# for idx in shared_ids:#   corners1 = [ marker_corners1[ii][0] for ii, id in enumerate(marker_ids1) if id[0]==idx ]#   corners2 = [marker_corners2[ii][0] for ii, id in enumerate(marker_ids2) if id[0] != idx]#   # check_match_by_fundentalmat(corners1[0], corners2[0], stereo_camera.F )#   check_match_by_polar(corners1[0], corners2[0], stereo_camera.F)## cv2.imshow('left', cv2.resize(frame1, (1200,800)))# cv2.imshow('right', cv2.resize(frame2,(1200,900)))# cv2.waitKey()# check_match_by_fundentalmat([ [100,200], [300,400] ], [ [803, 904], [1010,209]], stereo_camera.F)check_match_by_polar([[100, 200], [300, 400]], [[803, 904], [1010, 209]], stereo_camera.F)main()