text_plain_output_1.png image_output_2.png image_output_1.png
def estimateF(img_1, img_2):
    img1 = cv2.cvtColor(img_1, cv2.COLOR_BGR2GRAY)
    img2 = cv2.cvtColor(img_2, cv2.COLOR_BGR2GRAY)
    sift = cv2.SIFT_create()
    kp1, des1 = sift.detectAndCompute(img1, None)
    kp2, des2 = sift.detectAndCompute(img2, None)
    bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)
    cv_matches = bf.match(des1, des2)
    cur_kp_1 = ArrayFromCvKps(kp1)
    cur_kp_2 = ArrayFromCvKps(kp2)
    matches = np.array([[m.queryIdx, m.trainIdx] for m in cv_matches])
    im_matches = DrawMatches(img_1, img_2, cur_kp_1, cur_kp_2, matches)
    fig = plt.figure(figsize=(25, 25))
    plt.title('Matches before RANSAC')
    plt.imshow(im_matches)
    plt.axis('off')
    plt.show()
    F, inlier_mask = cv2.findFundamentalMat(cur_kp_1[matches[:, 0]], cur_kp_2[matches[:, 1]], cv2.USAC_MAGSAC, ransacReprojThreshold=0.25, confidence=0.99999, maxIters=10000)
    matches_after_ransac = np.array([match for match, is_inlier in zip(matches, inlier_mask) if is_inlier])
    im_inliers = DrawMatches(img_1, img_2, cur_kp_1, cur_kp_2, matches_after_ransac)
    fig = plt.figure(figsize=(25, 25))
    plt.title('Matches after RANSAC')
    plt.imshow(im_inliers)
    plt.axis('off')
    plt.show()
    scaling_dict = pd.read_csv(train_csv)
    inlier_kp_1 = ArrayFromCvKps([kp for i, kp in enumerate(kp1) if i in matches_after_ransac[:, 0]])
    inlier_kp_2 = ArrayFromCvKps([kp for i, kp in enumerate(kp2) if i in matches_after_ransac[:, 1]])
    E, R, T = ComputeEssentialMatrix(F, df['K'][0], df['K'][1], inlier_kp_1, inlier_kp_2)
    q = QuaternionFromMatrix(R)
    T = T.flatten()
    R1_gt, T1_gt = (df['R'][0], df['T'][0].reshape((3, 1)))
    R2_gt, T2_gt = (df['R'][1], df['T'][1].reshape((3, 1)))
    dR_gt = np.dot(R2_gt, R1_gt.T)
    dT_gt = (T2_gt - np.dot(dR_gt, T1_gt)).flatten()
    q_gt = QuaternionFromMatrix(dR_gt)
    q_gt = q_gt / (np.linalg.norm(q_gt) + eps)
    err_q, err_t = ComputeErrorForOneExample(q_gt, dT_gt, q, T, scaling_dict['scaling_factor'][0])
    print(f'rotation_error={err_q:.02f} (deg), translation_error={err_t:.02f} (m)', flush=True)
estimateF(df['imgs'][0], df['imgs'][1])